From 5ac792eed68aa349e5f34b6e122b8501963d30d9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 15 Oct 2022 20:57:42 -0400 Subject: [PATCH] [RFC] ekf2: rethink info/warning/errors --- .ci/Jenkinsfile-hardware | 1 - msg/CMakeLists.txt | 1 - msg/estimator_aid_source_1d.msg | 2 + msg/estimator_aid_source_2d.msg | 2 + msg/estimator_aid_source_3d.msg | 2 + msg/estimator_event_flags.msg | 37 --------- src/modules/ekf2/CMakeLists.txt | 1 + src/modules/ekf2/EKF/baro_height_control.cpp | 16 ++-- src/modules/ekf2/EKF/common.h | 43 ---------- src/modules/ekf2/EKF/control.cpp | 11 ++- src/modules/ekf2/EKF/covariance.cpp | 3 +- src/modules/ekf2/EKF/ekf.h | 4 + src/modules/ekf2/EKF/ekf_helper.cpp | 72 ++++++++--------- src/modules/ekf2/EKF/estimator_interface.h | 19 +---- src/modules/ekf2/EKF/ev_height_control.cpp | 20 ++--- src/modules/ekf2/EKF/fake_height_control.cpp | 11 ++- src/modules/ekf2/EKF/fake_pos_control.cpp | 9 +-- src/modules/ekf2/EKF/gnss_height_control.cpp | 16 ++-- src/modules/ekf2/EKF/gps_checks.cpp | 3 +- src/modules/ekf2/EKF/gps_control.cpp | 17 ++-- src/modules/ekf2/EKF/height_control.cpp | 2 +- src/modules/ekf2/EKF/range_height_control.cpp | 18 ++--- src/modules/ekf2/EKF2.cpp | 79 +------------------ src/modules/ekf2/EKF2.hpp | 6 -- src/modules/logger/logged_topics.cpp | 3 - 25 files changed, 104 insertions(+), 294 deletions(-) delete mode 100644 msg/estimator_event_flags.msg diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index f3e3f3ca46..2e9e438541 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -870,7 +870,6 @@ void printTopics() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_gnss_vel" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_baro_bias" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_event_flags" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_global_position" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_gps_status" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovation_test_ratios" || true' diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index d459488399..d1bf01e158 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -70,7 +70,6 @@ set(msg_files estimator_aid_source_2d.msg estimator_aid_source_3d.msg estimator_bias.msg - estimator_event_flags.msg estimator_gps_status.msg estimator_innovations.msg estimator_selector_status.msg diff --git a/msg/estimator_aid_source_1d.msg b/msg/estimator_aid_source_1d.msg index be544f2102..067311599d 100644 --- a/msg/estimator_aid_source_1d.msg +++ b/msg/estimator_aid_source_1d.msg @@ -18,6 +18,8 @@ bool fusion_enabled # true when measurements are being fused bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused +uint8 state_reset + # TOPICS estimator_aid_source_1d # TOPICS estimator_aid_src_baro_hgt estimator_aid_src_ev_hgt estimator_aid_src_gnss_hgt estimator_aid_src_rng_hgt # TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip diff --git a/msg/estimator_aid_source_2d.msg b/msg/estimator_aid_source_2d.msg index 53157c46cc..66143d26f2 100644 --- a/msg/estimator_aid_source_2d.msg +++ b/msg/estimator_aid_source_2d.msg @@ -18,6 +18,8 @@ bool fusion_enabled # true when measurements are being fused bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused +uint8 state_reset + # TOPICS estimator_aid_source_2d # TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos # TOPICS estimator_aid_src_aux_vel diff --git a/msg/estimator_aid_source_3d.msg b/msg/estimator_aid_source_3d.msg index e5c082eeec..8075d65b26 100644 --- a/msg/estimator_aid_source_3d.msg +++ b/msg/estimator_aid_source_3d.msg @@ -18,6 +18,8 @@ bool fusion_enabled # true when measurements are being fused bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused +uint8 state_reset + # TOPICS estimator_aid_source_3d # TOPICS estimator_aid_src_ev_vel # TOPICS estimator_aid_src_gnss_vel diff --git a/msg/estimator_event_flags.msg b/msg/estimator_event_flags.msg deleted file mode 100644 index 3baea13361..0000000000 --- a/msg/estimator_event_flags.msg +++ /dev/null @@ -1,37 +0,0 @@ -uint64 timestamp # time since system start (microseconds) -uint64 timestamp_sample # the timestamp of the raw data (microseconds) - -# information events -uint32 information_event_changes # number of information event changes -bool gps_checks_passed # 0 - true when gps quality checks are passing passed -bool reset_vel_to_gps # 1 - true when the velocity states are reset to the gps measurement -bool reset_vel_to_flow # 2 - true when the velocity states are reset using the optical flow measurement -bool reset_vel_to_vision # 3 - true when the velocity states are reset to the vision system measurement -bool reset_vel_to_zero # 4 - true when the velocity states are reset to zero -bool reset_pos_to_last_known # 5 - true when the position states are reset to the last known position -bool reset_pos_to_gps # 6 - true when the position states are reset to the gps measurement -bool reset_pos_to_vision # 7 - true when the position states are reset to the vision system measurement -bool starting_gps_fusion # 8 - true when the filter starts using gps measurements to correct the state estimates -bool starting_vision_pos_fusion # 9 - true when the filter starts using vision system position measurements to correct the state estimates -bool starting_vision_vel_fusion # 10 - true when the filter starts using vision system velocity measurements to correct the state estimates -bool starting_vision_yaw_fusion # 11 - true when the filter starts using vision system yaw measurements to correct the state estimates -bool yaw_aligned_to_imu_gps # 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data -bool reset_hgt_to_baro # 13 - true when the vertical position state is reset to the baro measurement -bool reset_hgt_to_gps # 14 - true when the vertical position state is reset to the gps measurement -bool reset_hgt_to_rng # 15 - true when the vertical position state is reset to the rng measurement -bool reset_hgt_to_ev # 16 - true when the vertical position state is reset to the ev measurement - -# warning events -uint32 warning_event_changes # number of warning event changes -bool gps_quality_poor # 0 - true when the gps is failing quality checks -bool gps_fusion_timout # 1 - true when the gps data has not been used to correct the state estimates for a significant time period -bool gps_data_stopped # 2 - true when the gps data has stopped for a significant time period -bool gps_data_stopped_using_alternate # 3 - true when the gps data has stopped for a significant time period but the filter is able to use other sources of data to maintain navigation -bool height_sensor_timeout # 4 - true when the height sensor has not been used to correct the state estimates for a significant time period -bool stopping_navigation # 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation -bool invalid_accel_bias_cov_reset # 6 - true when the filter has detected bad acceerometer bias state esitmstes and has reset the corresponding covariance matrix elements -bool bad_yaw_using_gps_course # 7 - true when the filter has detected an invalid yaw estimate and has reset the yaw angle to the GPS ground course -bool stopping_mag_use # 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetomer data -bool vision_data_stopped # 9 - true when the vision system data has stopped for a significant time period -bool emergency_yaw_reset_mag_stopped # 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetomer data -bool emergency_yaw_reset_gps_yaw_stopped # 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index f9324694bc..b02f500880 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -129,6 +129,7 @@ px4_add_module( ) if(BUILD_TESTING) + add_definitions(-DDEBUG_BUILD) add_subdirectory(EKF) add_subdirectory(test) endif() diff --git a/src/modules/ekf2/EKF/baro_height_control.cpp b/src/modules/ekf2/EKF/baro_height_control.cpp index e34cb19744..864c3ddfae 100644 --- a/src/modules/ekf2/EKF/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/baro_height_control.cpp @@ -125,41 +125,41 @@ void Ekf::controlBaroHeightFusion() if (isHeightResetRequired()) { // All height sources are failing - ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); + ECL_DEBUG("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); - _information_events.flags.reset_hgt_to_baro = true; resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var); bias_est.setBias(_state.pos(2) + _baro_lpf.getState()); // reset vertical velocity resetVerticalVelocityToZero(); + aid_src.state_reset++; aid_src.time_last_fuse = _imu_sample_delayed.time_us; } else if (is_fusion_failing) { // Some other height source is still working - ECL_WARN("stopping %s height fusion, fusion failing", HGT_SRC_NAME); + ECL_DEBUG("stopping %s height fusion, fusion failing", HGT_SRC_NAME); stopBaroHgtFusion(); _baro_hgt_faulty = true; } } else { - ECL_WARN("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME); + ECL_DEBUG("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME); stopBaroHgtFusion(); } } else { if (starting_conditions_passing) { if (_params.height_sensor_ref == HeightSensor::BARO) { - ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME); + ECL_DEBUG("starting %s height fusion, resetting height", HGT_SRC_NAME); _height_sensor_ref = HeightSensor::BARO; - _information_events.flags.reset_hgt_to_baro = true; + aid_src.state_reset++; resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var); bias_est.setBias(_state.pos(2) + _baro_lpf.getState()); } else { - ECL_INFO("starting %s height fusion", HGT_SRC_NAME); + ECL_DEBUG("starting %s height fusion", HGT_SRC_NAME); bias_est.setBias(_state.pos(2) + _baro_lpf.getState()); } @@ -172,7 +172,7 @@ void Ekf::controlBaroHeightFusion() } else if (_control_status.flags.baro_hgt && !isNewestSampleRecent(_time_last_baro_buffer_push, 2 * BARO_MAX_INTERVAL)) { // No data anymore. Stop until it comes back. - ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME); + ECL_DEBUG("stopping %s height fusion, no data", HGT_SRC_NAME); stopBaroHgtFusion(); } } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index dbc5b55680..da9a9389e4 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -565,48 +565,5 @@ union terrain_fusion_status_u { uint8_t value; }; -// define structure used to communicate information events -union information_event_status_u { - struct { - bool gps_checks_passed : 1; ///< 0 - true when gps quality checks are passing passed - bool reset_vel_to_gps : 1; ///< 1 - true when the velocity states are reset to the gps measurement - bool reset_vel_to_flow : 1; ///< 2 - true when the velocity states are reset using the optical flow measurement - bool reset_vel_to_vision : 1; ///< 3 - true when the velocity states are reset to the vision system measurement - bool reset_vel_to_zero : 1; ///< 4 - true when the velocity states are reset to zero - bool reset_pos_to_last_known : 1; ///< 5 - true when the position states are reset to the last known position - bool reset_pos_to_gps : 1; ///< 6 - true when the position states are reset to the gps measurement - bool reset_pos_to_vision : 1; ///< 7 - true when the position states are reset to the vision system measurement - bool starting_gps_fusion : 1; ///< 8 - true when the filter starts using gps measurements to correct the state estimates - bool starting_vision_pos_fusion : 1; ///< 9 - true when the filter starts using vision system position measurements to correct the state estimates - bool starting_vision_vel_fusion : 1; ///< 10 - true when the filter starts using vision system velocity measurements to correct the state estimates - bool starting_vision_yaw_fusion : 1; ///< 11 - true when the filter starts using vision system yaw measurements to correct the state estimates - bool yaw_aligned_to_imu_gps : 1; ///< 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data - bool reset_hgt_to_baro : 1; ///< 13 - true when the vertical position state is reset to the baro measurement - bool reset_hgt_to_gps : 1; ///< 14 - true when the vertical position state is reset to the gps measurement - bool reset_hgt_to_rng : 1; ///< 15 - true when the vertical position state is reset to the rng measurement - bool reset_hgt_to_ev : 1; ///< 16 - true when the vertical position state is reset to the ev measurement - } flags; - uint32_t value; -}; - -// define structure used to communicate information events -union warning_event_status_u { - struct { - bool gps_quality_poor : 1; ///< 0 - true when the gps is failing quality checks - bool gps_fusion_timout : 1; ///< 1 - true when the gps data has not been used to correct the state estimates for a significant time period - bool gps_data_stopped : 1; ///< 2 - true when the gps data has stopped for a significant time period - bool gps_data_stopped_using_alternate : 1; ///< 3 - true when the gps data has stopped for a significant time period but the filter is able to use other sources of data to maintain navigation - bool height_sensor_timeout : 1; ///< 4 - true when the height sensor has not been used to correct the state estimates for a significant time period - bool stopping_navigation : 1; ///< 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation - bool invalid_accel_bias_cov_reset : 1; ///< 6 - true when the filter has detected bad acceerometer bias state estimates and has reset the corresponding covariance matrix elements - bool bad_yaw_using_gps_course : 1; ///< 7 - true when the filter has detected an invalid yaw estimate and has reset the yaw angle to the GPS ground course - bool stopping_mag_use : 1; ///< 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetomer data - bool vision_data_stopped : 1; ///< 9 - true when the vision system data has stopped for a significant time period - bool emergency_yaw_reset_mag_stopped : 1; ///< 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetomer data - bool emergency_yaw_reset_gps_yaw_stopped: 1; ///< 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data - } flags; - uint32_t value; -}; - } #endif // !EKF_COMMON_H diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 264f7cbb84..8a7e866355 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -79,8 +79,8 @@ void Ekf::controlFusionModes() } if (height_source) { - ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)", - (unsigned long long)_imu_sample_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length); + ECL_DEBUG("EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)", + height_source, (int)_imu_buffer_length, (int)_obs_buffer_length); } } } @@ -424,8 +424,7 @@ void Ekf::controlExternalVisionFusion() // Turn off EV fusion mode if no data has been received stopEvFusion(); - _warning_events.flags.vision_data_stopped = true; - ECL_WARN("vision data stopped"); + ECL_DEBUG("vision data stopped"); } } @@ -528,7 +527,7 @@ void Ekf::controlOpticalFlowFusion() // If the heading is valid and use is not inhibited , start using optical flow aiding if (_control_status.flags.yaw_align || _params.mag_fusion_type == MagFuseType::NONE) { // set the flag and reset the fusion timeout - ECL_INFO("starting optical flow fusion"); + ECL_DEBUG("starting optical flow fusion"); _control_status.flags.opt_flow = true; _time_last_of_fuse = _imu_sample_delayed.time_us; @@ -728,7 +727,7 @@ void Ekf::controlAirDataFusion() } } else if (_control_status.flags.fuse_aspd && !isRecent(_airspeed_sample_delayed.time_us, (uint64_t)1e6)) { - ECL_WARN("Airspeed data stopped"); + ECL_DEBUG("Airspeed data stopped"); stopAirspeedFusion(); } } diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 9e310a0d57..0a388b06ee 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -437,8 +437,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) _time_acc_bias_check = _imu_sample_delayed.time_us; _fault_status.flags.bad_acc_bias = false; - _warning_events.flags.invalid_accel_bias_cov_reset = true; - ECL_WARN("invalid accel bias - covariance reset"); + ECL_ERR("invalid accel bias - covariance reset"); } else if (force_symmetry) { // ensure the covariance values are symmetrical diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index bce2c96413..e1bf33924b 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -1102,6 +1102,8 @@ private: status.fusion_enabled = false; status.innovation_rejected = true; status.fused = false; + + // preserve status.state_reset } } @@ -1126,6 +1128,8 @@ private: status.fusion_enabled = false; status.innovation_rejected = true; status.fused = false; + + // preserve status.state_reset } } diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index ae063ae9ef..af8e66ba5b 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -46,16 +46,16 @@ void Ekf::resetVelocityToGps(const gpsSample &gps_sample) { - _information_events.flags.reset_vel_to_gps = true; - ECL_INFO("reset velocity to GPS"); + _aid_src_gnss_vel.state_reset++; + ECL_DEBUG("reset velocity to GPS"); resetVelocityTo(gps_sample.vel); P.uncorrelateCovarianceSetVariance<3>(4, sq(gps_sample.sacc)); } void Ekf::resetHorizontalVelocityToOpticalFlow() { - _information_events.flags.reset_vel_to_flow = true; - ECL_INFO("reset velocity to flow"); + // _aid_src_optical_flow.state_reset++; // TODO: + ECL_DEBUG("reset velocity to flow"); // constrain height above ground to be above minimum possible const float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance); @@ -85,16 +85,15 @@ void Ekf::resetHorizontalVelocityToOpticalFlow() void Ekf::resetVelocityToVision() { - _information_events.flags.reset_vel_to_vision = true; - ECL_INFO("reset to vision velocity"); + _aid_src_ev_vel.state_reset++; + ECL_DEBUG("reset to vision velocity"); resetVelocityTo(getVisionVelocityInEkfFrame()); P.uncorrelateCovarianceSetVariance<3>(4, getVisionVelocityVarianceInEkfFrame()); } void Ekf::resetHorizontalVelocityToZero() { - _information_events.flags.reset_vel_to_zero = true; - ECL_INFO("reset velocity to zero"); + ECL_DEBUG("reset velocity to zero"); // Used when falling back to non-aiding mode of operation resetHorizontalVelocityTo(Vector2f{0.f, 0.f}); P.uncorrelateCovarianceSetVariance<2>(4, 25.0f); @@ -150,16 +149,16 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var) void Ekf::resetHorizontalPositionToGps(const gpsSample &gps_sample) { - _information_events.flags.reset_pos_to_gps = true; - ECL_INFO("reset position to GPS"); + _aid_src_gnss_pos.state_reset++; + ECL_DEBUG("reset position to GPS"); resetHorizontalPositionTo(gps_sample.pos); P.uncorrelateCovarianceSetVariance<2>(7, sq(gps_sample.hacc)); } void Ekf::resetHorizontalPositionToVision() { - _information_events.flags.reset_pos_to_vision = true; - ECL_INFO("reset position to ev position"); + _aid_src_ev_pos.state_reset++; + ECL_DEBUG("reset position to ev position"); Vector3f _ev_pos = _ev_sample_delayed.pos; if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) { @@ -175,8 +174,8 @@ void Ekf::resetHorizontalPositionToVision() void Ekf::resetHorizontalPositionToOpticalFlow() { - _information_events.flags.reset_pos_to_last_known = true; - ECL_INFO("reset position to last known position"); + //_aid_src_optical_flow.state_reset++; // TODO + ECL_DEBUG("reset position to last known position"); if (!_control_status.flags.in_air) { // we are likely starting OF for the first time so reset the horizontal position @@ -192,8 +191,7 @@ void Ekf::resetHorizontalPositionToOpticalFlow() void Ekf::resetHorizontalPositionToLastKnown() { - _information_events.flags.reset_pos_to_last_known = true; - ECL_INFO("reset position to last known position"); + ECL_DEBUG("reset position to last known position"); // Used when falling back to non-aiding mode of operation resetHorizontalPositionTo(_last_known_pos.xy()); P.uncorrelateCovarianceSetVariance<2>(7, sq(_params.pos_noaid_noise)); @@ -335,14 +333,12 @@ bool Ekf::realignYawGPS(const Vector3f &mag) // correct yaw angle using GPS ground course if compass yaw bad or yaw is previously not aligned if (badMagYaw || !_control_status.flags.yaw_align) { - _warning_events.flags.bad_yaw_using_gps_course = true; - ECL_WARN("bad yaw, using GPS course"); + ECL_ERR("bad yaw, using GPS course"); // declare the magnetometer as failed if a bad yaw has occurred more than once if (_control_status.flags.mag_aligned_in_flight && (_num_bad_flight_yaw_events >= 2) && !_control_status.flags.mag_fault) { - _warning_events.flags.stopping_mag_use = true; - ECL_WARN("stopping mag use"); + ECL_DEBUG("stopping mag use"); _control_status.flags.mag_fault = true; } @@ -1057,7 +1053,7 @@ void Ekf::updateHorizontalDeadReckoningstatus() if (!_horizontal_deadreckon_time_exceeded && deadreckon_time_exceeded) { // deadreckon time now exceeded - ECL_WARN("dead reckon time exceeded"); + ECL_DEBUG("dead reckon time exceeded"); } _horizontal_deadreckon_time_exceeded = deadreckon_time_exceeded; @@ -1265,7 +1261,7 @@ void Ekf::startMagHdgFusion() { if (!_control_status.flags.mag_hdg) { stopMag3DFusion(); - ECL_INFO("starting mag heading fusion"); + ECL_DEBUG("starting mag heading fusion"); _control_status.flags.mag_hdg = true; } } @@ -1459,8 +1455,7 @@ void Ekf::startGpsFusion(const gpsSample &gps_sample) resetVelocityToGps(gps_sample); } - _information_events.flags.starting_gps_fusion = true; - ECL_INFO("starting GPS fusion"); + ECL_DEBUG("starting GPS fusion"); _control_status.flags.gps = true; } } @@ -1486,7 +1481,7 @@ void Ekf::stopGpsFusion() void Ekf::stopGpsPosFusion() { if (_control_status.flags.gps) { - ECL_INFO("stopping GPS position fusion"); + ECL_DEBUG("stopping GPS position fusion"); _control_status.flags.gps = false; resetEstimatorAidStatus(_aid_src_gnss_pos); @@ -1495,7 +1490,7 @@ void Ekf::stopGpsPosFusion() void Ekf::stopGpsVelFusion() { - ECL_INFO("stopping GPS velocity fusion"); + ECL_DEBUG("stopping GPS velocity fusion"); resetEstimatorAidStatus(_aid_src_gnss_vel); } @@ -1503,7 +1498,7 @@ void Ekf::stopGpsVelFusion() void Ekf::startGpsYawFusion(const gpsSample &gps_sample) { if (!_control_status.flags.gps_yaw && resetYawToGps(gps_sample.yaw)) { - ECL_INFO("starting GPS yaw fusion"); + ECL_DEBUG("starting GPS yaw fusion"); _control_status.flags.yaw_align = true; _control_status.flags.mag_dec = false; stopEvYawFusion(); @@ -1516,7 +1511,7 @@ void Ekf::startGpsYawFusion(const gpsSample &gps_sample) void Ekf::stopGpsYawFusion() { if (_control_status.flags.gps_yaw) { - ECL_INFO("stopping GPS yaw fusion"); + ECL_DEBUG("stopping GPS yaw fusion"); _control_status.flags.gps_yaw = false; resetEstimatorAidStatus(_aid_src_gnss_yaw); } @@ -1526,16 +1521,14 @@ void Ekf::startEvPosFusion() { _control_status.flags.ev_pos = true; resetHorizontalPositionToVision(); - _information_events.flags.starting_vision_pos_fusion = true; - ECL_INFO("starting vision pos fusion"); + ECL_DEBUG("starting vision pos fusion"); } void Ekf::startEvVelFusion() { _control_status.flags.ev_vel = true; resetVelocityToVision(); - _information_events.flags.starting_vision_vel_fusion = true; - ECL_INFO("starting vision vel fusion"); + ECL_DEBUG("starting vision vel fusion"); } void Ekf::startEvYawFusion() @@ -1547,8 +1540,7 @@ void Ekf::startEvYawFusion() stopMagHdgFusion(); stopMag3DFusion(); - _information_events.flags.starting_vision_yaw_fusion = true; - ECL_INFO("starting vision yaw fusion"); + ECL_DEBUG("starting vision yaw fusion"); } void Ekf::stopEvFusion() @@ -1561,7 +1553,7 @@ void Ekf::stopEvFusion() void Ekf::stopEvPosFusion() { if (_control_status.flags.ev_pos) { - ECL_INFO("stopping EV pos fusion"); + ECL_DEBUG("stopping EV pos fusion"); _control_status.flags.ev_pos = false; resetEstimatorAidStatus(_aid_src_ev_pos); } @@ -1570,7 +1562,7 @@ void Ekf::stopEvPosFusion() void Ekf::stopEvVelFusion() { if (_control_status.flags.ev_vel) { - ECL_INFO("stopping EV vel fusion"); + ECL_DEBUG("stopping EV vel fusion"); _control_status.flags.ev_vel = false; resetEstimatorAidStatus(_aid_src_ev_vel); } @@ -1579,14 +1571,14 @@ void Ekf::stopEvVelFusion() void Ekf::stopEvYawFusion() { if (_control_status.flags.ev_yaw) { - ECL_INFO("stopping EV yaw fusion"); + ECL_DEBUG("stopping EV yaw fusion"); _control_status.flags.ev_yaw = false; } } void Ekf::stopAuxVelFusion() { - ECL_INFO("stopping aux vel fusion"); + ECL_DEBUG("stopping aux vel fusion"); //_control_status.flags.aux_vel = false; resetEstimatorAidStatus(_aid_src_aux_vel); } @@ -1594,7 +1586,7 @@ void Ekf::stopAuxVelFusion() void Ekf::stopFlowFusion() { if (_control_status.flags.opt_flow) { - ECL_INFO("stopping optical flow fusion"); + ECL_DEBUG("stopping optical flow fusion"); _control_status.flags.opt_flow = false; _flow_innov.setZero(); _flow_innov_var.setZero(); @@ -1661,11 +1653,9 @@ bool Ekf::resetYawToEKFGSF() // stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around // and cause another navigation failure _control_status.flags.mag_fault = true; - _warning_events.flags.emergency_yaw_reset_mag_stopped = true; } else if (_control_status.flags.gps_yaw) { _control_status.flags.gps_yaw_fault = true; - _warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true; } else if (_control_status.flags.ev_yaw) { _inhibit_ev_yaw_use = true; diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index e42bba901e..67742de65d 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -44,9 +44,9 @@ #if defined(MODULE_NAME) #include -# define ECL_INFO PX4_DEBUG -# define ECL_WARN PX4_DEBUG -# define ECL_ERR PX4_DEBUG +# define ECL_INFO PX4_INFO +# define ECL_WARN PX4_WARN +# define ECL_ERR PX4_ERR # define ECL_DEBUG PX4_DEBUG #else # define ECL_INFO(X, ...) printf(X "\n", ##__VA_ARGS__) @@ -239,14 +239,6 @@ public: const innovation_fault_status_u &innov_check_fail_status() const { return _innov_check_fail_status; } const decltype(innovation_fault_status_u::flags) &innov_check_fail_status_flags() const { return _innov_check_fail_status.flags; } - const warning_event_status_u &warning_event_status() const { return _warning_events; } - const decltype(warning_event_status_u::flags) &warning_event_flags() const { return _warning_events.flags; } - void clear_warning_events() { _warning_events.value = 0; } - - const information_event_status_u &information_event_status() const { return _information_events; } - const decltype(information_event_status_u::flags) &information_event_flags() const { return _information_events.flags; } - void clear_information_events() { _information_events.value = 0; } - // Getter for the average imu update period in s float get_dt_imu_avg() const { return _dt_imu_avg; } @@ -399,11 +391,6 @@ protected: virtual float compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const = 0; - // these are used to record single frame events for external monitoring and should NOT be used for - // state logic becasue they will be cleared externally after being read. - warning_event_status_u _warning_events{}; - information_event_status_u _information_events{}; - private: inline void setDragData(const imuSample &imu); diff --git a/src/modules/ekf2/EKF/ev_height_control.cpp b/src/modules/ekf2/EKF/ev_height_control.cpp index d360e99abd..58d07d60ed 100644 --- a/src/modules/ekf2/EKF/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/ev_height_control.cpp @@ -87,9 +87,9 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample) if (isHeightResetRequired()) { // All height sources are failing - ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); + ECL_DEBUG("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); - _information_events.flags.reset_hgt_to_ev = true; + aid_src.state_reset++; resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var); bias_est.setBias(-_state.pos(2) + measurement); @@ -105,10 +105,10 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample) } else if ((_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter) && !aid_src.fused) { // fusion failed and EV sample indicates reset - ECL_INFO("%s height reset", HGT_SRC_NAME); + ECL_DEBUG("%s height reset", HGT_SRC_NAME); if (_height_sensor_ref == HeightSensor::EV) { - _information_events.flags.reset_hgt_to_ev = true; + aid_src.state_reset++; resetVerticalPositionTo(measurement, measurement_var); bias_est.reset(); @@ -120,27 +120,27 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample) } else if (is_fusion_failing) { // Some other height source is still working - ECL_WARN("stopping %s height fusion, fusion failing", HGT_SRC_NAME); + ECL_DEBUG("stopping %s height fusion, fusion failing", HGT_SRC_NAME); stopEvHgtFusion(); } } else { - ECL_WARN("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME); + ECL_DEBUG("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME); stopEvHgtFusion(); } } else { if (starting_conditions_passing) { if (_params.height_sensor_ref == HeightSensor::EV) { - ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME); + ECL_DEBUG("starting %s height fusion, resetting height", HGT_SRC_NAME); _height_sensor_ref = HeightSensor::EV; - _information_events.flags.reset_hgt_to_ev = true; + aid_src.state_reset++; resetVerticalPositionTo(measurement, measurement_var); bias_est.reset(); } else { - ECL_INFO("starting %s height fusion", HGT_SRC_NAME); + ECL_DEBUG("starting %s height fusion", HGT_SRC_NAME); bias_est.setBias(-_state.pos(2) + measurement); } @@ -153,7 +153,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample) } else if (_control_status.flags.ev_hgt && !isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL)) { // No data anymore. Stop until it comes back. - ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME); + ECL_DEBUG("stopping %s height fusion, no data", HGT_SRC_NAME); stopEvHgtFusion(); } } diff --git a/src/modules/ekf2/EKF/fake_height_control.cpp b/src/modules/ekf2/EKF/fake_height_control.cpp index 9763e6425c..68d6dde842 100644 --- a/src/modules/ekf2/EKF/fake_height_control.cpp +++ b/src/modules/ekf2/EKF/fake_height_control.cpp @@ -79,7 +79,7 @@ void Ekf::controlFakeHgtFusion() } else { if (starting_conditions_passing) { - ECL_INFO("start fake height fusion"); + ECL_DEBUG("start fake height fusion"); _control_status.flags.fake_hgt = true; resetFakeHgtFusion(); } @@ -92,26 +92,25 @@ void Ekf::controlFakeHgtFusion() void Ekf::resetFakeHgtFusion() { - ECL_INFO("reset fake height fusion"); + ECL_DEBUG("reset fake height fusion"); _last_known_pos(2) = _state.pos(2); resetVerticalVelocityToZero(); resetHeightToLastKnown(); - + _aid_src_fake_hgt.state_reset++; _aid_src_fake_hgt.time_last_fuse = _imu_sample_delayed.time_us; } void Ekf::resetHeightToLastKnown() { - _information_events.flags.reset_pos_to_last_known = true; - ECL_INFO("reset height to last known"); + ECL_DEBUG("reset height to last known"); resetVerticalPositionTo(_last_known_pos(2), sq(_params.pos_noaid_noise)); } void Ekf::stopFakeHgtFusion() { if (_control_status.flags.fake_hgt) { - ECL_INFO("stop fake height fusion"); + ECL_DEBUG("stop fake height fusion"); _control_status.flags.fake_hgt = false; resetEstimatorAidStatus(_aid_src_fake_hgt); diff --git a/src/modules/ekf2/EKF/fake_pos_control.cpp b/src/modules/ekf2/EKF/fake_pos_control.cpp index ed44b0d197..8e5ca490c0 100644 --- a/src/modules/ekf2/EKF/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/fake_pos_control.cpp @@ -93,15 +93,14 @@ void Ekf::controlFakePosFusion() } else { if (starting_conditions_passing) { - ECL_INFO("start fake position fusion"); + ECL_DEBUG("start fake position fusion"); _control_status.flags.fake_pos = true; _fuse_hpos_as_odom = false; // TODO: needed? resetFakePosFusion(); if (_control_status.flags.tilt_align) { // The fake position fusion is not started for initial alignement - _warning_events.flags.stopping_navigation = true; - ECL_WARN("stopping navigation"); + ECL_DEBUG("stopping navigation"); } } } @@ -113,7 +112,7 @@ void Ekf::controlFakePosFusion() void Ekf::resetFakePosFusion() { - ECL_INFO("reset fake position fusion"); + ECL_DEBUG("reset fake position fusion"); _last_known_pos.xy() = _state.pos.xy(); resetHorizontalPositionToLastKnown(); @@ -125,7 +124,7 @@ void Ekf::resetFakePosFusion() void Ekf::stopFakePosFusion() { if (_control_status.flags.fake_pos) { - ECL_INFO("stop fake position fusion"); + ECL_DEBUG("stop fake position fusion"); _control_status.flags.fake_pos = false; resetEstimatorAidStatus(_aid_src_fake_pos); diff --git a/src/modules/ekf2/EKF/gnss_height_control.cpp b/src/modules/ekf2/EKF/gnss_height_control.cpp index ce13617e42..0861b5b42d 100644 --- a/src/modules/ekf2/EKF/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/gnss_height_control.cpp @@ -108,9 +108,9 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample) if (isHeightResetRequired()) { // All height sources are failing - ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); + ECL_DEBUG("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); - _information_events.flags.reset_hgt_to_gps = true; + aid_src.state_reset++; resetVerticalPositionTo(-(measurement - bias_est.getBias()), measurement_var); bias_est.setBias(_state.pos(2) + measurement); @@ -127,27 +127,27 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample) } else if (is_fusion_failing) { // Some other height source is still working - ECL_WARN("stopping %s height fusion, fusion failing", HGT_SRC_NAME); + ECL_DEBUG("stopping %s height fusion, fusion failing", HGT_SRC_NAME); stopGpsHgtFusion(); } } else { - ECL_WARN("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME); + ECL_DEBUG("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME); stopGpsHgtFusion(); } } else { if (starting_conditions_passing) { if (_params.height_sensor_ref == HeightSensor::GNSS) { - ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME); + ECL_DEBUG("starting %s height fusion, resetting height", HGT_SRC_NAME); _height_sensor_ref = HeightSensor::GNSS; - _information_events.flags.reset_hgt_to_gps = true; + aid_src.state_reset++; resetVerticalPositionTo(-measurement, measurement_var); bias_est.reset(); } else { - ECL_INFO("starting %s height fusion", HGT_SRC_NAME); + ECL_DEBUG("starting %s height fusion", HGT_SRC_NAME); bias_est.setBias(_state.pos(2) + measurement); } @@ -160,7 +160,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample) } else if (_control_status.flags.gps_hgt && !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL)) { // No data anymore. Stop until it comes back. - ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME); + ECL_DEBUG("stopping %s height fusion, no data", HGT_SRC_NAME); stopGpsHgtFusion(); } } diff --git a/src/modules/ekf2/EKF/gps_checks.cpp b/src/modules/ekf2/EKF/gps_checks.cpp index 97385fbd25..f9c908fbf7 100644 --- a/src/modules/ekf2/EKF/gps_checks.cpp +++ b/src/modules/ekf2/EKF/gps_checks.cpp @@ -103,8 +103,7 @@ bool Ekf::collect_gps(const gpsMessage &gps) _gps_origin_eph = gps.eph; _gps_origin_epv = gps.epv; - _information_events.flags.gps_checks_passed = true; - ECL_INFO("GPS checks passed"); + ECL_DEBUG("GPS checks passed"); } else if (!_NED_origin_initialised) { // a rough 2D fix is still sufficient to lookup declination diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index a0ad6b0bc5..328ed97fea 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -125,7 +125,7 @@ void Ekf::controlGpsFusion() // The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time // to improve its estimate if the previous reset was not successful. if (resetYawToEKFGSF()) { - ECL_WARN("GPS emergency yaw reset"); + ECL_DEBUG("GPS emergency yaw reset"); } } else { @@ -135,8 +135,7 @@ void Ekf::controlGpsFusion() _mag_yaw_reset_req = true; } - _warning_events.flags.gps_fusion_timout = true; - ECL_WARN("GPS fusion timeout - resetting"); + ECL_DEBUG("GPS fusion timeout - resetting"); } resetVelocityToGps(gps_sample); @@ -145,8 +144,7 @@ void Ekf::controlGpsFusion() } else { stopGpsFusion(); - _warning_events.flags.gps_quality_poor = true; - ECL_WARN("GPS quality poor - stopping use"); + ECL_DEBUG("GPS quality poor - stopping use"); // TODO: move this to EV control logic // Reset position state to external vision if we are going to use absolute values @@ -180,8 +178,7 @@ void Ekf::controlGpsFusion() } else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MagFuseType::NONE)) { // If no mag is used, align using the yaw estimator (if available) if (resetYawToEKFGSF()) { - _information_events.flags.yaw_aligned_to_imu_gps = true; - ECL_INFO("Yaw aligned using IMU and GPS"); + ECL_DEBUG("Yaw aligned using IMU and GPS"); resetVelocityToGps(gps_sample); resetHorizontalPositionToGps(gps_sample); } @@ -189,18 +186,16 @@ void Ekf::controlGpsFusion() } } else if (_control_status.flags.gps && !isNewestSampleRecent(_time_last_gps_buffer_push, (uint64_t)10e6)) { + ECL_DEBUG("GPS data stopped"); stopGpsFusion(); - _warning_events.flags.gps_data_stopped = true; - ECL_WARN("GPS data stopped"); } else if (_control_status.flags.gps && !isNewestSampleRecent(_time_last_gps_buffer_push, (uint64_t)1e6) && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { // Handle the case where we are fusing another position source along GPS, // stop waiting for GPS after 1 s of lost signal + ECL_DEBUG("GPS data stopped, using only EV, OF or air data"); stopGpsFusion(); - _warning_events.flags.gps_data_stopped_using_alternate = true; - ECL_WARN("GPS data stopped, using only EV, OF or air data"); } } diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index 1ad9d372e6..fde511dea3 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -104,7 +104,7 @@ void Ekf::checkHeightSensorRefFallback() || ((fallback_list[i] == HeightSensor::GNSS) && _control_status.flags.gps_hgt) || ((fallback_list[i] == HeightSensor::RANGE) && _control_status.flags.rng_hgt) || ((fallback_list[i] == HeightSensor::EV) && _control_status.flags.ev_hgt)) { - ECL_INFO("fallback to secondary height reference"); + ECL_DEBUG("fallback to secondary height reference"); _height_sensor_ref = fallback_list[i]; break; } diff --git a/src/modules/ekf2/EKF/range_height_control.cpp b/src/modules/ekf2/EKF/range_height_control.cpp index 4108098a8d..c5589191bb 100644 --- a/src/modules/ekf2/EKF/range_height_control.cpp +++ b/src/modules/ekf2/EKF/range_height_control.cpp @@ -92,9 +92,9 @@ void Ekf::controlRangeHeightFusion() if (isHeightResetRequired()) { // All height sources are failing - ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); + ECL_DEBUG("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); - _information_events.flags.reset_hgt_to_rng = true; + aid_src.state_reset++; resetVerticalPositionTo(-(measurement - bias_est.getBias())); bias_est.setBias(_state.pos(2) + measurement); @@ -105,14 +105,14 @@ void Ekf::controlRangeHeightFusion() } else if (is_fusion_failing) { // Some other height source is still working - ECL_WARN("stopping %s height fusion, fusion failing", HGT_SRC_NAME); + ECL_DEBUG("stopping %s height fusion, fusion failing", HGT_SRC_NAME); stopRngHgtFusion(); _control_status.flags.rng_fault = true; _range_sensor.setFaulty(); } } else { - ECL_WARN("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME); + ECL_DEBUG("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME); stopRngHgtFusion(); } @@ -120,22 +120,22 @@ void Ekf::controlRangeHeightFusion() if (starting_conditions_passing) { if ((_params.height_sensor_ref == HeightSensor::RANGE) && (_params.rng_ctrl == RngCtrl::CONDITIONAL)) { // Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference. - ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME); + ECL_DEBUG("starting conditional %s height fusion", HGT_SRC_NAME); _height_sensor_ref = HeightSensor::RANGE; bias_est.setBias(_state.pos(2) + measurement); } else if ((_params.height_sensor_ref == HeightSensor::RANGE) && (_params.rng_ctrl != RngCtrl::CONDITIONAL)) { // Range finder is the primary height source, the ground is now the datum used // to compute the local vertical position - ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME); + ECL_DEBUG("starting %s height fusion, resetting height", HGT_SRC_NAME); _height_sensor_ref = HeightSensor::RANGE; - _information_events.flags.reset_hgt_to_rng = true; + aid_src.state_reset++; resetVerticalPositionTo(-measurement, measurement_var); bias_est.reset(); } else { - ECL_INFO("starting %s height fusion", HGT_SRC_NAME); + ECL_DEBUG("starting %s height fusion", HGT_SRC_NAME); bias_est.setBias(_state.pos(2) + measurement); } @@ -148,7 +148,7 @@ void Ekf::controlRangeHeightFusion() } else if (_control_status.flags.rng_hgt && !isNewestSampleRecent(_time_last_range_buffer_push, 2 * RNG_MAX_INTERVAL)) { // No data anymore. Stop until it comes back. - ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME); + ECL_DEBUG("stopping %s height fusion, no data", HGT_SRC_NAME); stopRngHgtFusion(); } } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 4af4f9a00e..dfdf7e96a6 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -172,7 +172,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _attitude_pub.advertise(); _local_position_pub.advertise(); - _estimator_event_flags_pub.advertise(); _estimator_innovation_test_ratios_pub.advertise(); _estimator_innovation_variances_pub.advertise(); _estimator_innovations_pub.advertise(); @@ -203,7 +202,6 @@ bool EKF2::multi_init(int imu, int mag) _ekf2_timestamps_pub.advertise(); _estimator_baro_bias_pub.advertise(); _estimator_ev_hgt_bias_pub.advertise(); - _estimator_event_flags_pub.advertise(); _estimator_gnss_hgt_bias_pub.advertise(); _estimator_gps_status_pub.advertise(); _estimator_innovation_test_ratios_pub.advertise(); @@ -613,7 +611,6 @@ void EKF2::Run() PublishGnssHgtBias(now); PublishRngHgtBias(now); PublishEvHgtBias(now); - PublishEventFlags(now); PublishGpsStatus(now); PublishInnovations(now); PublishInnovationTestRatios(now); @@ -839,79 +836,6 @@ estimator_bias_s EKF2::fillEstimatorBiasMsg(const BiasEstimator::status &status, return bias; } -void EKF2::PublishEventFlags(const hrt_abstime ×tamp) -{ - // information events - uint32_t information_events = _ekf.information_event_status().value; - bool information_event_updated = false; - - if (information_events != 0) { - information_event_updated = true; - _filter_information_event_changes++; - } - - // warning events - uint32_t warning_events = _ekf.warning_event_status().value; - bool warning_event_updated = false; - - if (warning_events != 0) { - warning_event_updated = true; - _filter_warning_event_changes++; - } - - if (information_event_updated || warning_event_updated) { - estimator_event_flags_s event_flags{}; - event_flags.timestamp_sample = _ekf.get_imu_sample_delayed().time_us; - - event_flags.information_event_changes = _filter_information_event_changes; - event_flags.gps_checks_passed = _ekf.information_event_flags().gps_checks_passed; - event_flags.reset_vel_to_gps = _ekf.information_event_flags().reset_vel_to_gps; - event_flags.reset_vel_to_flow = _ekf.information_event_flags().reset_vel_to_flow; - event_flags.reset_vel_to_vision = _ekf.information_event_flags().reset_vel_to_vision; - event_flags.reset_vel_to_zero = _ekf.information_event_flags().reset_vel_to_zero; - event_flags.reset_pos_to_last_known = _ekf.information_event_flags().reset_pos_to_last_known; - event_flags.reset_pos_to_gps = _ekf.information_event_flags().reset_pos_to_gps; - event_flags.reset_pos_to_vision = _ekf.information_event_flags().reset_pos_to_vision; - event_flags.starting_gps_fusion = _ekf.information_event_flags().starting_gps_fusion; - event_flags.starting_vision_pos_fusion = _ekf.information_event_flags().starting_vision_pos_fusion; - event_flags.starting_vision_vel_fusion = _ekf.information_event_flags().starting_vision_vel_fusion; - event_flags.starting_vision_yaw_fusion = _ekf.information_event_flags().starting_vision_yaw_fusion; - event_flags.yaw_aligned_to_imu_gps = _ekf.information_event_flags().yaw_aligned_to_imu_gps; - event_flags.reset_hgt_to_baro = _ekf.information_event_flags().reset_hgt_to_baro; - event_flags.reset_hgt_to_gps = _ekf.information_event_flags().reset_hgt_to_gps; - event_flags.reset_hgt_to_rng = _ekf.information_event_flags().reset_hgt_to_rng; - event_flags.reset_hgt_to_ev = _ekf.information_event_flags().reset_hgt_to_ev; - - event_flags.warning_event_changes = _filter_warning_event_changes; - event_flags.gps_quality_poor = _ekf.warning_event_flags().gps_quality_poor; - event_flags.gps_fusion_timout = _ekf.warning_event_flags().gps_fusion_timout; - event_flags.gps_data_stopped = _ekf.warning_event_flags().gps_data_stopped; - event_flags.gps_data_stopped_using_alternate = _ekf.warning_event_flags().gps_data_stopped_using_alternate; - event_flags.height_sensor_timeout = _ekf.warning_event_flags().height_sensor_timeout; - event_flags.stopping_navigation = _ekf.warning_event_flags().stopping_mag_use; - event_flags.invalid_accel_bias_cov_reset = _ekf.warning_event_flags().invalid_accel_bias_cov_reset; - event_flags.bad_yaw_using_gps_course = _ekf.warning_event_flags().bad_yaw_using_gps_course; - event_flags.stopping_mag_use = _ekf.warning_event_flags().stopping_mag_use; - event_flags.vision_data_stopped = _ekf.warning_event_flags().vision_data_stopped; - event_flags.emergency_yaw_reset_mag_stopped = _ekf.warning_event_flags().emergency_yaw_reset_mag_stopped; - event_flags.emergency_yaw_reset_gps_yaw_stopped = _ekf.warning_event_flags().emergency_yaw_reset_gps_yaw_stopped; - - event_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); - _estimator_event_flags_pub.update(event_flags); - - _last_event_flags_publish = event_flags.timestamp; - - _ekf.clear_information_events(); - _ekf.clear_warning_events(); - - } else if ((_last_event_flags_publish != 0) && (timestamp >= _last_event_flags_publish + 1_s)) { - // continue publishing periodically - _estimator_event_flags_pub.get().timestamp = _replay_mode ? timestamp : hrt_absolute_time(); - _estimator_event_flags_pub.update(); - _last_event_flags_publish = _estimator_event_flags_pub.get().timestamp; - } -} - void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) { if (_ekf.global_position_is_valid()) { @@ -2136,8 +2060,7 @@ void EKF2::UpdateAccelCalibration(const hrt_abstime ×tamp) && (_ekf.fault_status().value == 0) && !_ekf.fault_status_flags().bad_acc_bias && !_ekf.fault_status_flags().bad_acc_clipping - && !_ekf.fault_status_flags().bad_acc_vertical - && !_ekf.warning_event_flags().invalid_accel_bias_cov_reset; + && !_ekf.fault_status_flags().bad_acc_vertical; const bool learning_valid = bias_valid && !_ekf.accel_bias_inhibited(); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index a28d24f928..992e0e9c05 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -69,7 +69,6 @@ #include #include #include -#include #include #include #include @@ -145,7 +144,6 @@ private: void PublishEvHgtBias(const hrt_abstime ×tamp); estimator_bias_s fillEstimatorBiasMsg(const BiasEstimator::status &status, uint64_t timestamp_sample_us, uint64_t timestamp, uint32_t device_id = 0); - void PublishEventFlags(const hrt_abstime ×tamp); void PublishGlobalPosition(const hrt_abstime ×tamp); void PublishGpsStatus(const hrt_abstime ×tamp); void PublishInnovations(const hrt_abstime ×tamp); @@ -320,7 +318,6 @@ private: bool _callback_registered{false}; - hrt_abstime _last_event_flags_publish{0}; hrt_abstime _last_status_flags_publish{0}; uint64_t _filter_control_status{0}; @@ -330,15 +327,12 @@ private: uint32_t _filter_control_status_changes{0}; uint32_t _filter_fault_status_changes{0}; uint32_t _innov_check_fail_status_changes{0}; - uint32_t _filter_warning_event_changes{0}; - uint32_t _filter_information_event_changes{0}; uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; uORB::PublicationMulti _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)}; uORB::PublicationMulti _estimator_gnss_hgt_bias_pub{ORB_ID(estimator_gnss_hgt_bias)}; uORB::PublicationMulti _estimator_rng_hgt_bias_pub{ORB_ID(estimator_rng_hgt_bias)}; uORB::PublicationMulti _estimator_ev_hgt_bias_pub{ORB_ID(estimator_ev_hgt_bias)}; - uORB::PublicationMultiData _estimator_event_flags_pub{ORB_ID(estimator_event_flags)}; uORB::PublicationMulti _estimator_gps_status_pub{ORB_ID(estimator_gps_status)}; uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; uORB::PublicationMulti _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 7d78da5d5b..01b592ab01 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -148,7 +148,6 @@ void LoggedTopics::add_default_topics() add_topic("estimator_gnss_hgt_bias", 500); add_topic("estimator_rng_hgt_bias", 500); add_topic("estimator_ev_hgt_bias", 500); - add_topic("estimator_event_flags", 0); add_topic("estimator_gps_status", 1000); add_topic("estimator_innovation_test_ratios", 500); add_topic("estimator_innovation_variances", 500); @@ -165,7 +164,6 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("estimator_gnss_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_rng_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_ev_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_gps_status", 1000, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES); @@ -248,7 +246,6 @@ void LoggedTopics::add_default_topics() add_topic("estimator_gnss_hgt_bias"); add_topic("estimator_rng_hgt_bias"); add_topic("estimator_ev_hgt_bias"); - add_topic("estimator_event_flags"); add_topic("estimator_gps_status"); add_topic("estimator_innovation_test_ratios"); add_topic("estimator_innovation_variances");