ZephyrXtreme::ZephyrXtreme(int id,QObject *parent)
: LPSwitcher(id,LPSwitcher::TypeZephyrXtreme,parent)
{
zep_locked=false;
zep_xmt_algo=LPSwitcher::XmtAlgoLast;
zep_rcv_algo=LPSwitcher::RcvAlgoLast;
zep_channel_rate=LPSwitcher::ChannelRateLast;
zep_pending_channel_rate=LPSwitcher::ChannelRateLast;
zep_sample_rate=0;
zep_pending_sample_rate=0;
for(int i=0;i<2;i++) {
zep_line_state[i]=LPSwitcher::StateOffline;
zep_dialed_string[i]="";
zep_pending_string[i]="";
}
zep_device=new LPTTYDevice(this);
zep_device->setSpeed(ZEPHYRXTREME_SPEED);
zep_device->setWordLength(ZEPHYRXTREME_WORD_LENGTH);
zep_device->setParity(ZEPHYRXTREME_PARITY);
zep_device->setFlowControl(ZEPHYRXTREME_FLOW_CONTROL);
connect(zep_device,SIGNAL(readyRead()),this,SLOT(readyReadData()));
zep_socket=new QTcpSocket(this);
connect(zep_socket,SIGNAL(connected()),this,SLOT(connectedData()));
connect(zep_socket,SIGNAL(disconnected()),this,SLOT(disconnectedData()));
connect(zep_socket,SIGNAL(readyRead()),this,SLOT(socketReadyRead()));
zep_poll_timer=new QTimer(this);
connect(zep_poll_timer,SIGNAL(timeout()),this,SLOT(pollData()));
}
void PWMController::run() {
//std::cout << "PWMController started" << std::endl;
while(1) {
// pollData();
// calculateOutputs();
// writeOutputs();
// std::this_thread::sleep_for(std::chrono::milliseconds(PWMLOOPTIME));
//}
while (!isKilled()) {
pollData();
calculateOutputs();
writeOutputs(false);
std::this_thread::sleep_for(std::chrono::milliseconds(PWMLOOPTIME));
}
stop();
std::cout << "KILLED" << std::endl;
while (isKilled()) {
pollData();
writeOutputs(true);
std::this_thread::sleep_for(std::chrono::milliseconds(PWMLOOPTIME));
}
_imuController->reset();
_xRotationController.reset();
_yRotationController.reset();
_zRotationController.reset();
_depthController.reset();
_xRotationController.start();
_yRotationController.start();
_zRotationController.start();
_depthController.start();
_pwm->enable();
std::cout << "ACTIVE" << std::endl;
}
}
Gpio::Gpio(int id,QObject *parent)
: LPSwitcher(id,LPSwitcher::TypeGpio,parent)
{
#ifdef HAVE_MC_GPIO
gpio_fd=-1;
gpio_gpis=0;
gpio_gpos=0;
gpio_gpo_mapper=new QSignalMapper(this);
connect(gpio_gpo_mapper,SIGNAL(mapped(int)),this,SLOT(gpoData(int)));
gpio_poll_timer=new QTimer(this);
connect(gpio_poll_timer,SIGNAL(timeout()),this,SLOT(pollData()));
#endif // HAVE_MC_GPIO
}
BtSs82::BtSs82(int id,QObject *parent)
: LPSwitcher(id,LPSwitcher::TypeBtSs82,parent)
{
for(unsigned i=0;i<BTSS82_OUTPUTS;i++) {
bt_crosspoints[i]=0;
}
for(unsigned i=0;i<BTSS82_GPIS;i++) {
bt_gpi_states[i]=false;
}
bt_silence_sense_states[0]=false;
bt_silence_sense_states[1]=false;
bt_device=new LPTTYDevice(this);
bt_device->setSpeed(BTSS82_SPEED);
bt_device->setWordLength(BTSS82_WORD_LENGTH);
bt_device->setParity(BTSS82_PARITY);
bt_device->setFlowControl(BTSS82_FLOW_CONTROL);
connect(bt_device,SIGNAL(readyRead()),this,SLOT(readyReadData()));
bt_poll_counter=0;
bt_poll_timer=new QTimer(this);
connect(bt_poll_timer,SIGNAL(timeout()),this,SLOT(pollData()));
}
void AttitudePositionEstimatorEKF::task_main()
{
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
_ekf = new AttPosEKF();
_filter_start_time = hrt_absolute_time();
if (!_ekf) {
warnx("OUT OF MEM!");
return;
}
/*
* do subscriptions
*/
_distance_sub = orb_subscribe(ORB_ID(distance_sensor));
_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_home_sub = orb_subscribe(ORB_ID(home_position));
_landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected));
_armedSub = orb_subscribe(ORB_ID(actuator_armed));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200);
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
/* XXX remove this!, BUT increase the data buffer size! */
orb_set_interval(_sensor_combined_sub, 9);
/* sets also parameters in the EKF object */
parameters_update();
/* wakeup source(s) */
px4_pollfd_struct_t fds[2];
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _sensor_combined_sub;
fds[1].events = POLLIN;
_gps.vel_n_m_s = 0.0f;
_gps.vel_e_m_s = 0.0f;
_gps.vel_d_m_s = 0.0f;
_task_running = true;
while (!_task_should_exit) {
/* wait for up to 100ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("POLL ERR %d, %d", pret, errno);
continue;
}
perf_begin(_loop_perf);
perf_count(_loop_intvl);
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
/* update parameters from storage */
parameters_update();
}
/* only run estimator if gyro updated */
if (fds[1].revents & POLLIN) {
/* check vehicle status for changes to publication state */
bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON);
vehicle_status_poll();
perf_count(_perf_gyro);
/* Reset baro reference if switching to HIL, reset sensor states */
if (!prev_hil && (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON)) {
/* system is in HIL now, wait for measurements to come in one last round */
usleep(60000);
/* now read all sensor publications to ensure all real sensor data is purged */
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
/* set sensors to de-initialized state */
_gyro_valid = false;
_accel_valid = false;
_mag_valid = false;
_baro_init = false;
_gps_initialized = false;
_last_sensor_timestamp = hrt_absolute_time();
_last_run = _last_sensor_timestamp;
_ekf->ZeroVariables();
_ekf->dtIMU = 0.01f;
_filter_start_time = _last_sensor_timestamp;
/* now skip this loop and get data on the next one, which will also re-init the filter */
continue;
}
/**
* PART ONE: COLLECT ALL DATA
**/
pollData();
/*
* CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
*/
if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) {
continue;
}
/**
* PART TWO: EXECUTE THE FILTER
*
* We run the filter only once all data has been fetched
**/
if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) {
// maintain filtered baro and gps altitudes to calculate weather offset
// baro sample rate is ~70Hz and measurement bandwidth is high
// gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds
// maintain heavily filtered values for both baro and gps altitude
// Assume the filtered output should be identical for both sensors
_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
// if (hrt_elapsed_time(&_last_debug_print) >= 5e6) {
// _last_debug_print = hrt_absolute_time();
// perf_print_counter(_perf_baro);
// perf_reset(_perf_baro);
// warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f",
// (double)_baro_gps_offset,
// (double)_baro_alt_filt,
// (double)_gps_alt_filt,
// (double)_global_pos.alt,
// (double)_local_pos.z);
// }
/* Initialize the filter first */
if (!_ekf->statesInitialised) {
// North, East Down position (m)
float initVelNED[3] = {0.0f, 0.0f, 0.0f};
_ekf->posNE[0] = 0.0f;
_ekf->posNE[1] = 0.0f;
_local_pos.ref_alt = 0.0f;
_baro_ref_offset = 0.0f;
_baro_gps_offset = 0.0f;
_baro_alt_filt = _baro.altitude;
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
} else {
if (!_gps_initialized && _gpsIsGood) {
initializeGPS();
continue;
}
// Check if on ground - status is used by covariance prediction
_ekf->setOnGround(_landDetector.landed);
// We're apparently initialized in this case now
// check (and reset the filter as needed)
int check = check_filter_state();
if (check) {
// Let the system re-initialize itself
continue;
}
//Run EKF data fusion steps
updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData);
//Publish attitude estimations
publishAttitude();
//Publish Local Position estimations
publishLocalPosition();
//Publish Global Position, but only if it's any good
if (_gps_initialized && (_gpsIsGood || _global_pos.dead_reckoning)) {
publishGlobalPosition();
}
//Publish wind estimates
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
publishWindEstimate();
}
}
}
}
perf_end(_loop_perf);
}
_task_running = false;
_estimator_task = -1;
return;
}
