Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 21:55:34 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

ekf2-mag: rework mag strength check

Browse Source
This commit is contained in:
bresch
2023-07-14 17:28:19 +02:00
committed by Daniel Agar
parent e878d0c0ef
commit e4a16bfc80
7 changed files with 80 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/common.h
+8 -1
View File
@@ -155,6 +155,12 @@ enum class EvCtrl : uint8_t {
YAW = (1<<3)
};
enum class MagCheckMask : uint8_t {
STRENGTH = (1 << 0),
INCLINATION = (1 << 1),
FORCE_WMM = (1 << 2)
};
struct gpsMessage {
uint64_t time_usec{};
int32_t lat{}; ///< Latitude in 1E-7 degrees
@@ -486,7 +492,8 @@ struct parameters {
// compute synthetic magnetomter Z value if possible
int32_t synthesize_mag_z{0};
int32_t check_mag_strength{0};
int32_t mag_check{0};
float mag_check_strength_tolerance_gs{0.2f};
// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
float EKFGSF_tas_default{15.0f}; ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
src/modules/ekf2/EKF/ekf.h
+3 -1
View File
@@ -585,6 +585,7 @@ private:
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
uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time
SquareMatrix24f P{}; ///< state covariance matrix
@@ -707,6 +708,7 @@ private:
// Variables used to control activation of post takeoff functionality
uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec)
uint64_t _time_last_mov_3d_mag_suitable{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
uint64_t _time_last_mag_check_failing{0};
Vector3f _saved_mag_bf_variance {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
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)
@@ -1035,7 +1037,7 @@ private:
void checkMagDeclRequired();
bool shouldInhibitMag() const;
bool magFieldStrengthDisturbed(const Vector3f &mag) const;
bool checkMagField(const Vector3f &mag);
static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
void runMagAndMagDeclFusions(const Vector3f &mag);
void run3DMagAndDeclFusions(const Vector3f &mag);
src/modules/ekf2/EKF/mag_control.cpp
+38 -18
View File
@@ -42,6 +42,7 @@
void Ekf::controlMagFusion()
{
bool mag_data_ready = false;
bool mag_data_valid = false;
magSample mag_sample;
@@ -85,8 +86,7 @@ void Ekf::controlMagFusion()
_control_status.flags.synthetic_mag_z = false;
}
_control_status.flags.mag_field_disturbed = magFieldStrengthDisturbed(mag_sample.mag);
mag_data_valid = checkMagField(mag_sample.mag);
// compute mag heading innovation (for estimator_aid_src_mag_heading logging)
const Vector3f mag_observation = mag_sample.mag - _state.mag_B;
@@ -120,7 +120,7 @@ void Ekf::controlMagFusion()
_control_status.flags.mag_aligned_in_flight = false;
}
if (mag_data_ready && !_control_status.flags.tilt_align && !_control_status.flags.yaw_align) {
if (mag_data_ready && mag_data_valid && !_control_status.flags.tilt_align && !_control_status.flags.yaw_align) {
// calculate the initial magnetic field and yaw alignment
// but do not mark the yaw alignement complete as it needs to be
// reset once the leveling phase is done
@@ -169,7 +169,7 @@ void Ekf::controlMagFusion()
if (mag_data_ready && !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw) {
if (shouldInhibitMag()) {
if (shouldInhibitMag() || !mag_data_valid) {
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();
@@ -432,23 +432,50 @@ bool Ekf::shouldInhibitMag() const
return (user_selected && heading_not_required_for_navigation) || _control_status.flags.mag_field_disturbed;
}
bool Ekf::magFieldStrengthDisturbed(const Vector3f &mag_sample) const
bool Ekf::checkMagField(const Vector3f &mag_sample)
{
if (_params.check_mag_strength
&& ((_params.mag_fusion_type <= MagFuseType::MAG_3D) || (_params.mag_fusion_type == MagFuseType::INDOOR && _control_status.flags.gps))) {
_control_status.flags.mag_field_disturbed = false;
if ((_params.mag_fusion_type == MagFuseType::NONE)
|| (_params.mag_fusion_type == MagFuseType::INDOOR && !_control_status.flags.gps)) {
//TODO: review this: gps flag cannnot be set if yaw isn't aligned
return false;
}
if (_params.mag_check == 0) {
// skip all checks
return true;
}
bool is_check_failing = false;
const float mag_strength = mag_sample.length();
if (_params.mag_check & static_cast<int32_t>(MagCheckMask::STRENGTH)) {
if (PX4_ISFINITE(_mag_strength_gps)) {
constexpr float wmm_gate_size = 0.2f; // +/- Gauss
return !isMeasuredMatchingExpected(mag_sample.length(), _mag_strength_gps, wmm_gate_size);
if (!isMeasuredMatchingExpected(mag_strength, _mag_strength_gps, _params.mag_check_strength_tolerance_gs)) {
_control_status.flags.mag_field_disturbed = true;
is_check_failing = true;
}
} else if (_params.mag_check & static_cast<int32_t>(MagCheckMask::FORCE_WMM)) {
is_check_failing = true;
} else {
constexpr float average_earth_mag_field_strength = 0.45f; // Gauss
constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss
return !isMeasuredMatchingExpected(mag_sample.length(), average_earth_mag_field_strength, average_earth_mag_gate_size);
if (!isMeasuredMatchingExpected(mag_sample.length(), average_earth_mag_field_strength, average_earth_mag_gate_size)) {
_control_status.flags.mag_field_disturbed = true;
is_check_failing = true;
}
}
}
return false;
if (is_check_failing || (_time_last_mag_check_failing == 0)) {
_time_last_mag_check_failing = _time_delayed_us;
}
return ((_time_delayed_us - _time_last_mag_check_failing) > (uint64_t)_min_mag_health_time_us);
}
bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected, const float gate)
@@ -527,13 +554,6 @@ bool Ekf::resetMagHeading(const Vector3f &mag)
const Vector3f mag_init = _mag_lpf.getState() - mag_bias;
const bool mag_available = !magFieldStrengthDisturbed(mag_init);
// low pass filtered mag required
if (!mag_available) {
return false;
}
const bool heading_required_for_navigation = _control_status.flags.gps;
if ((_params.mag_fusion_type <= MagFuseType::MAG_3D) || ((_params.mag_fusion_type == MagFuseType::INDOOR) && heading_required_for_navigation)) {
src/modules/ekf2/EKF2.cpp
+2 -1
View File
@@ -189,7 +189,8 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_pcoef_yn(_params->static_pressure_coef_yn),
_param_ekf2_pcoef_z(_params->static_pressure_coef_z),
#endif // CONFIG_EKF2_BARO_COMPENSATION
_param_ekf2_mag_check(_params->check_mag_strength),
_param_ekf2_mag_check(_params->mag_check),
_param_ekf2_mag_chk_str(_params->mag_check_strength_tolerance_gs),
_param_ekf2_synthetic_mag_z(_params->synthesize_mag_z),
_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default)
{
src/modules/ekf2/EKF2.hpp
+1
View File
@@ -737,6 +737,7 @@ private:
(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h, ///< Required GPS health time
(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check, ///< Mag field strength check
(ParamExtFloat<px4::params::EKF2_MAG_CHK_STR>) _param_ekf2_mag_chk_str,
(ParamExtInt<px4::params::EKF2_SYNT_MAG_Z>)
_param_ekf2_synthetic_mag_z, ///< Enables the use of a synthetic value for the Z axis of the magnetometer calculated from the 3D magnetic field vector at the location of the drone.
src/modules/ekf2/ekf2_params.c
+26 -3
View File
@@ -1502,17 +1502,40 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_GPS_H, 10.0f);
/**
* Magnetic field strength test selection
*
* When set, the EKF checks the strength of the magnetic field
* to decide whether the magnetometer data is valid.
* If GPS data is received, the magnetic field is compared to a World
* Bitmask to set which check is used to decide whether the magnetometer data is valid.
*
* If GNSS data is received, the magnetic field is compared to a World
* Magnetic Model (WMM), otherwise an average value is used.
* This check is useful to reject occasional hard iron disturbance.
*
* Set bits to 1 to enable checks. Checks enabled by the following bit positions
* 0 : Magnetic field strength. Set tolerance using EKF2_MAG_CHK_STR
* 1 : Reserved
* 2 : Wait for GNSS to find the theoretical strength and inclination using the WMM
*
* @group EKF2
* @boolean
* @min 0
* @max 7
* @bit 0 Strength (EKF2_MAG_CHK_STR)
* @bit 1 Reserved
* @bit 2 Wait for WMM
*/
PARAM_DEFINE_INT32(EKF2_MAG_CHECK, 1);
/**
* Magnetic field strength check tolerance
*
* Maximum allowed deviation from the expected magnetic field strength to pass the check.
*
* @group EKF2
* @min 0.0
* @max 1.0
* @unit gauss
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_MAG_CHK_STR, 0.2f);
/**
* Enable synthetic magnetometer Z component measurement.
*
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
+2 -1
View File
@@ -212,7 +212,8 @@ void EkfWrapper::setMagFuseTypeNone()
void EkfWrapper::enableMagStrengthCheck()
{
_ekf_params->check_mag_strength = 1;
_ekf_params->mag_check |= static_cast<int32_t>(MagCheckMask::STRENGTH);
}
}
bool EkfWrapper::isWindVelocityEstimated() const