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

ekf2: EV overhaul yaw and position fusion (#20501)

Browse Source 
- move EV yaw and EV position to new state machines
 - EV yaw and EV pos now configured via EKF2_EV_CTRL (migrated from EKF2_AID_MASK)
 - new EV position offset estimator to enable EV position while GPS position is active (no more EV pos delta fusion)
 - yaw_align now strictly means north (no more rotate external vision aid mask)
 - automatic switching between EV yaw, and yaw align north based on GPS quality
This commit is contained in:
Daniel Agar
2022-12-20 10:23:56 -05:00
committed by GitHub
parent 20342216e2
commit 54a32eb2f7
34 changed files with 1019 additions and 553 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_control.cpp
+21 -10
View File
@@ -218,6 +218,12 @@ bool Ekf::canResetMagHeading() const
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)
|| (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
return;
}
if (_mag_yaw_reset_req && !_is_yaw_fusion_inhibited) {
bool has_realigned_yaw = false;
@@ -252,6 +258,8 @@ void Ekf::runInAirYawReset()
);
resetMagCov();
has_realigned_yaw = true;
}
if (!has_realigned_yaw && canResetMagHeading()) {
@@ -296,14 +304,19 @@ void Ekf::check3DMagFusionSuitability()
void Ekf::checkYawAngleObservability()
{
// Check if there has been enough change in horizontal velocity to make yaw observable
// Apply hysteresis to check to avoid rapid toggling
_yaw_angle_observable = _yaw_angle_observable
? _accel_lpf_NE.norm() > _params.mag_acc_gate
: _accel_lpf_NE.norm() > 2.0f * _params.mag_acc_gate;
if (_control_status.flags.gps) {
// Check if there has been enough change in horizontal velocity to make yaw observable
// Apply hysteresis to check to avoid rapid toggling
if (_yaw_angle_observable) {
_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate;
_yaw_angle_observable = _yaw_angle_observable
&& (_control_status.flags.gps || _control_status.flags.ev_pos); // Do we have to add ev_vel here?
} else {
_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate * 2.f;
}
} else {
_yaw_angle_observable = false;
}
}
void Ekf::checkMagBiasObservability()
@@ -366,9 +379,7 @@ bool Ekf::shouldInhibitMag() const
// has explicitly stopped magnetometer use.
const bool user_selected = (_params.mag_fusion_type == MagFuseType::INDOOR);
const bool heading_not_required_for_navigation = !_control_status.flags.gps
&& !_control_status.flags.ev_pos
&& !_control_status.flags.ev_vel;
const bool heading_not_required_for_navigation = !_control_status.flags.gps;
return (user_selected && heading_not_required_for_navigation) || _control_status.flags.mag_field_disturbed;
}