fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 16:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2/commander: simplify navigation filter preflight checks

Browse Source 
- remove commander test ratio "tuning knobs" (COM_ARM_EKF_{HGT,POS,VEL,YAW})
   - these are effectively redundant with the actual tuning (noise & gate)
     in the estimator, plus most users have no idea why they'd be
     adjusting these other than to silence an annoying preflight complaint
 - remove ekf2 "PreFlightChecker" with hard coded innovation limits
 - ekf2 preflight innovation flags are now simply if any active source
   exceeds half the limit preflight
This commit is contained in:
Daniel Agar
2024-06-17 18:48:12 -04:00
committed by Mathieu Bresciani
parent a42dc2165c
commit eac14b7db2
15 changed files with 104 additions and 806 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+24 -42
View File
@@ -1333,43 +1333,6 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_innovations_pub.publish(innovations);
// calculate noise filtered velocity innovations which are used for pre-flight checking
if (_ekf.control_status_prev_flags().in_air != _ekf.control_status_flags().in_air) {
// fully reset on takeoff or landing
_preflt_checker.reset();
}
if (!_ekf.control_status_flags().in_air) {
// TODO: move to run before publications
_preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps);
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
_preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow);
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_OPTICAL_FLOW)
// set dist bottom to scale flow innovation
const float dist_bottom = _ekf.getHagl();
_preflt_checker.setDistBottom(dist_bottom);
#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
_preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos);
_preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel);
_preflt_checker.setUsingEvHgtAiding(_ekf.control_status_flags().ev_hgt);
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_BAROMETER)
_preflt_checker.setUsingBaroHgtAiding(_ekf.control_status_flags().baro_hgt);
#endif // CONFIG_EKF2_BAROMETER
_preflt_checker.setUsingGpsHgtAiding(_ekf.control_status_flags().gps_hgt);
#if defined(CONFIG_EKF2_RANGE_FINDER)
_preflt_checker.setUsingRngHgtAiding(_ekf.control_status_flags().rng_hgt);
#endif // CONFIG_EKF2_RANGE_FINDER
_preflt_checker.setVehicleCanObserveHeadingInFlight(_ekf.control_status_flags().fixed_wing);
_preflt_checker.update(_ekf.get_dt_ekf_avg(), innovations);
}
}
void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
@@ -1816,8 +1779,24 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
status.control_mode_flags = _ekf.control_status().value;
status.filter_fault_flags = _ekf.fault_status().value;
// vel_test_ratio
float vel_xy_test_ratio = _ekf.getHorizontalVelocityInnovationTestRatio();
float vel_z_test_ratio = _ekf.getVerticalVelocityInnovationTestRatio();
if (PX4_ISFINITE(vel_xy_test_ratio) && PX4_ISFINITE(vel_z_test_ratio)) {
status.vel_test_ratio = math::max(vel_xy_test_ratio, vel_z_test_ratio);
} else if (PX4_ISFINITE(vel_xy_test_ratio)) {
status.vel_test_ratio = vel_xy_test_ratio;
} else if (PX4_ISFINITE(vel_z_test_ratio)) {
status.vel_test_ratio = vel_z_test_ratio;
} else {
status.vel_test_ratio = NAN;
}
status.mag_test_ratio = _ekf.getHeadingInnovationTestRatio();
status.vel_test_ratio = _ekf.getVelocityInnovationTestRatio();
status.pos_test_ratio = _ekf.getHorizontalPositionInnovationTestRatio();
status.hgt_test_ratio = _ekf.getVerticalPositionInnovationTestRatio();
status.tas_test_ratio = _ekf.getAirspeedInnovationTestRatio();
@@ -1836,10 +1815,13 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
status.time_slip = _last_time_slip_us * 1e-6f;
status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed();
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();
static constexpr float kMinTestRatioPreflight = 0.5f;
status.pre_flt_fail_innov_heading = (kMinTestRatioPreflight < status.mag_test_ratio);
status.pre_flt_fail_innov_height = (kMinTestRatioPreflight < status.hgt_test_ratio);
status.pre_flt_fail_innov_pos_horiz = (kMinTestRatioPreflight < status.pos_test_ratio);
status.pre_flt_fail_innov_vel_horiz = (kMinTestRatioPreflight < vel_xy_test_ratio);
status.pre_flt_fail_innov_vel_vert = (kMinTestRatioPreflight < vel_z_test_ratio);
status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed;
status.accel_device_id = _device_id_accel;