diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg index 56b147b2a7..898c58648f 100644 --- a/msg/estimator_status.msg +++ b/msg/estimator_status.msg @@ -47,7 +47,7 @@ uint8 CS_RNG_STUCK = 21 # 21 - true when a stuck range finder sensor has been d uint8 CS_GPS_YAW = 22 # 22 - true when yaw (not ground course) data from a GPS receiver is being fused uint8 CS_MAG_ALIGNED = 23 # 23 - true when the in-flight mag field alignment has been completed -uint16 filter_fault_flags # Bitmask to indicate EKF internal faults +uint32 filter_fault_flags # Bitmask to indicate EKF internal faults # 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error # 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error # 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error @@ -64,6 +64,8 @@ uint16 filter_fault_flags # Bitmask to indicate EKF internal faults # 13 - true if fusion of the East position has encountered a numerical error # 14 - true if fusion of the Down position has encountered a numerical error # 15 - true if bad delta velocity bias estimates have been detected +# 16 - true if bad vertical accelerometer data has been detected +# 17 - true if delta velocity data contains clipping (asymmetric railing) float32 pos_horiz_accuracy # 1-Sigma estimated horizontal position accuracy relative to the estimators origin (m) float32 pos_vert_accuracy # 1-Sigma estimated vertical position accuracy relative to the estimators origin (m) diff --git a/src/lib/ecl b/src/lib/ecl index 8f3df7a97b..1541e4b414 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 8f3df7a97b348dd7bf06233004f9821fe2ea88d1 +Subproject commit 1541e4b414fe63ec9d6c3077375db7e20c20ef4a diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 6a356b9f5d..d4793c3bd9 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -598,13 +598,10 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp, const imuSample &imu // calculate noise filtered velocity innovations which are used for pre-flight checking if (_standby) { // TODO: move to run before publications - filter_control_status_u control_status; - _ekf.get_control_mode(&control_status.value); - - _preflt_checker.setUsingGpsAiding(control_status.flags.gps); - _preflt_checker.setUsingFlowAiding(control_status.flags.opt_flow); - _preflt_checker.setUsingEvPosAiding(control_status.flags.ev_pos); - _preflt_checker.setUsingEvVelAiding(control_status.flags.ev_vel); + _preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps); + _preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow); + _preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos); + _preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel); _preflt_checker.update(imu.delta_ang_dt, innovations); } @@ -948,11 +945,8 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) // the GPS Fix bit, which is always checked) status.gps_check_fail_flags &= ((uint16_t)_params->gps_check_mask << 1) | 1; - filter_control_status_u control_status; - _ekf.get_control_mode(&control_status.value); - status.control_mode_flags = control_status.value; - - _ekf.get_filter_fault_status(&status.filter_fault_flags); + status.control_mode_flags = _ekf.control_status().value; + status.filter_fault_flags = _ekf.fault_status().value; _ekf.get_innovation_test_status(status.innovation_check_flags, status.mag_test_ratio, status.vel_test_ratio, status.pos_test_ratio, status.hgt_test_ratio, status.tas_test_ratio, @@ -968,7 +962,7 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed(); status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed(); status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed(); - status.pre_flt_fail_mag_field_disturbed = control_status.flags.mag_field_disturbed; + status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed; status.accel_device_id = _device_id_accel; status.baro_device_id = _device_id_baro; @@ -1384,12 +1378,9 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp) { - fault_status_u fault_status; - _ekf.get_filter_fault_status(&fault_status.value); - // Check if conditions are OK for learning of magnetometer bias values // the EKF is operating in the correct mode and there are no filter faults - if (_ekf.control_status_flags().in_air && _ekf.control_status_flags().mag_3D && (fault_status.value == 0)) { + if (_ekf.control_status_flags().in_air && _ekf.control_status_flags().mag_3D && (_ekf.fault_status().value == 0)) { if (_last_magcal_us == 0) { _last_magcal_us = timestamp; @@ -1417,7 +1408,7 @@ void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp) } } - } else if (fault_status.value != 0) { + } else if (_ekf.fault_status().value != 0) { // if a filter fault has occurred, assume previous learning was invalid and do not // count it towards total learning time. _total_cal_time_us = 0;