Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 22:32:11 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

ekf2: update to new ecl to fix fault status getter

Browse Source 
- estimator_status filter_fault_flags was broken because the union within ecl/EKF has exceeded 16 bits
This commit is contained in:
Daniel Agar
2020-12-10 12:45:41 -05:00
committed by GitHub
parent 72420dfff3
commit d44e537084
3 changed files with 13 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/estimator_status.msg
+3 -1
View File
@@ -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)
src/lib/ecl
+1 -1
Submodule src/lib/ecl updated: 8f3df7a97b...1541e4b414
src/modules/ekf2/EKF2.cpp
+9 -18
View File
@@ -598,13 +598,10 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp, 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 &timestamp)
// 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 &timestamp)
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 &timestamp)
{
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 &timestamp)
}
}
} 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;