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

ekf2: merge runOnGroundYawReset() + runInAirYawReset() into unified magReset()

Browse Source
This commit is contained in:
Daniel Agar
2023-03-09 09:08:27 -05:00
parent c1f244a6fd
commit b3cc945a5a
2 changed files with 58 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+2 -3
View File
@@ -890,11 +890,10 @@ private:
// control fusion of magnetometer observations // control fusion of magnetometer observations
void controlMagFusion(); void controlMagFusion();
void checkHaglYawResetReq();
float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; } float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
void runOnGroundYawReset(); bool magReset();
void runInAirYawReset(); bool haglYawResetReq();
void selectMagAuto(); void selectMagAuto();
void check3DMagFusionSuitability(); void check3DMagFusionSuitability();
src/modules/ekf2/EKF/mag_control.cpp
+56 -62
View File
@@ -206,18 +206,18 @@ void Ekf::controlMagFusion()
break; break;
} }
const bool mag_enabled = _control_status.flags.mag_hdg || _control_status.flags.mag_3D; if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
if ((!mag_enabled_previously && mag_enabled) || mag_sample.reset) { if (_mag_yaw_reset_req || !_control_status.flags.yaw_align || mag_sample.reset || !mag_enabled_previously || haglYawResetReq()) {
_mag_yaw_reset_req = true;
}
if (_control_status.flags.in_air) { if (magReset()) {
checkHaglYawResetReq(); _mag_yaw_reset_req = false;
runInAirYawReset();
} else { } else {
runOnGroundYawReset(); // mag reset failed, try again next time
_mag_yaw_reset_req = true;
}
}
} }
if (!_control_status.flags.yaw_align) { if (!_control_status.flags.yaw_align) {
@@ -231,90 +231,84 @@ void Ekf::controlMagFusion()
} }
} }
void Ekf::checkHaglYawResetReq() bool Ekf::haglYawResetReq()
{ {
// We need to reset the yaw angle after climbing away from the ground to enable // We need to reset the yaw angle after climbing away from the ground to enable
// recovery from ground level magnetic interference. // recovery from ground level magnetic interference.
if (!_control_status.flags.mag_aligned_in_flight) { if (_control_status.flags.in_air && _control_status.flags.yaw_align && !_control_status.flags.mag_aligned_in_flight) {
// Check if height has increased sufficiently to be away from ground magnetic anomalies // Check if height has increased sufficiently to be away from ground magnetic anomalies
// and request a yaw reset if not already requested. // and request a yaw reset if not already requested.
static constexpr float mag_anomalies_max_hagl = 1.5f; static constexpr float mag_anomalies_max_hagl = 1.5f;
const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl; const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
_mag_yaw_reset_req = _mag_yaw_reset_req || above_mag_anomalies; return above_mag_anomalies;
} }
return false;
} }
void Ekf::runOnGroundYawReset() bool Ekf::magReset()
{
if (_mag_yaw_reset_req) {
const bool has_realigned_yaw = resetMagHeading();
if (has_realigned_yaw) {
_mag_yaw_reset_req = false;
_control_status.flags.yaw_align = true;
}
}
}
void Ekf::runInAirYawReset()
{ {
// prevent a reset being performed more than once on the same frame // prevent a reset being performed more than once on the same frame
if ((_flt_mag_align_start_time == _time_delayed_us) if ((_flt_mag_align_start_time == _time_delayed_us)
|| (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) { || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
return; return false;
} }
if (_mag_yaw_reset_req) { bool has_realigned_yaw = false;
bool has_realigned_yaw = false;
// use yaw estimator if available // use yaw estimator if available
if (_control_status.flags.gps && isYawEmergencyEstimateAvailable() if (_control_status.flags.gps && isYawEmergencyEstimateAvailable()
&& (_mag_counter > 1) // mag LPF available && (_mag_counter > 1) // mag LPF available
) { ) {
resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar()); resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
_information_events.flags.yaw_aligned_to_imu_gps = true; _information_events.flags.yaw_aligned_to_imu_gps = true;
// if world magnetic model (inclination, declination, strength) available then use it to reset mag states // if world magnetic model (inclination, declination, strength) available then use it to reset mag states
if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) { if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {
// use predicted earth field to reset states // use predicted earth field to reset states
const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0); const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
_state.mag_I = mag_earth_pred; _state.mag_I = mag_earth_pred;
const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal); const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
_state.mag_B = _mag_lpf.getState() - (R_to_body * mag_earth_pred); _state.mag_B = _mag_lpf.getState() - (R_to_body * mag_earth_pred);
} else { } else {
// Use the last magnetometer measurements to reset the field states // Use the last magnetometer measurements to reset the field states
// calculate initial earth magnetic field states // calculate initial earth magnetic field states
_state.mag_I = _R_to_earth * _mag_lpf.getState(); _state.mag_I = _R_to_earth * _mag_lpf.getState();
_state.mag_B.zero(); _state.mag_B.zero();
}
ECL_DEBUG("resetting mag I: [%.3f, %.3f, %.3f], B: [%.3f, %.3f, %.3f]",
(double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
(double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2)
);
resetMagCov();
has_realigned_yaw = true;
} }
if (!has_realigned_yaw) { ECL_DEBUG("resetting mag I: [%.3f, %.3f, %.3f], B: [%.3f, %.3f, %.3f]",
has_realigned_yaw = resetMagHeading(); (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
} (double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2)
);
if (has_realigned_yaw) { resetMagCov();
_mag_yaw_reset_req = false;
_control_status.flags.yaw_align = true; has_realigned_yaw = true;
}
if (!has_realigned_yaw) {
has_realigned_yaw = resetMagHeading();
}
if (has_realigned_yaw) {
_control_status.flags.yaw_align = true;
if (_control_status.flags.in_air) {
_control_status.flags.mag_aligned_in_flight = true; _control_status.flags.mag_aligned_in_flight = true;
// record the time for the magnetic field alignment event // record the time for the magnetic field alignment event
_flt_mag_align_start_time = _time_delayed_us; _flt_mag_align_start_time = _time_delayed_us;
} }
return true;
} }
return false;
} }
void Ekf::selectMagAuto() void Ekf::selectMagAuto()