ekf2: log mag inclination and strength for tuning

SEESAI · Jul 24, 2023 · 72be724 · 72be724
1 parent 357bf02
commit 72be724
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 4 deletions.
diff --git 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
@@ -227,6 +227,14 @@ class EstimatorInterface
 		}
 	}
 
+	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 @@ class EstimatorInterface
 	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
@@ -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<int32_t>(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<int32_t>(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
@@ -1665,6 +1665,9 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
 	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);
 }