diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 5b0e7acf2f..4f6f653932 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -208,7 +208,6 @@ private: perf_counter_t _perf_airspeed; ///GetLastErrorState(&ekf_report); - // set sensors to de-initialized state - _gyro_valid = false; - _accel_valid = false; - _mag_valid = false; - - _baro_init = false; - _gps_initialized = false; - _initialized = false; - _last_sensor_timestamp = hrt_absolute_time(); - _last_run = _last_sensor_timestamp; - - _ekf->dtIMU = 0.01f; - } else if (_ekf_logging) { _ekf->GetFilterState(&ekf_report); } @@ -665,6 +650,7 @@ FixedwingEstimator::check_filter_state() rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout) << 2); + rep.timeout_flags |= (((uint8_t)(check == 2)) << 3); // IMU timeout if (_debug > 10) { @@ -680,7 +666,8 @@ FixedwingEstimator::check_filter_state() warnx("timeout: %s%s%s", ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), - ((rep.timeout_flags & (1 << 2)) ? "HGT " : "")); + ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""), + ((rep.timeout_flags & (1 << 3)) ? "IMU " : "")); } } @@ -1263,8 +1250,6 @@ FixedwingEstimator::task_main() dt = 0.0f; } - _initialized = true; - // Fuse GPS Measurements if (newDataGps && _gps_initialized) { // Convert GPS measurements to Pos NE, hgt and Vel NED