fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 17:53: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/mag_control.cpp
+10 -10
View File
@@ -46,7 +46,7 @@ void Ekf::controlMagFusion()
magSample mag_sample;
if (_mag_buffer) {
mag_data_ready = _mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &mag_sample);
mag_data_ready = _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample);
if (mag_data_ready) {
@@ -129,7 +129,7 @@ void Ekf::controlMagFusion()
if (mag_data_ready && !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw) {
if (shouldInhibitMag()) {
if (uint32_t(_imu_sample_delayed.time_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) {
if (uint32_t(_time_delayed_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) {
// If magnetometer use has been inhibited continuously then stop the fusion
stopMagFusion();
}
@@ -137,7 +137,7 @@ void Ekf::controlMagFusion()
return;
} else {
_mag_use_not_inhibit_us = _imu_sample_delayed.time_us;
_mag_use_not_inhibit_us = _time_delayed_us;
}
const bool mag_enabled_previously = _control_status_prev.flags.mag_hdg || _control_status_prev.flags.mag_3D;
@@ -216,7 +216,7 @@ void Ekf::runOnGroundYawReset()
void Ekf::runInAirYawReset()
{
// prevent a reset being performed more than once on the same frame
if ((_flt_mag_align_start_time == _imu_sample_delayed.time_us)
if ((_flt_mag_align_start_time == _time_delayed_us)
|| (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
return;
}
@@ -269,7 +269,7 @@ void Ekf::runInAirYawReset()
_control_status.flags.mag_aligned_in_flight = true;
// record the time for the magnetic field alignment event
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
_flt_mag_align_start_time = _time_delayed_us;
}
}
}
@@ -286,7 +286,7 @@ void Ekf::check3DMagFusionSuitability()
checkMagBiasObservability();
if (_mag_bias_observable || _yaw_angle_observable) {
_time_last_mov_3d_mag_suitable = _imu_sample_delayed.time_us;
_time_last_mov_3d_mag_suitable = _time_delayed_us;
}
}
@@ -316,13 +316,13 @@ void Ekf::checkMagBiasObservability()
} else if (_mag_bias_observable) {
// require sustained yaw motion of 50% the initial yaw rate threshold
const float yaw_dt = 1e-6f * (float)(_imu_sample_delayed.time_us - _time_yaw_started);
const float yaw_dt = 1e-6f * (float)(_time_delayed_us - _time_yaw_started);
const float min_yaw_change_req = 0.5f * _params.mag_yaw_rate_gate * yaw_dt;
_mag_bias_observable = fabsf(_yaw_delta_ef) > min_yaw_change_req;
}
_yaw_delta_ef = 0.0f;
_time_yaw_started = _imu_sample_delayed.time_us;
_time_yaw_started = _time_delayed_us;
}
bool Ekf::canUse3DMagFusion() const
@@ -330,7 +330,7 @@ bool Ekf::canUse3DMagFusion() const
// Use of 3D fusion requires an in-air heading alignment but it should not
// be used when the heading and mag biases are not observable for more than 2 seconds
return _control_status.flags.mag_aligned_in_flight
&& ((_imu_sample_delayed.time_us - _time_last_mov_3d_mag_suitable) < (uint64_t)2e6);
&& ((_time_delayed_us - _time_last_mov_3d_mag_suitable) < (uint64_t)2e6);
}
void Ekf::checkMagDeclRequired()
@@ -411,7 +411,7 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
{
// For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise
// before they are used to constrain heading drift
const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6);
const bool update_all_states = ((_time_delayed_us - _flt_mag_align_start_time) > (uint64_t)5e6);
if (!_mag_decl_cov_reset) {
// After any magnetic field covariance reset event the earth field state