fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-15 04:20:35 +08:00
Code Issues 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
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;