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

ekf2: refactor output predictor to class

Browse Source 
- refactor all EKF backend output predictor pieces into new OutputPredictor class
 - output states are now calculated immediately with new high rate IMU rather than after EKF update
 - IMU delayed sample is passed as around as control data to avoid storing an extra copy and make the requirement clear
This commit is contained in:
Daniel Agar
2023-01-18 10:59:34 -05:00
committed by GitHub
parent c054ca20cc
commit 95300d5637
39 changed files with 855 additions and 618 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/height_control.cpp
+9 -8
View File
@@ -37,9 +37,9 @@
#include "ekf.h"
void Ekf::controlHeightFusion()
void Ekf::controlHeightFusion(const imuSample &imu_delayed)
{
checkVerticalAccelerationHealth();
checkVerticalAccelerationHealth(imu_delayed);
updateGroundEffect();
@@ -61,6 +61,7 @@ void Ekf::checkHeightSensorRefFallback()
switch (_params.height_sensor_ref) {
default:
/* FALLTHROUGH */
case HeightSensor::UNKNOWN:
fallback_list[0] = HeightSensor::GNSS;
@@ -110,7 +111,7 @@ void Ekf::checkHeightSensorRefFallback()
}
}
void Ekf::checkVerticalAccelerationHealth()
void Ekf::checkVerticalAccelerationHealth(const imuSample &imu_delayed)
{
// Check for IMU accelerometer vibration induced clipping as evidenced by the vertical
// innovations being positive and not stale.
@@ -121,9 +122,9 @@ void Ekf::checkVerticalAccelerationHealth()
// Check for more than 50% clipping affected IMU samples within the past 1 second
const uint16_t clip_count_limit = 1.f / _dt_ekf_avg;
const bool is_clipping = _imu_sample_delayed.delta_vel_clipping[0] ||
_imu_sample_delayed.delta_vel_clipping[1] ||
_imu_sample_delayed.delta_vel_clipping[2];
const bool is_clipping = imu_delayed.delta_vel_clipping[0] ||
imu_delayed.delta_vel_clipping[1] ||
imu_delayed.delta_vel_clipping[2];
if (is_clipping && _clip_counter < clip_count_limit) {
_clip_counter++;
@@ -141,10 +142,10 @@ void Ekf::checkVerticalAccelerationHealth()
|| (inertial_nav_falling_likelihood == Likelihood::HIGH);
if (bad_vert_accel) {
_time_bad_vert_accel = _imu_sample_delayed.time_us;
_time_bad_vert_accel = imu_delayed.time_us;
} else {
_time_good_vert_accel = _imu_sample_delayed.time_us;
_time_good_vert_accel = imu_delayed.time_us;
}
// declare a bad vertical acceleration measurement and make the declaration persist