fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-10 05:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: replace mag reset request flags with timeout checks

Browse Source
This commit is contained in:
Daniel Agar
2022-04-04 10:42:56 -04:00
parent ca893e55a5
commit 0546f4082b
6 changed files with 45 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+1 -2
View File
@@ -1048,13 +1048,12 @@ void Ekf::controlBetaFusion()
void Ekf::controlDragFusion()
{
if ((_params.fusion_mode & MASK_USE_DRAG) && _drag_buffer &&
!_using_synthetic_position && _control_status.flags.in_air && !_mag_inhibit_yaw_reset_req) {
!_using_synthetic_position && _control_status.flags.in_air) {
if (!_control_status.flags.wind) {
// reset the wind states and covariances when starting drag accel fusion
_control_status.flags.wind = true;
resetWind();
}
src/modules/ekf2/EKF/ekf.h
+2 -4
View File
@@ -422,11 +422,8 @@ private:
bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable
uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation manoeuvre was detected
uint8_t _num_bad_flight_yaw_events{0}; ///< number of times a bad heading has been detected in flight and required a yaw reset
uint64_t _mag_use_not_inhibit_us{0}; ///< last system time in usec before magnetometer use was inhibited
float _last_static_yaw{NAN}; ///< last yaw angle recorded when on ground motion checks were passing (rad)
bool _mag_inhibit_yaw_reset_req{false}; ///< true when magnetometer inhibit has been active for long enough to require a yaw reset when conditions improve.
bool _mag_yaw_reset_req{false}; ///< true when a reset of the yaw using the magnetometer data has been requested
bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
bool _synthetic_mag_z_active{false}; ///< true if we are generating synthetic magnetometer Z measurements
bool _is_yaw_fusion_inhibited{false}; ///< true when yaw sensor use is being inhibited
@@ -537,6 +534,8 @@ private:
Matrix2f _saved_mag_ef_ne_covmat{}; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
float _saved_mag_ef_d_variance{}; ///< D magnetic field state variance saved for use at the next initialisation (Gauss**2)
float _mag_heading_last_declination{NAN};
gps_check_fail_status_u _gps_check_fail_status{};
// variables used to inhibit accel bias learning
@@ -859,7 +858,6 @@ private:
bool canUse3DMagFusion() const;
void checkMagDeclRequired();
void checkMagInhibition();
bool shouldInhibitMag() const;
bool magFieldStrengthDisturbed(const Vector3f &mag) const;
static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
src/modules/ekf2/EKF/ekf_helper.cpp
+5 -10
View File
@@ -481,6 +481,7 @@ bool Ekf::resetMagHeading()
// record the time for the magnetic field alignment event
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
_time_last_mag_heading_fuse = _time_last_imu;
return true;
}
@@ -1556,18 +1557,11 @@ void Ekf::startGpsFusion()
yaw_reset_needed = true;
}
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
if (_mag_inhibit_yaw_reset_req || _mag_yaw_reset_req) {
yaw_reset_needed = true;
}
}
if (isYawEmergencyEstimateAvailable() && isYawError(math::radians(10.f))) {
yaw_reset_needed = true;
}
// Do not use external vision for yaw if using GPS because yaw needs to be
// defined relative to an NED reference frame
// yaw needs to be defined relative to an NED reference frame
if (yaw_reset_needed) {
if (resetYawToEKFGSF()) {
ECL_INFO("starting GPS, reset yaw using yaw estimator");
@@ -1583,8 +1577,7 @@ void Ekf::startGpsFusion()
} else {
// all failed
ECL_WARN("starting GPS, yaw reset failed");
_mag_yaw_reset_req = true;
ECL_ERR("starting GPS, yaw reset failed");
}
}
@@ -1769,6 +1762,8 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
// capture the reset event
_state_reset_status.quat_counter++;
_time_last_heading_fuse = _time_last_imu;
}
// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
src/modules/ekf2/EKF/gps_checks.cpp
-17
View File
@@ -84,19 +84,11 @@ bool Ekf::collect_gps(const gps_message &gps)
_earth_rate_NED = calcEarthRateNED((float)math::radians(_pos_ref.getProjectionReferenceLat()));
_last_gps_origin_time_us = _time_last_imu;
const bool declination_was_valid = PX4_ISFINITE(_mag_declination_gps);
// set the magnetic field data returned by the geo library using the current GPS position
_mag_declination_gps = get_mag_declination_radians(lat, lon);
_mag_inclination_gps = get_mag_inclination_radians(lat, lon);
_mag_strength_gps = get_mag_strength_gauss(lat, lon);
// request a reset of the yaw using the new declination
if ((_params.mag_fusion_type != MAG_FUSE_TYPE_NONE)
&& !declination_was_valid) {
_mag_yaw_reset_req = true;
}
// save the horizontal and vertical position uncertainty of the origin
_gps_origin_eph = gps.eph;
_gps_origin_epv = gps.epv;
@@ -108,8 +100,6 @@ bool Ekf::collect_gps(const gps_message &gps)
// a rough 2D fix is still sufficient to lookup declination
if ((gps.fix_type >= 2) && (gps.eph < 1000)) {
const bool declination_was_valid = PX4_ISFINITE(_mag_declination_gps);
// If we have good GPS data set the origin's WGS-84 position to the last gps fix
const double lat = gps.lat * 1.0e-7;
const double lon = gps.lon * 1.0e-7;
@@ -119,13 +109,6 @@ bool Ekf::collect_gps(const gps_message &gps)
_mag_inclination_gps = get_mag_inclination_radians(lat, lon);
_mag_strength_gps = get_mag_strength_gauss(lat, lon);
// request mag yaw reset if there's a mag declination for the first time
if (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE) {
if (!declination_was_valid && PX4_ISFINITE(_mag_declination_gps)) {
_mag_yaw_reset_req = true;
}
}
_earth_rate_NED = calcEarthRateNED((float)math::radians(lat));
}
}
src/modules/ekf2/EKF/gps_control.cpp
+2
View File
@@ -97,10 +97,12 @@ void Ekf::controlGpsFusion()
// and cause another navigation failure
_control_status.flags.mag_fault = true;
_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
stopMagFusion();
} else if (_control_status.flags.gps_yaw) {
_control_status.flags.gps_yaw_fault = true;
_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;
stopGpsYawFusion();
} else if (_control_status.flags.ev_yaw) {
stopEvYawFusion();
src/modules/ekf2/EKF/mag_control.cpp
+35 -43
View File
@@ -126,21 +126,31 @@ void Ekf::controlMagFusion()
const bool mag_enabled = _control_status.flags.mag_hdg || _control_status.flags.mag_3D;
const bool declination_changed = _control_status.flags.mag_hdg && (fabsf(_mag_heading_last_declination - getMagDeclination()) > math::radians(1.f));
bool mag_fuse_recent = !isTimedOut(_time_last_mag_heading_fuse, (uint64_t)5e6) || !isTimedOut(_time_last_mag_3d_fuse, (uint64_t)5e6);
if (!_control_status.flags.yaw_align
|| _mag_yaw_reset_req || _mag_inhibit_yaw_reset_req
|| !mag_fuse_recent
|| declination_changed
|| haglYawResetReq()
|| (!mag_enabled_previously && mag_enabled)) {
runYawReset();
}
if (_control_status.flags.mag_hdg) {
_mag_heading_last_declination = getMagDeclination();
}
if (!_control_status.flags.yaw_align) {
// Having the yaw aligned is mandatory to continue
return;
}
checkMagDeclRequired();
checkMagInhibition();
_is_yaw_fusion_inhibited = shouldInhibitMag();
runMagAndMagDeclFusions(mag_sample.mag);
}
@@ -165,34 +175,34 @@ bool Ekf::haglYawResetReq() const
void Ekf::runYawReset()
{
if (!_is_yaw_fusion_inhibited) {
bool has_realigned_yaw = false;
bool has_realigned_yaw = false;
if (_control_status.flags.gps && _control_status.flags.fixed_wing) {
has_realigned_yaw = realignYawGPS();
if (_control_status.flags.gps && _control_status.flags.fixed_wing) {
has_realigned_yaw = realignYawGPS();
}
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;
}
if (!has_realigned_yaw) {
has_realigned_yaw = resetMagHeading();
}
if (has_realigned_yaw) {
_mag_yaw_reset_req = false;
_control_status.flags.yaw_align = true;
if (_control_status.flags.in_air) {
_control_status.flags.mag_aligned_in_flight = true;
}
// 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) {
_mag_inhibit_yaw_reset_req = false;
// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * _dt_ekf_avg));
}
// 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.
bool mag_fuse_recent = !isTimedOut(_time_last_mag_heading_fuse, (uint64_t)5e6) || !isTimedOut(_time_last_mag_3d_fuse, (uint64_t)5e6);
if (!mag_fuse_recent) {
// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * _dt_ekf_avg));
}
// reset
_time_last_mag_heading_fuse = _time_last_imu;
}
}
@@ -261,20 +271,6 @@ void Ekf::checkMagDeclRequired()
_control_status.flags.mag_dec = (_control_status.flags.mag_3D && (not_using_ne_aiding || user_selected));
}
void Ekf::checkMagInhibition()
{
_is_yaw_fusion_inhibited = shouldInhibitMag();
if (!_is_yaw_fusion_inhibited) {
_mag_use_not_inhibit_us = _imu_sample_delayed.time_us;
}
// If magnetometer use has been inhibited continuously then a yaw reset is required for a valid heading
if (uint32_t(_imu_sample_delayed.time_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) {
_mag_inhibit_yaw_reset_req = true;
}
}
bool Ekf::shouldInhibitMag() const
{
// If the user has selected auto protection against indoor magnetic field errors, only use the magnetometer
@@ -405,10 +401,6 @@ bool Ekf::resetMagStates()
if (reset) {
resetMagCov();
// clear any pending resets
_mag_yaw_reset_req = false;
_mag_inhibit_yaw_reset_req = false;
if (mag_available) {
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _imu_sample_delayed.time_us;