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

ekf2: remove realignYawGPS() (replaced with yaw estimator)

Browse Source
This commit is contained in:
Daniel Agar
2022-10-21 09:01:30 -04:00
parent c267cf71c3
commit ed558e199f
4 changed files with 39 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_control.cpp
+37 -5
View File
@@ -94,7 +94,6 @@ void Ekf::controlMagFusion()
// re-initialised next time we achieve flight altitude
if (!_control_status.flags.in_air) {
_control_status.flags.mag_aligned_in_flight = false;
_num_bad_flight_yaw_events = 0;
}
if (_params.mag_fusion_type >= MagFuseType::NONE
@@ -141,7 +140,7 @@ void Ekf::controlMagFusion()
if (_control_status.flags.in_air) {
checkHaglYawResetReq();
runInAirYawReset(mag_sample.mag);
runInAirYawReset();
} else {
runOnGroundYawReset();
@@ -197,13 +196,43 @@ bool Ekf::canResetMagHeading() const
return !_control_status.flags.mag_field_disturbed && (_params.mag_fusion_type != MagFuseType::NONE);
}
void Ekf::runInAirYawReset(const Vector3f &mag_sample)
void Ekf::runInAirYawReset()
{
if (_mag_yaw_reset_req && !_is_yaw_fusion_inhibited) {
bool has_realigned_yaw = false;
if (_control_status.flags.gps && _control_status.flags.fixed_wing) {
has_realigned_yaw = realignYawGPS(mag_sample);
// use yaw estimator if available
if (_control_status.flags.gps && isYawEmergencyEstimateAvailable() &&
(_mag_counter != 0) && isNewestSampleRecent(_time_last_mag_buffer_push, 500'000) // mag LPF available
) {
resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
_information_events.flags.yaw_aligned_to_imu_gps = true;
_ekfgsf_yaw_reset_time = _imu_sample_delayed.time_us;
// 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)) {
// 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);
_state.mag_I = mag_earth_pred;
const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
_state.mag_B = _mag_lpf.getState() - (R_to_body * mag_earth_pred);
} else {
// Use the last magnetometer measurements to reset the field states
// calculate initial earth magnetic field states
_state.mag_I = _R_to_earth * _mag_lpf.getState();
_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();
}
if (!has_realigned_yaw && canResetMagHeading()) {
@@ -215,6 +244,9 @@ void Ekf::runInAirYawReset(const Vector3f &mag_sample)
_control_status.flags.yaw_align = true;
_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;
// Handle the special case where we have not been constraining yaw drift or learning yaw bias due
// to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor.
if (_mag_inhibit_yaw_reset_req) {