diff --git a/msg/EstimatorStatus.msg b/msg/EstimatorStatus.msg index f3b76f18ed..5491734b1d 100644 --- a/msg/EstimatorStatus.msg +++ b/msg/EstimatorStatus.msg @@ -127,3 +127,8 @@ uint32 mag_device_id # legacy local position estimator (LPE) flags uint8 health_flags # Bitmask to indicate sensor health states (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 diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 52a1677576..9c358e66cd 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -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 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; } @@ -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_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_inclination{NAN}; + float _mag_strength{NAN}; // this is the current status of the filter control modes filter_control_status_u _control_status{}; diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 47d2bbeaa7..b678189674 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -448,11 +448,11 @@ bool Ekf::checkMagField(const Vector3f &mag_sample) } bool is_check_failing = false; - const float mag_strength = mag_sample.length(); + _mag_strength = mag_sample.length(); if (_params.mag_check & static_cast(MagCheckMask::STRENGTH)) { 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; 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 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(MagCheckMask::INCLINATION)) { if (PX4_ISFINITE(_mag_inclination_gps)) { 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) { _control_status.flags.mag_field_disturbed = true; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index a5c08e5d55..5d3f282d64 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1665,6 +1665,9 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) status.gyro_device_id = _device_id_gyro; 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(); _estimator_status_pub.publish(status); }