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

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

View File

@ -863,7 +863,7 @@ private:
void checkMagDeclRequired();
void checkMagInhibition();
bool shouldInhibitMag() const;
void checkMagFieldStrength(const Vector3f &mag);
bool magFieldStrengthDisturbed(const Vector3f &mag) const;
static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
void runMagAndMagDeclFusions(const Vector3f &mag);
void run3DMagAndDeclFusions(const Vector3f &mag);

View File

@ -64,11 +64,9 @@ void Ekf::controlMagFusion()
} else {
_control_status.flags.synthetic_mag_z = false;
}
}
}
if (mag_data_ready) {
checkMagFieldStrength(mag_sample.mag);
_control_status.flags.mag_field_disturbed = magFieldStrengthDisturbed(mag_sample.mag);
}
}
// 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;
}
void Ekf::checkMagFieldStrength(const Vector3f &mag_sample)
bool Ekf::magFieldStrengthDisturbed(const Vector3f &mag_sample) const
{
if (_params.check_mag_strength
&& ((_params.mag_fusion_type <= MagFuseType::MAG_3D) || (_params.mag_fusion_type == MagFuseType::INDOOR && _control_status.flags.gps))) {
if (PX4_ISFINITE(_mag_strength_gps)) {
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 {
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.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)