estimator_status split out estimator_states

src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp

@@ -232,6 +232,10 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 		if (!ekf2Check(mavlink_log_pub, status, false, reportFailures && report_ekf_fail, checkGNSS)) {
 			failed = true;
 		}
+
+		if (!ekf2CheckStates(mavlink_log_pub, reportFailures && report_ekf_fail)) {
+			failed = true;
+		}
 	}
 
 	/* ---- Failure Detector ---- */

src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp

@@ -110,6 +110,9 @@ private:
 			       const bool prearm);
 	static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
 			      const bool report_fail, const bool enforce_gps_required);
+
+	static bool ekf2CheckStates(orb_advert_t *mavlink_log_pub, const bool report_fail);
+
 	static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
 					 const bool prearm);
 

src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp

@@ -38,6 +38,7 @@
 #include <lib/parameters/param.h>
 #include <systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
+#include <uORB/topics/estimator_states.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/subsystem_info.h>
 
@@ -145,40 +146,6 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 		goto out;
 	}
 
-	// check accelerometer delta velocity bias estimates
-	param_get(param_find("COM_ARM_EKF_AB"), &test_limit);
-
-	for (uint8_t index = 13; index < 16; index++) {
-		// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
-		// adjust test threshold by 3-sigma
-		float test_uncertainty = 3.0f * sqrtf(fmaxf(status.covariances[index], 0.0f));
-
-		if (fabsf(status.states[index]) > test_limit + test_uncertainty) {
-
-			if (report_fail) {
-				PX4_ERR("state %d: |%.8f| > %.8f + %.8f", index, (double)status.states[index], (double)test_limit,
-					(double)test_uncertainty);
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias");
-			}
-
-			success = false;
-			goto out;
-		}
-	}
-
-	// check gyro delta angle bias estimates
-	param_get(param_find("COM_ARM_EKF_GB"), &test_limit);
-
-	if (fabsf(status.states[10]) > test_limit || fabsf(status.states[11]) > test_limit
-	    || fabsf(status.states[12]) > test_limit) {
-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias");
-		}
-
-		success = false;
-		goto out;
-	}
-
 	// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
 	if (enforce_gps_required || report_fail) {
 		const bool ekf_gps_fusion = status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
@@ -259,3 +226,50 @@ out:
 
 	return success;
 }
+
+bool PreFlightCheck::ekf2CheckStates(orb_advert_t *mavlink_log_pub, const bool report_fail)
+{
+	// Get estimator states data if available and exit with a fail recorded if not
+	uORB::Subscription states_sub{ORB_ID(estimator_states)};
+	estimator_states_s states;
+
+	if (states_sub.copy(&states)) {
+
+		// check accelerometer delta velocity bias estimates
+		float test_limit = 1.0f; // pass limit re-used for each test
+		param_get(param_find("COM_ARM_EKF_AB"), &test_limit);
+
+		for (uint8_t index = 13; index < 16; index++) {
+			// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
+			// adjust test threshold by 3-sigma
+			float test_uncertainty = 3.0f * sqrtf(fmaxf(states.covariances[index], 0.0f));
+
+			if (fabsf(states.states[index]) > test_limit + test_uncertainty) {
+
+				if (report_fail) {
+					PX4_ERR("state %d: |%.8f| > %.8f + %.8f", index, (double)states.states[index], (double)test_limit,
+						(double)test_uncertainty);
+					mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias");
+				}
+
+				return false;
+			}
+		}
+
+		// check gyro delta angle bias estimates
+		param_get(param_find("COM_ARM_EKF_GB"), &test_limit);
+
+		if (fabsf(states.states[10]) > test_limit
+		    || fabsf(states.states[11]) > test_limit
+		    || fabsf(states.states[12]) > test_limit) {
+
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias");
+			}
+
+			return false;
+		}
+	}
+
+	return true;
+}

src/modules/ekf2/EKF2.cpp

@@ -989,6 +989,41 @@ void EKF2::Run()
 				}
 			}
 
+			// publish estimator states
+			estimator_states_s states;
+			states.n_states = 24;
+			_ekf.getStateAtFusionHorizonAsVector().copyTo(states.states);
+			_ekf.covariances_diagonal().copyTo(states.covariances);
+			states.timestamp = _replay_mode ? now : hrt_absolute_time();
+			_estimator_states_pub.publish(states);
+
+			// publish estimator status
+			estimator_status_s status{};
+			_ekf.getOutputTrackingError().copyTo(status.output_tracking_error);
+			_ekf.get_gps_check_status(&status.gps_check_fail_flags);
+			// only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include
+			// the GPS Fix bit, which is always checked)
+			status.gps_check_fail_flags &= ((uint16_t)_params->gps_check_mask << 1) | 1;
+			status.control_mode_flags = control_status.value;
+			_ekf.get_filter_fault_status(&status.filter_fault_flags);
+			_ekf.get_innovation_test_status(status.innovation_check_flags, status.mag_test_ratio,
+							status.vel_test_ratio, status.pos_test_ratio,
+							status.hgt_test_ratio, status.tas_test_ratio,
+							status.hagl_test_ratio, status.beta_test_ratio);
+
+			status.pos_horiz_accuracy = _vehicle_local_position_pub.get().eph;
+			status.pos_vert_accuracy = _vehicle_local_position_pub.get().epv;
+			_ekf.get_ekf_soln_status(&status.solution_status_flags);
+			_ekf.getImuVibrationMetrics().copyTo(status.vibe);
+			status.time_slip = _last_time_slip_us * 1e-6f;
+			status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed();
+			status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed();
+			status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed();
+			status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed();
+			status.pre_flt_fail_mag_field_disturbed = control_status.flags.mag_field_disturbed;
+
+			_estimator_status_pub.publish(status);
+
 			{
 				// publish all corrected sensor readings and bias estimates after mag calibration is updated above
 				bias.timestamp = now;
@@ -1012,39 +1047,6 @@ void EKF2::Run()
 				_estimator_sensor_bias_pub.publish(bias);
 			}
 
-			// publish estimator status
-			estimator_status_s status;
-			status.timestamp = now;
-			_ekf.getStateAtFusionHorizonAsVector().copyTo(status.states);
-			status.n_states = 24;
-			_ekf.covariances_diagonal().copyTo(status.covariances);
-			_ekf.getOutputTrackingError().copyTo(status.output_tracking_error);
-			_ekf.get_gps_check_status(&status.gps_check_fail_flags);
-			// only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include
-			// the GPS Fix bit, which is always checked)
-			status.gps_check_fail_flags &= ((uint16_t)_params->gps_check_mask << 1) | 1;
-			status.control_mode_flags = control_status.value;
-			_ekf.get_filter_fault_status(&status.filter_fault_flags);
-			_ekf.get_innovation_test_status(status.innovation_check_flags, status.mag_test_ratio,
-							status.vel_test_ratio, status.pos_test_ratio,
-							status.hgt_test_ratio, status.tas_test_ratio,
-							status.hagl_test_ratio, status.beta_test_ratio);
-
-			status.pos_horiz_accuracy = _vehicle_local_position_pub.get().eph;
-			status.pos_vert_accuracy = _vehicle_local_position_pub.get().epv;
-			_ekf.get_ekf_soln_status(&status.solution_status_flags);
-			_ekf.getImuVibrationMetrics().copyTo(status.vibe);
-			status.time_slip = _last_time_slip_us * 1e-6f;
-			status.health_flags = 0.0f; // unused
-			status.timeout_flags = 0.0f; // unused
-			status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed();
-			status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed();
-			status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed();
-			status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed();
-			status.pre_flt_fail_mag_field_disturbed = control_status.flags.mag_field_disturbed;
-
-			_estimator_status_pub.publish(status);
-
 			// publish GPS drift data only when updated to minimise overhead
 			float gps_drift[3];
 			bool blocked;
@@ -1103,8 +1105,8 @@ void EKF2::Run()
 					bool all_estimates_invalid = false;
 
 					for (uint8_t axis_index = 0; axis_index <= 2; axis_index++) {
-						if (status.covariances[axis_index + 19] < min_var_allowed
-						    || status.covariances[axis_index + 19] > max_var_allowed) {
+						if (states.covariances[axis_index + 19] < min_var_allowed
+						    || states.covariances[axis_index + 19] > max_var_allowed) {
 							all_estimates_invalid = true;
 						}
 					}
@@ -1112,9 +1114,9 @@ void EKF2::Run()
 					// Store valid estimates and their associated variances
 					if (!all_estimates_invalid) {
 						for (uint8_t axis_index = 0; axis_index <= 2; axis_index++) {
-							_last_valid_mag_cal[axis_index] = status.states[axis_index + 19];
+							_last_valid_mag_cal[axis_index] = states.states[axis_index + 19];
 							_valid_cal_available[axis_index] = true;
-							_last_valid_variance[axis_index] = status.covariances[axis_index + 19];
+							_last_valid_variance[axis_index] = states.covariances[axis_index + 19];
 						}
 					}
 				}

src/modules/ekf2/EKF2.hpp

@@ -63,6 +63,7 @@
 #include <uORB/topics/ekf_gps_position.h>
 #include <uORB/topics/estimator_innovations.h>
 #include <uORB/topics/estimator_sensor_bias.h>
+#include <uORB/topics/estimator_states.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/landing_target_pose.h>
 #include <uORB/topics/optical_flow.h>
@@ -271,6 +272,7 @@ private:
 	uORB::Publication<estimator_innovations_s>		_estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
 	uORB::Publication<estimator_innovations_s>		_estimator_innovations_pub{ORB_ID(estimator_innovations)};
 	uORB::Publication<estimator_sensor_bias_s>		_estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
+	uORB::Publication<estimator_states_s>			_estimator_states_pub{ORB_ID(estimator_states)};
 	uORB::Publication<estimator_status_s>			_estimator_status_pub{ORB_ID(estimator_status)};
 	uORB::Publication<vehicle_attitude_s>			_att_pub{ORB_ID(vehicle_attitude)};
 	uORB::Publication<vehicle_odometry_s>			_vehicle_odometry_pub{ORB_ID(vehicle_odometry)};

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -712,45 +712,46 @@ void BlockLocalPositionEstimator::publishOdom()
 
 void BlockLocalPositionEstimator::publishEstimatorStatus()
 {
+	_pub_est_states.get().timestamp = _timeStamp;
 	_pub_est_status.get().timestamp = _timeStamp;
 
 	for (size_t i = 0; i < n_x; i++) {
-		_pub_est_status.get().states[i] = _x(i);
+		_pub_est_states.get().states[i] = _x(i);
 	}
 
 	// matching EKF2 covariances indexing
 	// quaternion - not determined, as it is a position estimator
-	_pub_est_status.get().covariances[0] = NAN;
-	_pub_est_status.get().covariances[1] = NAN;
-	_pub_est_status.get().covariances[2] = NAN;
-	_pub_est_status.get().covariances[3] = NAN;
+	_pub_est_states.get().covariances[0] = NAN;
+	_pub_est_states.get().covariances[1] = NAN;
+	_pub_est_states.get().covariances[2] = NAN;
+	_pub_est_states.get().covariances[3] = NAN;
 	// linear velocity
-	_pub_est_status.get().covariances[4] = m_P(X_vx, X_vx);
-	_pub_est_status.get().covariances[5] = m_P(X_vy, X_vy);
-	_pub_est_status.get().covariances[6] = m_P(X_vz, X_vz);
+	_pub_est_states.get().covariances[4] = m_P(X_vx, X_vx);
+	_pub_est_states.get().covariances[5] = m_P(X_vy, X_vy);
+	_pub_est_states.get().covariances[6] = m_P(X_vz, X_vz);
 	// position
-	_pub_est_status.get().covariances[7] = m_P(X_x, X_x);
-	_pub_est_status.get().covariances[8] = m_P(X_y, X_y);
-	_pub_est_status.get().covariances[9] = m_P(X_z, X_z);
+	_pub_est_states.get().covariances[7] = m_P(X_x, X_x);
+	_pub_est_states.get().covariances[8] = m_P(X_y, X_y);
+	_pub_est_states.get().covariances[9] = m_P(X_z, X_z);
 	// gyro bias - not determined
-	_pub_est_status.get().covariances[10] = NAN;
-	_pub_est_status.get().covariances[11] = NAN;
-	_pub_est_status.get().covariances[12] = NAN;
+	_pub_est_states.get().covariances[10] = NAN;
+	_pub_est_states.get().covariances[11] = NAN;
+	_pub_est_states.get().covariances[12] = NAN;
 	// accel bias
-	_pub_est_status.get().covariances[13] = m_P(X_bx, X_bx);
-	_pub_est_status.get().covariances[14] = m_P(X_by, X_by);
-	_pub_est_status.get().covariances[15] = m_P(X_bz, X_bz);
+	_pub_est_states.get().covariances[13] = m_P(X_bx, X_bx);
+	_pub_est_states.get().covariances[14] = m_P(X_by, X_by);
+	_pub_est_states.get().covariances[15] = m_P(X_bz, X_bz);
 
 	// mag - not determined
 	for (size_t i = 16; i <= 21; i++) {
-		_pub_est_status.get().covariances[i] = NAN;
+		_pub_est_states.get().covariances[i] = NAN;
 	}
 
 	// replacing the hor wind cov with terrain altitude covariance
-	_pub_est_status.get().covariances[22] = m_P(X_tz, X_tz);
-	_pub_est_status.get().covariances[23] = NAN;
+	_pub_est_states.get().covariances[22] = m_P(X_tz, X_tz);
+	_pub_est_states.get().covariances[23] = NAN;
 
-	_pub_est_status.get().n_states = n_x;
+	_pub_est_states.get().n_states = n_x;
 	_pub_est_status.get().health_flags = _sensorFault;
 	_pub_est_status.get().timeout_flags = _sensorTimeout;
 	_pub_est_status.get().pos_horiz_accuracy = _pub_gpos.get().eph;

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -33,6 +33,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/estimator_states.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/estimator_innovations.h>
 
@@ -285,6 +286,7 @@ private:
 	uORB::PublicationData<vehicle_local_position_s> _pub_lpos{ORB_ID(vehicle_local_position)};
 	uORB::PublicationData<vehicle_global_position_s> _pub_gpos{ORB_ID(vehicle_global_position)};
 	uORB::PublicationData<vehicle_odometry_s> _pub_odom{ORB_ID(vehicle_odometry)};
+	uORB::PublicationData<estimator_states_s> _pub_est_states{ORB_ID(estimator_states)};
 	uORB::PublicationData<estimator_status_s> _pub_est_status{ORB_ID(estimator_status)};
 	uORB::PublicationData<estimator_innovations_s> _pub_innov{ORB_ID(estimator_innovations)};
 	uORB::PublicationData<estimator_innovations_s> _pub_innov_var{ORB_ID(estimator_innovation_variances)};

src/modules/logger/logged_topics.cpp

@@ -61,6 +61,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("estimator_innovation_variances", 200);
 	add_topic("estimator_innovations", 200);
 	add_topic("estimator_sensor_bias", 1000);
+	add_topic("estimator_states", 1000);
 	add_topic("estimator_status", 200);
 	add_topic("home_position");
 	add_topic("hover_thrust_estimate", 100);