ekf2: update flags (in_air, at_rest, etc) on delayed time horizon (#20576)

- in a lot of cases this won't really matter, although you can see it in logs (especially with things like vehicle_at_rest), but it's something we might as well do properly

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>

src/modules/ekf2/EKF/common.h

@@ -254,6 +254,14 @@ struct auxVelSample {
 	Vector2f    velVar{};      ///< estimated error variance of the NE velocity (m/sec)**2
 };
 
+struct systemFlagUpdate {
+	uint64_t time_us{};
+	bool at_rest{false};
+	bool in_air{true};
+	bool is_fixed_wing{false};
+	bool gnd_effect{false};
+};
+
 struct stateSample {
 	Quatf    quat_nominal{};        ///< quaternion defining the rotation from body to earth frame
 	Vector3f vel{};                 ///< NED velocity in earth frame in m/s

src/modules/ekf2/EKF/control.cpp

@@ -48,6 +48,22 @@ void Ekf::controlFusionModes()
 	// Store the status to enable change detection
 	_control_status_prev.value = _control_status.value;
 
+	if (_system_flag_buffer) {
+		systemFlagUpdate system_flags_delayed;
+
+		if (_system_flag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &system_flags_delayed)) {
+
+			set_vehicle_at_rest(system_flags_delayed.at_rest);
+			set_in_air_status(system_flags_delayed.in_air);
+
+			set_is_fixed_wing(system_flags_delayed.is_fixed_wing);
+
+			if (system_flags_delayed.gnd_effect) {
+				set_gnd_effect();
+			}
+		}
+	}
+
 	// monitor the tilt alignment
 	if (!_control_status.flags.tilt_align) {
 		// whilst we are aligning the tilt, monitor the variances

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -410,6 +410,42 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
 	}
 }
 
+void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
+{
+	if (!_initialised) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	if (_system_flag_buffer == nullptr) {
+		_system_flag_buffer = new RingBuffer<systemFlagUpdate>(_obs_buffer_length);
+
+		if (_system_flag_buffer == nullptr || !_system_flag_buffer->valid()) {
+			delete _system_flag_buffer;
+			_system_flag_buffer = nullptr;
+			printBufferAllocationFailed("system flag");
+			return;
+		}
+	}
+
+	_system_flag_buffer->push(system_flags);
+
+	const int64_t time_us = system_flags.time_us
+				- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
+
+	// limit data rate to prevent data being lost
+	if (time_us >= static_cast<int64_t>(_system_flag_buffer->get_newest().time_us + _min_obs_interval_us)) {
+
+		systemFlagUpdate system_flags_new{system_flags};
+		system_flags_new.time_us = time_us;
+
+		_system_flag_buffer->push(system_flags_new);
+
+	} else {
+		ECL_WARN("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us);
+	}
+}
+
 void EstimatorInterface::setDragData(const imuSample &imu)
 {
 	// down-sample the drag specific force data by accumulating and calculating the mean when

src/modules/ekf2/EKF/estimator_interface.h

@@ -101,6 +101,8 @@ public:
 
 	void setAuxVelData(const auxVelSample &auxvel_sample);
 
+	void setSystemFlagData(const systemFlagUpdate &system_flags);
+
 	// return a address to the parameters struct
 	// in order to give access to the application
 	parameters *getParamHandle() { return &_params; }
@@ -355,9 +357,10 @@ protected:
 	uint64_t _time_last_in_air{0};		///< last time we were in air (uSec)
 
 	// data buffer instances
-	RingBuffer<imuSample> _imu_buffer{12};           // buffer length 12 with default parameters
-	RingBuffer<outputSample> _output_buffer{12};
-	RingBuffer<outputVert> _output_vert_buffer{12};
+	static constexpr uint8_t kBufferLengthDefault = 12;
+	RingBuffer<imuSample> _imu_buffer{kBufferLengthDefault};
+	RingBuffer<outputSample> _output_buffer{kBufferLengthDefault};
+	RingBuffer<outputVert> _output_vert_buffer{kBufferLengthDefault};
 
 	RingBuffer<gpsSample> *_gps_buffer{nullptr};
 	RingBuffer<magSample> *_mag_buffer{nullptr};
@@ -368,6 +371,7 @@ protected:
 	RingBuffer<extVisionSample> *_ext_vision_buffer{nullptr};
 	RingBuffer<dragSample> *_drag_buffer{nullptr};
 	RingBuffer<auxVelSample> *_auxvel_buffer{nullptr};
+	RingBuffer<systemFlagUpdate> *_system_flag_buffer{nullptr};
 
 	uint64_t _time_last_gps_buffer_push{0};
 	uint64_t _time_last_gps_yaw_buffer_push{0};