Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 10:26:52 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

ekf2: mag field strength check respect requirements

Browse Source 
- requires valid mag data (_mag_data_ready)
 - respect mag_fusion_type (including MAG_FUSE_TYPE_INDOOR)
This commit is contained in:
Daniel Agar
2022-01-25 14:24:22 -05:00
parent 0ddf76ed7f
commit f499749c23
2 changed files with 16 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
-2
View File
@@ -852,8 +852,6 @@ private:
bool shouldInhibitMag() const; bool shouldInhibitMag() const;
void checkMagFieldStrength(); void checkMagFieldStrength();
bool isStrongMagneticDisturbance() const { return _control_status.flags.mag_field_disturbed; } bool isStrongMagneticDisturbance() const { return _control_status.flags.mag_field_disturbed; }
bool isMeasuredMatchingGpsMagStrength() const;
bool isMeasuredMatchingAverageMagStrength() const;
static bool isMeasuredMatchingExpected(float measured, float expected, float gate); static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
void runMagAndMagDeclFusions(); void runMagAndMagDeclFusions();
void run3DMagAndDeclFusions(); void run3DMagAndDeclFusions();
src/modules/ekf2/EKF/mag_control.cpp
+16 -21
View File
@@ -302,31 +302,26 @@ bool Ekf::shouldInhibitMag() const
void Ekf::checkMagFieldStrength() void Ekf::checkMagFieldStrength()
{ {
if (_params.check_mag_strength) { if (_mag_data_ready) {
_control_status.flags.mag_field_disturbed = _NED_origin_initialised if (_params.check_mag_strength
? !isMeasuredMatchingGpsMagStrength() && ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) || (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _control_status.flags.gps))) {
: !isMeasuredMatchingAverageMagStrength();
} else { if (PX4_ISFINITE(_mag_strength_gps)) {
_control_status.flags.mag_field_disturbed = false; constexpr float wmm_gate_size = 0.2f; // +/- Gauss
_control_status.flags.mag_field_disturbed = !isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(), _mag_strength_gps, wmm_gate_size);
} else {
constexpr float average_earth_mag_field_strength = 0.45f; // Gauss
constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss
_control_status.flags.mag_field_disturbed = !isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(), average_earth_mag_field_strength, average_earth_mag_gate_size);
}
} else {
_control_status.flags.mag_field_disturbed = false;
}
} }
} }
bool Ekf::isMeasuredMatchingGpsMagStrength() const
{
constexpr float wmm_gate_size = 0.2f; // +/- Gauss
return isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(), _mag_strength_gps, wmm_gate_size);
}
bool Ekf::isMeasuredMatchingAverageMagStrength() const
{
constexpr float average_earth_mag_field_strength = 0.45f; // Gauss
constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss
return isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(),
average_earth_mag_field_strength,
average_earth_mag_gate_size);
}
bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected, const float gate) bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected, const float gate)
{ {
return (measured >= expected - gate) return (measured >= expected - gate)