diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-27 13:35:23 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-27 13:35:23 +0200 |
commit | 3195eb100516b7a4aabadd3e2640434678dbc7ad (patch) | |
tree | fb36d2b41061a976364777d62a32403dd9680292 /src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | |
parent | 615277d346e60afd74a9223d5b5f8a20b7b47ece (diff) | |
download | px4-firmware-3195eb100516b7a4aabadd3e2640434678dbc7ad.tar.gz px4-firmware-3195eb100516b7a4aabadd3e2640434678dbc7ad.tar.bz2 px4-firmware-3195eb100516b7a4aabadd3e2640434678dbc7ad.zip |
estimator: Remove bogus timeout flag, do not reset states not in need of a reset. Do not alter baro offset or GPS positions.
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 21 |
1 files changed, 3 insertions, 18 deletions
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 5b0e7acf2..4f6f65393 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; ///<local performance counter for airspeed updates perf_counter_t _perf_reset; ///<local performance counter for filter resets - bool _initialized; bool _baro_init; bool _gps_initialized; hrt_abstime _gps_start_time; @@ -380,7 +379,6 @@ FixedwingEstimator::FixedwingEstimator() : _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), /* states */ - _initialized(false), _baro_init(false), _gps_initialized(false), _gyro_valid(false), @@ -628,19 +626,6 @@ FixedwingEstimator::check_filter_state() _ekf->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 |