Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 11:59:17 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

ekf2: move checkMagFieldStrength() to magFieldStrengthDisturbed() const method

Browse Source
This commit is contained in:
Daniel Agar
2022-03-21 14:27:27 -04:00
parent a7f573e150
commit 9efadad06a
2 changed files with 8 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+1 -1
View File
@@ -863,7 +863,7 @@ private:
void checkMagDeclRequired(); void checkMagDeclRequired();
void checkMagInhibition(); void checkMagInhibition();
bool shouldInhibitMag() const; bool shouldInhibitMag() const;
void checkMagFieldStrength(const Vector3f &mag); bool magFieldStrengthDisturbed(const Vector3f &mag) const;
static bool isMeasuredMatchingExpected(float measured, float expected, float gate); static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
void runMagAndMagDeclFusions(const Vector3f &mag); void runMagAndMagDeclFusions(const Vector3f &mag);
void run3DMagAndDeclFusions(const Vector3f &mag); void run3DMagAndDeclFusions(const Vector3f &mag);
src/modules/ekf2/EKF/mag_control.cpp
+7 -10
View File
@@ -64,11 +64,9 @@ void Ekf::controlMagFusion()
} else { } else {
_control_status.flags.synthetic_mag_z = false; _control_status.flags.synthetic_mag_z = false;
} }
}
}
if (mag_data_ready) { _control_status.flags.mag_field_disturbed = magFieldStrengthDisturbed(mag_sample.mag);
checkMagFieldStrength(mag_sample.mag); }
} }
// If we are on ground, reset the flight alignment flag so that the mag fields will be // If we are on ground, reset the flight alignment flag so that the mag fields will be
@@ -302,24 +300,23 @@ bool Ekf::shouldInhibitMag() const
return (user_selected && heading_not_required_for_navigation) || _control_status.flags.mag_field_disturbed; return (user_selected && heading_not_required_for_navigation) || _control_status.flags.mag_field_disturbed;
} }
void Ekf::checkMagFieldStrength(const Vector3f &mag_sample) bool Ekf::magFieldStrengthDisturbed(const Vector3f &mag_sample) const
{ {
if (_params.check_mag_strength if (_params.check_mag_strength
&& ((_params.mag_fusion_type <= MagFuseType::MAG_3D) || (_params.mag_fusion_type == MagFuseType::INDOOR && _control_status.flags.gps))) { && ((_params.mag_fusion_type <= MagFuseType::MAG_3D) || (_params.mag_fusion_type == MagFuseType::INDOOR && _control_status.flags.gps))) {
if (PX4_ISFINITE(_mag_strength_gps)) { if (PX4_ISFINITE(_mag_strength_gps)) {
constexpr float wmm_gate_size = 0.2f; // +/- Gauss constexpr float wmm_gate_size = 0.2f; // +/- Gauss
_control_status.flags.mag_field_disturbed = !isMeasuredMatchingExpected(mag_sample.length(), _mag_strength_gps, wmm_gate_size); return !isMeasuredMatchingExpected(mag_sample.length(), _mag_strength_gps, wmm_gate_size);
} else { } else {
constexpr float average_earth_mag_field_strength = 0.45f; // Gauss constexpr float average_earth_mag_field_strength = 0.45f; // Gauss
constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss
_control_status.flags.mag_field_disturbed = !isMeasuredMatchingExpected(mag_sample.length(), average_earth_mag_field_strength, average_earth_mag_gate_size); return !isMeasuredMatchingExpected(mag_sample.length(), average_earth_mag_field_strength, average_earth_mag_gate_size);
} }
} else {
_control_status.flags.mag_field_disturbed = false;
} }
return false;
} }
bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected, const float gate) bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected, const float gate)