ekf: add vehicle at rest to flags

EKF/common.h

@@ -457,6 +457,7 @@ union filter_control_status_u {
 		uint32_t mag_aligned_in_flight   : 1; ///< 23 - true when the in-flight mag field alignment has been completed
 		uint32_t ev_vel      : 1; ///< 24 - true when local frame velocity data fusion from external vision measurements is intended
 		uint32_t synthetic_mag_z : 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component
+		uint32_t vehicle_at_rest : 1; ///< 26 - true when the vehicle is at rest
 	} flags;
 	uint32_t value;
 };

EKF/ekf_helper.cpp

@@ -1127,7 +1127,7 @@ void Ekf::get_imu_vibe_metrics(float vibe[3])
 bool Ekf::get_gps_drift_metrics(float drift[3], bool *blocked)
 {
 	memcpy(drift, _gps_drift_metrics, 3 * sizeof(float));
-	*blocked = !_vehicle_at_rest;
+	*blocked = !_control_status.flags.vehicle_at_rest;
 	if (_gps_drift_updated) {
 		_gps_drift_updated = false;
 		return true;

EKF/estimator_interface.cpp

@@ -66,7 +66,7 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 
 	// Do not change order of computeVibrationMetric and checkIfVehicleAtRest
 	computeVibrationMetric();
-	_vehicle_at_rest = checkIfVehicleAtRest(dt);
+	_control_status.flags.vehicle_at_rest = checkIfVehicleAtRest(dt);
 
 	const bool new_downsampled_imu_sample_ready = _imu_down_sampler.update(_newest_high_rate_imu_sample);
 	_imu_updated = new_downsampled_imu_sample_ready;

EKF/estimator_interface.h

@@ -364,7 +364,7 @@ public:
 		*val = _fault_status.value;
 	}
 
-	bool isVehicleAtRest() const { return _vehicle_at_rest; }
+	bool isVehicleAtRest() const { return _control_status.flags.vehicle_at_rest; }
 
 	// get GPS check status
 	virtual void get_gps_check_status(uint16_t *val) = 0;
@@ -521,7 +521,6 @@ protected:
 					// [0] Horizontal position drift rate (m/s)
 					// [1] Vertical position drift rate (m/s)
 					// [2] Filtered horizontal velocity (m/s)
-	bool _vehicle_at_rest{false};	// true when the vehicle is at rest
 	uint64_t _time_last_move_detect_us{0};	// timestamp of last movement detection event in microseconds
 	bool _gps_drift_updated{false};	// true when _gps_drift_metrics has been updated and is ready for retrieval
 

EKF/gps_checks.cpp

@@ -143,7 +143,7 @@ bool Ekf::gps_is_good(const gps_message &gps)
 	// The following checks are only valid when the vehicle is at rest
 	const double lat = gps.lat * 1.0e-7;
 	const double lon = gps.lon * 1.0e-7;
-	if (!_control_status.flags.in_air && _vehicle_at_rest) {
+	if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) {
 		// Calculate position movement since last measurement
 		float delta_pos_n = 0.0f;
 		float delta_pos_e = 0.0f;

EKF/mag_fusion.cpp

@@ -716,7 +716,7 @@ void Ekf::fuseHeading()
 	if (_mag_use_inhibit) {
 		// The magnetometer cannot be trusted but we need to fuse a heading to prevent a badly
 		// conditioned covariance matrix developing over time.
-		if (!_vehicle_at_rest) {
+		if (!_control_status.flags.vehicle_at_rest) {
 			// Vehicle is not at rest so fuse a zero innovation and record the
 			// predicted heading to use as an observation when movement ceases.
 			_heading_innov = 0.0f;