dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: log mag inclination and strength for tuning

This commit is contained in:
bresch 2023-04-18 21:35:03 +02:00 committed by Daniel Agar
parent 357bf024f6
commit 72be724b86
4 changed files with 22 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
msg/EstimatorStatus.msg
View File

@ -127,3 +127,8 @@ uint32 mag_device_id
# legacy local position estimator (LPE) flags # legacy local position estimator (LPE) flags
uint8 health_flags # Bitmask to indicate sensor health states (vel, pos, hgt) uint8 health_flags # Bitmask to indicate sensor health states (vel, pos, hgt)
uint8 timeout_flags # Bitmask to indicate timeout flags (vel, pos, hgt) uint8 timeout_flags # Bitmask to indicate timeout flags (vel, pos, hgt)
float32 mag_inclination_deg
float32 mag_inclination_ref_deg
float32 mag_strength_gs
float32 mag_strength_ref_gs

10
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -227,6 +227,14 @@ public:
} }
} }
void get_mag_checks(float &inc_deg, float &inc_ref_deg, float &strength_gs, float &strength_ref_gs) const
{
inc_deg = math::degrees(_mag_inclination);
inc_ref_deg = math::degrees(_mag_inclination_gps);
strength_gs = _mag_strength;
strength_ref_gs = _mag_strength_gps;
}
// get EKF mode status // get EKF mode status
const filter_control_status_u &control_status() const { return _control_status; } const filter_control_status_u &control_status() const { return _control_status; }
const decltype(filter_control_status_u::flags) &control_status_flags() const { return _control_status.flags; } const decltype(filter_control_status_u::flags) &control_status_flags() const { return _control_status.flags; }
@ -405,6 +413,8 @@ protected:
float _mag_declination_gps{NAN}; // magnetic declination returned by the geo library using the last valid GPS position (rad) float _mag_declination_gps{NAN}; // magnetic declination returned by the geo library using the last valid GPS position (rad)
float _mag_inclination_gps{NAN}; // magnetic inclination returned by the geo library using the last valid GPS position (rad) float _mag_inclination_gps{NAN}; // magnetic inclination returned by the geo library using the last valid GPS position (rad)
float _mag_strength_gps{NAN}; // magnetic strength returned by the geo library using the last valid GPS position (T) float _mag_strength_gps{NAN}; // magnetic strength returned by the geo library using the last valid GPS position (T)
float _mag_inclination{NAN};
float _mag_strength{NAN};
// this is the current status of the filter control modes // this is the current status of the filter control modes
filter_control_status_u _control_status{}; filter_control_status_u _control_status{};

8
src/modules/ekf2/EKF/mag_control.cpp
View File

@ -448,11 +448,11 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
} }
bool is_check_failing = false; bool is_check_failing = false;
const float mag_strength = mag_sample.length(); _mag_strength = mag_sample.length();
if (_params.mag_check & static_cast<int32_t>(MagCheckMask::STRENGTH)) { if (_params.mag_check & static_cast<int32_t>(MagCheckMask::STRENGTH)) {
if (PX4_ISFINITE(_mag_strength_gps)) { if (PX4_ISFINITE(_mag_strength_gps)) {
if (!isMeasuredMatchingExpected(mag_strength, _mag_strength_gps, _params.mag_check_strength_tolerance_gs)) { if (!isMeasuredMatchingExpected(_mag_strength, _mag_strength_gps, _params.mag_check_strength_tolerance_gs)) {
_control_status.flags.mag_field_disturbed = true; _control_status.flags.mag_field_disturbed = true;
is_check_failing = true; is_check_failing = true;
} }
@ -472,12 +472,12 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
} }
const Vector3f mag_earth = _R_to_earth * mag_sample; const Vector3f mag_earth = _R_to_earth * mag_sample;
const float mag_inclination = asin(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f)); _mag_inclination = asin(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f));
if (_params.mag_check & static_cast<int32_t>(MagCheckMask::INCLINATION)) { if (_params.mag_check & static_cast<int32_t>(MagCheckMask::INCLINATION)) {
if (PX4_ISFINITE(_mag_inclination_gps)) { if (PX4_ISFINITE(_mag_inclination_gps)) {
const float inc_tol_rad = radians(_params.mag_check_inclination_tolerance_deg); const float inc_tol_rad = radians(_params.mag_check_inclination_tolerance_deg);
const float inc_error_rad = wrap_pi(mag_inclination - _mag_inclination_gps); const float inc_error_rad = wrap_pi(_mag_inclination - _mag_inclination_gps);
if (fabsf(inc_error_rad) > inc_tol_rad) { if (fabsf(inc_error_rad) > inc_tol_rad) {
_control_status.flags.mag_field_disturbed = true; _control_status.flags.mag_field_disturbed = true;

3
src/modules/ekf2/EKF2.cpp
View File

@ -1665,6 +1665,9 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
status.gyro_device_id = _device_id_gyro; status.gyro_device_id = _device_id_gyro;
status.mag_device_id = _device_id_mag; status.mag_device_id = _device_id_mag;
_ekf.get_mag_checks(status.mag_inclination_deg, status.mag_inclination_ref_deg, status.mag_strength_gs,
status.mag_strength_ref_gs);
status.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); status.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_status_pub.publish(status); _estimator_status_pub.publish(status);
} }