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

ekf2: directly use IMU sample to find corresponding aid source sample

Browse Source 
- I think this helps make it clear we're using a sensor sample
   corresponding with a particular IMU sample
This commit is contained in:
Daniel Agar 2024-06-27 12:33:17 -04:00
parent f93dc61770
commit ac77049c47
8 changed files with 23 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp
View File

@ -33,12 +33,12 @@
#include "ekf.h"
void Ekf::controlAuxVelFusion()
void Ekf::controlAuxVelFusion(const imuSample &imu_sample)
{
if (_auxvel_buffer) {
auxVelSample sample;
if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &sample)) {
if (_auxvel_buffer->pop_first_older_than(imu_sample.time_us, &sample)) {
updateAidSourceStatus(_aid_src_aux_vel,
sample.time_us, // sample timestamp

6
src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp
View File

@ -49,7 +49,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
baroSample baro_sample;
if (_baro_buffer && _baro_buffer->pop_first_older_than(_time_delayed_us, &baro_sample)) {
if (_baro_buffer && _baro_buffer->pop_first_older_than(imu_sample.time_us, &baro_sample)) {
#if defined(CONFIG_EKF2_BARO_COMPENSATION)
const float measurement = compensateBaroForDynamicPressure(imu_sample, baro_sample.hgt);
@ -137,7 +137,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
// reset vertical velocity
resetVerticalVelocityToZero();
aid_src.time_last_fuse = _time_delayed_us;
aid_src.time_last_fuse = imu_sample.time_us;
} else if (is_fusion_failing) {
// Some other height source is still working
@ -166,7 +166,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
}
aid_src.time_last_fuse = _time_delayed_us;
aid_src.time_last_fuse = imu_sample.time_us;
bias_est.setFusionActive();
_control_status.flags.baro_hgt = true;
}

2
src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp
View File

@ -46,7 +46,7 @@ void Ekf::controlExternalVisionFusion(const imuSample &imu_sample)
// Check for new external vision data
extVisionSample ev_sample;
if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(_time_delayed_us, &ev_sample)) {
if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(imu_sample.time_us, &ev_sample)) {
bool ev_reset = (ev_sample.reset_counter != _ev_sample_prev.reset_counter);

10
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
View File

@ -41,7 +41,7 @@
#include <ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h>
void Ekf::controlMagFusion()
void Ekf::controlMagFusion(const imuSample &imu_sample)
{
static constexpr const char *AID_SRC_NAME = "mag";
estimator_aid_source3d_s &aid_src = _aid_src_mag;
@ -59,7 +59,7 @@ void Ekf::controlMagFusion()
magSample mag_sample;
if (_mag_buffer && _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample)) {
if (_mag_buffer && _mag_buffer->pop_first_older_than(imu_sample.time_us, &mag_sample)) {
if (mag_sample.reset || (_mag_counter == 0)) {
// sensor or calibration has changed, reset low pass filter
@ -185,7 +185,7 @@ void Ekf::controlMagFusion()
if (mag_sample.reset || checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_pre_takeoff)) {
ECL_INFO("reset to %s", AID_SRC_NAME);
resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
aid_src.time_last_fuse = _time_delayed_us;
aid_src.time_last_fuse = imu_sample.time_us;
} else {
// The normal sequence is to fuse the magnetometer data first before fusing
@ -213,7 +213,7 @@ void Ekf::controlMagFusion()
if (no_ne_aiding_or_pre_takeoff) {
ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
aid_src.time_last_fuse = _time_delayed_us;
aid_src.time_last_fuse = imu_sample.time_us;
} else {
ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
@ -242,7 +242,7 @@ void Ekf::controlMagFusion()
bool reset_heading = !_control_status.flags.yaw_align;
resetMagStates(_mag_lpf.getState(), reset_heading);
aid_src.time_last_fuse = _time_delayed_us;
aid_src.time_last_fuse = imu_sample.time_us;
if (reset_heading) {
_control_status.flags.yaw_align = true;

12
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
View File

@ -39,7 +39,7 @@
#include "ekf.h"
#include "ekf_derivation/generated/compute_hagl_innov_var.h"
void Ekf::controlRangeHaglFusion()
void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
{
static constexpr const char *HGT_SRC_NAME = "RNG";
@ -47,7 +47,7 @@ void Ekf::controlRangeHaglFusion()
if (_range_buffer) {
// Get range data from buffer and check validity
rng_data_ready = _range_buffer->pop_first_older_than(_time_delayed_us, _range_sensor.getSampleAddress());
rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress());
_range_sensor.setDataReadiness(rng_data_ready);
// update range sensor angle parameters in case they have changed
@ -55,7 +55,7 @@ void Ekf::controlRangeHaglFusion()
_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
_range_sensor.runChecks(_time_delayed_us, _R_to_earth);
_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);
if (_range_sensor.isDataHealthy()) {
// correct the range data for position offset relative to the IMU
@ -72,7 +72,7 @@ void Ekf::controlRangeHaglFusion()
_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2),
P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, _time_delayed_us);
P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, imu_sample.time_us);
}
} else {
@ -151,7 +151,7 @@ void Ekf::controlRangeHaglFusion()
_control_status.flags.rng_hgt = true;
stopRngTerrFusion();
aid_src.time_last_fuse = _time_delayed_us;
aid_src.time_last_fuse = imu_sample.time_us;
}
} else {
@ -183,7 +183,7 @@ void Ekf::controlRangeHaglFusion()
// reset vertical velocity
resetVerticalVelocityToZero();
aid_src.time_last_fuse = _time_delayed_us;
aid_src.time_last_fuse = imu_sample.time_us;
} else if (is_fusion_failing) {
// Some other height source is still working

4
src/modules/ekf2/EKF/control.cpp
View File

@ -102,7 +102,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
#if defined(CONFIG_EKF2_MAGNETOMETER)
// control use of observations for aiding
controlMagFusion();
controlMagFusion(imu_delayed);
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
@ -142,7 +142,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
#if defined(CONFIG_EKF2_AUXVEL)
// Additional horizontal velocity data from an auxiliary sensor can be fused
controlAuxVelFusion();
controlAuxVelFusion(imu_delayed);
#endif // CONFIG_EKF2_AUXVEL
//
#if defined(CONFIG_EKF2_TERRAIN)

6
src/modules/ekf2/EKF/ekf.h
View File

@ -841,7 +841,7 @@ private:
#if defined(CONFIG_EKF2_RANGE_FINDER)
// range height
void controlRangeHaglFusion();
void controlRangeHaglFusion(const imuSample &imu_delayed);
bool isConditionalRangeAidSuitable();
void stopRngHgtFusion();
void stopRngTerrFusion();
@ -1019,7 +1019,7 @@ private:
#if defined(CONFIG_EKF2_MAGNETOMETER)
// control fusion of magnetometer observations
void controlMagFusion();
void controlMagFusion(const imuSample &imu_sample);
bool checkHaglYawResetReq() const;
@ -1052,7 +1052,7 @@ private:
#if defined(CONFIG_EKF2_AUXVEL)
// control fusion of auxiliary velocity observations
void controlAuxVelFusion();
void controlAuxVelFusion(const imuSample &imu_sample);
void stopAuxVelFusion();
#endif // CONFIG_EKF2_AUXVEL

2
src/modules/ekf2/EKF/height_control.cpp
View File

@ -53,7 +53,7 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed)
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_RANGE_FINDER)
controlRangeHaglFusion();
controlRangeHaglFusion(imu_delayed);
#endif // CONFIG_EKF2_RANGE_FINDER
checkHeightSensorRefFallback();