ekf: merge backend classes EstimatorInterface + Ekf

msg/EstimatorAidSource2d.msg

@@ -19,3 +19,4 @@ bool fused                   # true if the sample was successfully fused
 
 # TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos
 # TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow
+# TOPICS estimator_aid_src_drag

msg/EstimatorInnovations.msg

@@ -19,7 +19,6 @@ float32 baro_vpos	# barometer height innovation (m) and innovation variance (m**
 
 # Auxiliary velocity
 float32[2] aux_hvel	# horizontal auxiliary velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2)
-float32    aux_vvel	# vertical auxiliary velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2)
 
 # Optical flow
 float32[2] flow		# flow innvoation (rad/sec) and innovation variance ((rad/sec)**2)

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -49,6 +49,41 @@
 
 #include <mathlib/mathlib.h>
 
+void Ekf::setAirspeedData(const airspeedSample &airspeed_sample)
+{
+	if (!_initialised) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	if (_airspeed_buffer == nullptr) {
+		_airspeed_buffer = new RingBuffer<airspeedSample>(_obs_buffer_length);
+
+		if (_airspeed_buffer == nullptr || !_airspeed_buffer->valid()) {
+			delete _airspeed_buffer;
+			_airspeed_buffer = nullptr;
+			printBufferAllocationFailed("airspeed");
+			return;
+		}
+	}
+
+	const int64_t time_us = airspeed_sample.time_us
+				- static_cast<int64_t>(_params.airspeed_delay_ms * 1000)
+				- 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>(_airspeed_buffer->get_newest().time_us + _min_obs_interval_us)) {
+
+		airspeedSample airspeed_sample_new{airspeed_sample};
+		airspeed_sample_new.time_us = time_us;
+
+		_airspeed_buffer->push(airspeed_sample_new);
+
+	} else {
+		ECL_WARN("airspeed data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _airspeed_buffer->get_newest().time_us, _min_obs_interval_us);
+	}
+}
+
 void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 {
 	// control activation and initialisation/reset of wind states required for airspeed fusion

src/modules/ekf2/EKF/auxvel_fusion.cpp

@@ -33,6 +33,41 @@
 
 #include "ekf.h"
 
+void Ekf::setAuxVelData(const auxVelSample &auxvel_sample)
+{
+	if (!_initialised) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	if (_auxvel_buffer == nullptr) {
+		_auxvel_buffer = new RingBuffer<auxVelSample>(_obs_buffer_length);
+
+		if (_auxvel_buffer == nullptr || !_auxvel_buffer->valid()) {
+			delete _auxvel_buffer;
+			_auxvel_buffer = nullptr;
+			printBufferAllocationFailed("aux vel");
+			return;
+		}
+	}
+
+	const int64_t time_us = auxvel_sample.time_us
+				- static_cast<int64_t>(_params.auxvel_delay_ms * 1000)
+				- 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>(_auxvel_buffer->get_newest().time_us + _min_obs_interval_us)) {
+
+		auxVelSample auxvel_sample_new{auxvel_sample};
+		auxvel_sample_new.time_us = time_us;
+
+		_auxvel_buffer->push(auxvel_sample_new);
+
+	} else {
+		ECL_WARN("aux velocity data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _auxvel_buffer->get_newest().time_us, _min_obs_interval_us);
+	}
+}
+
 void Ekf::controlAuxVelFusion()
 {
 	if (_auxvel_buffer) {

src/modules/ekf2/EKF/baro_height_control.cpp

@@ -38,6 +38,42 @@
 
 #include "ekf.h"
 
+void Ekf::setBaroData(const baroSample &baro_sample)
+{
+	if (!_initialised) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	if (_baro_buffer == nullptr) {
+		_baro_buffer = new RingBuffer<baroSample>(_obs_buffer_length);
+
+		if (_baro_buffer == nullptr || !_baro_buffer->valid()) {
+			delete _baro_buffer;
+			_baro_buffer = nullptr;
+			printBufferAllocationFailed("baro");
+			return;
+		}
+	}
+
+	const int64_t time_us = baro_sample.time_us
+				- static_cast<int64_t>(_params.baro_delay_ms * 1000)
+				- 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>(_baro_buffer->get_newest().time_us + _min_obs_interval_us)) {
+
+		baroSample baro_sample_new{baro_sample};
+		baro_sample_new.time_us = time_us;
+
+		_baro_buffer->push(baro_sample_new);
+		_time_last_baro_buffer_push = _time_latest_us;
+
+	} else {
+		ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _baro_buffer->get_newest().time_us, _min_obs_interval_us);
+	}
+}
+
 void Ekf::controlBaroHeightFusion()
 {
 	static constexpr const char *HGT_SRC_NAME = "baro";

src/modules/ekf2/EKF/control.cpp

@@ -122,7 +122,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 #endif // CONFIG_EKF2_SIDESLIP
 
 #if defined(CONFIG_EKF2_DRAG_FUSION)
-	controlDragFusion();
+	controlDragFusion(imu_delayed);
 #endif // CONFIG_EKF2_DRAG_FUSION
 
 	controlHeightFusion(imu_delayed);

src/modules/ekf2/EKF/drag_fusion.cpp

@@ -42,12 +42,73 @@
 
 #include <mathlib/mathlib.h>
 
-void Ekf::controlDragFusion()
+void Ekf::setDragData(const imuSample &imu)
 {
-	if ((_params.drag_ctrl > 0) && _drag_buffer &&
-	    !_control_status.flags.fake_pos && _control_status.flags.in_air) {
+	// down-sample the drag specific force data by accumulating and calculating the mean when
+	// sufficient samples have been collected
+	if (_params.drag_ctrl > 0) {
 
-		if (!_control_status.flags.wind) {
+		// Allocate the required buffer size if not previously done
+		if (_drag_buffer == nullptr) {
+			_drag_buffer = new RingBuffer<dragSample>(_obs_buffer_length);
+
+			if (_drag_buffer == nullptr || !_drag_buffer->valid()) {
+				delete _drag_buffer;
+				_drag_buffer = nullptr;
+				printBufferAllocationFailed("drag");
+				return;
+			}
+		}
+
+		// don't use any accel samples that are clipping
+		if (imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]) {
+			// reset accumulators
+			_drag_sample_count = 0;
+			_drag_down_sampled.accelXY.zero();
+			_drag_down_sampled.time_us = 0;
+			_drag_sample_time_dt = 0.0f;
+
+			return;
+		}
+
+		_drag_sample_count++;
+		// note acceleration is accumulated as a delta velocity
+		_drag_down_sampled.accelXY(0) += imu.delta_vel(0);
+		_drag_down_sampled.accelXY(1) += imu.delta_vel(1);
+		_drag_down_sampled.time_us += imu.time_us;
+		_drag_sample_time_dt += imu.delta_vel_dt;
+
+		// calculate the downsample ratio for drag specific force data
+		uint8_t min_sample_ratio = (uint8_t) ceilf((float)_imu_buffer_length / _obs_buffer_length);
+
+		if (min_sample_ratio < 5) {
+			min_sample_ratio = 5;
+		}
+
+		// calculate and store means from accumulated values
+		if (_drag_sample_count >= min_sample_ratio) {
+			// note conversion from accumulated delta velocity to acceleration
+			_drag_down_sampled.accelXY(0) /= _drag_sample_time_dt;
+			_drag_down_sampled.accelXY(1) /= _drag_sample_time_dt;
+			_drag_down_sampled.time_us /= _drag_sample_count;
+
+			// write to buffer
+			_drag_buffer->push(_drag_down_sampled);
+
+			// reset accumulators
+			_drag_sample_count = 0;
+			_drag_down_sampled.accelXY.zero();
+			_drag_down_sampled.time_us = 0;
+			_drag_sample_time_dt = 0.0f;
+		}
+	}
+}
+
+void Ekf::controlDragFusion(const imuSample &imu_delayed)
+{
+	if ((_params.drag_ctrl > 0) && _drag_buffer) {
+
+		if (!_control_status.flags.wind && !_control_status.flags.fake_pos && _control_status.flags.in_air) {
 			// reset the wind states and covariances when starting drag accel fusion
 			_control_status.flags.wind = true;
 			resetWindToZero();
@@ -55,7 +116,7 @@ void Ekf::controlDragFusion()
 
 		dragSample drag_sample;
 
-		if (_drag_buffer->pop_first_older_than(_time_delayed_us, &drag_sample)) {
+		if (_drag_buffer->pop_first_older_than(imu_delayed.time_us, &drag_sample)) {
 			fuseDrag(drag_sample);
 		}
 	}
@@ -87,14 +148,14 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 	const float rel_wind_speed = rel_wind_body.norm();
 	const VectorState state_vector_prev = getStateAtFusionHorizonAsVector();
 
-	Vector2f bcoef_inv;
+	Vector2f bcoef_inv{0.f, 0.f};
 
 	if (using_bcoef_x) {
-		bcoef_inv(0) = 1.0f / _params.bcoef_x;
+		bcoef_inv(0) = 1.f / _params.bcoef_x;
 	}
 
 	if (using_bcoef_y) {
-		bcoef_inv(1) = 1.0f / _params.bcoef_y;
+		bcoef_inv(1) = 1.f / _params.bcoef_y;
 	}
 
 	if (using_bcoef_x && using_bcoef_y) {
@@ -105,6 +166,11 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 		bcoef_inv(1) = bcoef_inv(0);
 	}
 
+	_aid_src_drag.timestamp_sample = drag_sample.time_us;
+	_aid_src_drag.fused = false;
+
+	bool fused[] {false, false};
+
 	VectorState Kfusion;
 
 	// perform sequential fusion of XY specific forces
@@ -115,36 +181,51 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 		// Drag is modelled as an arbitrary combination of bluff body drag that proportional to
 		// equivalent airspeed squared, and rotor momentum drag that is proportional to true airspeed
 		// parallel to the rotor disc and mass flow through the rotor disc.
+		const float pred_acc = -0.5f * bcoef_inv(axis_index) * rho * rel_wind_body(axis_index) * rel_wind_speed - rel_wind_body(axis_index) * mcoef_corrrected;
+
+		_aid_src_drag.observation[axis_index] = mea_acc;
+		_aid_src_drag.observation_variance[axis_index] = R_ACC;
+		_aid_src_drag.innovation[axis_index] = pred_acc - mea_acc;
+		_aid_src_drag.innovation_variance[axis_index] = NAN; // reset
 
 		if (axis_index == 0) {
+			sym::ComputeDragXInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON,
+						      &_aid_src_drag.innovation_variance[axis_index], &Kfusion);
+
 			if (!using_bcoef_x && !using_mcoef) {
 				continue;
 			}
 
-			sym::ComputeDragXInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, &_drag_innov_var(axis_index), &Kfusion);
-
 		} else if (axis_index == 1) {
+			sym::ComputeDragYInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON,
+						      &_aid_src_drag.innovation_variance[axis_index], &Kfusion);
+
 			if (!using_bcoef_y && !using_mcoef) {
 				continue;
 			}
-
-			sym::ComputeDragYInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, &_drag_innov_var(axis_index), &Kfusion);
 		}
 
-		if (_drag_innov_var(axis_index) < R_ACC) {
+		if (_aid_src_drag.innovation_variance[axis_index] < R_ACC) {
 			// calculation is badly conditioned
 			return;
 		}
 
-		const float pred_acc = -0.5f * bcoef_inv(axis_index) * rho * rel_wind_body(axis_index) * rel_wind_speed - rel_wind_body(axis_index) * mcoef_corrrected;
-
 		// Apply an innovation consistency check with a 5 Sigma threshold
-		_drag_innov(axis_index) = pred_acc - mea_acc;
-		_drag_test_ratio(axis_index) = sq(_drag_innov(axis_index)) / (sq(5.0f) * _drag_innov_var(axis_index));
+		const float innov_gate = 5.f;
+		setEstimatorAidStatusTestRatio(_aid_src_drag, innov_gate);
 
-		// if the innovation consistency check fails then don't fuse the sample
-		if (_drag_test_ratio(axis_index) <= 1.0f) {
-			measurementUpdate(Kfusion, _drag_innov_var(axis_index), _drag_innov(axis_index));
+		if (_control_status.flags.in_air && _control_status.flags.wind && !_control_status.flags.fake_pos
+		    && PX4_ISFINITE(_aid_src_drag.innovation_variance[axis_index]) && PX4_ISFINITE(_aid_src_drag.innovation[axis_index])
+		    && (_aid_src_drag.test_ratio[axis_index] < 1.f)
+		   ) {
+			if (measurementUpdate(Kfusion, _aid_src_drag.innovation_variance[axis_index], _aid_src_drag.innovation[axis_index])) {
+				fused[axis_index] = true;
 			}
 		}
 	}
+
+	if (fused[0] && fused[1]) {
+		_aid_src_drag.fused = true;
+		_aid_src_drag.time_last_fuse = _time_delayed_us;
+	}
+}

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -291,84 +291,6 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const
 			-CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad));
 }
 
-void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2],  float &vpos) const
-{
-	hvel[0] = _aid_src_gnss_vel.innovation[0];
-	hvel[1] = _aid_src_gnss_vel.innovation[1];
-	vvel    = _aid_src_gnss_vel.innovation[2];
-
-	hpos[0] = _aid_src_gnss_pos.innovation[0];
-	hpos[1] = _aid_src_gnss_pos.innovation[1];
-	vpos    = _aid_src_gnss_hgt.innovation;
-}
-
-void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos)  const
-{
-	hvel[0] = _aid_src_gnss_vel.innovation_variance[0];
-	hvel[1] = _aid_src_gnss_vel.innovation_variance[1];
-	vvel    = _aid_src_gnss_vel.innovation_variance[2];
-
-	hpos[0] = _aid_src_gnss_pos.innovation_variance[0];
-	hpos[1] = _aid_src_gnss_pos.innovation_variance[1];
-	vpos    = _aid_src_gnss_hgt.innovation_variance;
-}
-
-void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const
-{
-	hvel = fmaxf(_aid_src_gnss_vel.test_ratio[0], _aid_src_gnss_vel.test_ratio[1]);
-	vvel = _aid_src_gnss_vel.test_ratio[2];
-
-	hpos = fmaxf(_aid_src_gnss_pos.test_ratio[0], _aid_src_gnss_pos.test_ratio[1]);
-	vpos = _aid_src_gnss_hgt.test_ratio;
-}
-
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
-{
-	hvel[0] = _aid_src_ev_vel.innovation[0];
-	hvel[1] = _aid_src_ev_vel.innovation[1];
-	vvel    = _aid_src_ev_vel.innovation[2];
-
-	hpos[0] = _aid_src_ev_pos.innovation[0];
-	hpos[1] = _aid_src_ev_pos.innovation[1];
-	vpos    = _aid_src_ev_hgt.innovation;
-}
-
-void Ekf::getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const
-{
-	hvel[0] = _aid_src_ev_vel.innovation_variance[0];
-	hvel[1] = _aid_src_ev_vel.innovation_variance[1];
-	vvel    = _aid_src_ev_vel.innovation_variance[2];
-
-	hpos[0] = _aid_src_ev_pos.innovation_variance[0];
-	hpos[1] = _aid_src_ev_pos.innovation_variance[1];
-	vpos    = _aid_src_ev_hgt.innovation_variance;
-}
-
-void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const
-{
-	hvel = fmaxf(_aid_src_ev_vel.test_ratio[0], _aid_src_ev_vel.test_ratio[1]);
-	vvel = _aid_src_ev_vel.test_ratio[2];
-
-	hpos = fmaxf(_aid_src_ev_pos.test_ratio[0], _aid_src_ev_pos.test_ratio[1]);
-	vpos = _aid_src_ev_hgt.test_ratio;
-}
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-
-#if defined(CONFIG_EKF2_AUXVEL)
-void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const
-{
-	aux_vel_innov[0] = _aid_src_aux_vel.innovation[0];
-	aux_vel_innov[1] = _aid_src_aux_vel.innovation[1];
-}
-
-void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
-{
-	aux_vel_innov_var[0] = _aid_src_aux_vel.innovation_variance[0];
-	aux_vel_innov_var[1] = _aid_src_aux_vel.innovation_variance[1];
-}
-#endif // CONFIG_EKF2_AUXVEL
-
 // get the state vector at the delayed time horizon
 matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
 {

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -40,11 +40,11 @@
  * @author Siddharth B Purohit <siddharthbharatpurohit@gmail.com>
  */
 
-#include "estimator_interface.h"
+#include "ekf.h"
 
 #include <mathlib/mathlib.h>
 
-EstimatorInterface::~EstimatorInterface()
+Ekf::~Ekf()
 {
 	delete _gps_buffer;
 #if defined(CONFIG_EKF2_MAGNETOMETER)
@@ -74,7 +74,7 @@ EstimatorInterface::~EstimatorInterface()
 }
 
 // Accumulate imu data and store to buffer at desired rate
-void EstimatorInterface::setIMUData(const imuSample &imu_sample)
+void Ekf::setIMUData(const imuSample &imu_sample)
 {
 	// TODO: resolve misplaced responsibility
 	if (!_initialised) {
@@ -84,7 +84,9 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 	_time_latest_us = imu_sample.time_us;
 
 	// the output observer always runs
-	_output_predictor.calculateOutputStates(imu_sample.time_us, imu_sample.delta_ang, imu_sample.delta_ang_dt, imu_sample.delta_vel, imu_sample.delta_vel_dt);
+	_output_predictor.calculateOutputStates(imu_sample.time_us,
+						imu_sample.delta_ang, imu_sample.delta_ang_dt,
+						imu_sample.delta_vel, imu_sample.delta_vel_dt);
 
 	// accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available
 	if (_imu_down_sampler.update(imu_sample)) {
@@ -106,350 +108,7 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 #endif // CONFIG_EKF2_DRAG_FUSION
 }
 
-#if defined(CONFIG_EKF2_MAGNETOMETER)
-void EstimatorInterface::setMagData(const magSample &mag_sample)
-{
-	if (!_initialised) {
-		return;
-	}
-
-	// Allocate the required buffer size if not previously done
-	if (_mag_buffer == nullptr) {
-		_mag_buffer = new RingBuffer<magSample>(_obs_buffer_length);
-
-		if (_mag_buffer == nullptr || !_mag_buffer->valid()) {
-			delete _mag_buffer;
-			_mag_buffer = nullptr;
-			printBufferAllocationFailed("mag");
-			return;
-		}
-	}
-
-	const int64_t time_us = mag_sample.time_us
-				- static_cast<int64_t>(_params.mag_delay_ms * 1000)
-				- 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>(_mag_buffer->get_newest().time_us + _min_obs_interval_us)) {
-
-		magSample mag_sample_new{mag_sample};
-		mag_sample_new.time_us = time_us;
-
-		_mag_buffer->push(mag_sample_new);
-		_time_last_mag_buffer_push = _time_latest_us;
-
-	} else {
-		ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, _min_obs_interval_us);
-	}
-}
-#endif // CONFIG_EKF2_MAGNETOMETER
-
-void EstimatorInterface::setGpsData(const gpsMessage &gps)
-{
-	if (!_initialised) {
-		return;
-	}
-
-	// Allocate the required buffer size if not previously done
-	if (_gps_buffer == nullptr) {
-		_gps_buffer = new RingBuffer<gpsSample>(_obs_buffer_length);
-
-		if (_gps_buffer == nullptr || !_gps_buffer->valid()) {
-			delete _gps_buffer;
-			_gps_buffer = nullptr;
-			printBufferAllocationFailed("GPS");
-			return;
-		}
-	}
-
-	const int64_t time_us = gps.time_usec
-				- static_cast<int64_t>(_params.gps_delay_ms * 1000)
-				- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
-
-	if (time_us >= static_cast<int64_t>(_gps_buffer->get_newest().time_us + _min_obs_interval_us)) {
-
-		if (!gps.vel_ned_valid || (gps.fix_type == 0)) {
-			return;
-		}
-
-		gpsSample gps_sample_new;
-
-		gps_sample_new.time_us = time_us;
-
-		gps_sample_new.vel = gps.vel_ned;
-
-		gps_sample_new.sacc = gps.sacc;
-		gps_sample_new.hacc = gps.eph;
-		gps_sample_new.vacc = gps.epv;
-
-		gps_sample_new.hgt = (float)gps.alt * 1e-3f;
-
-#if defined(CONFIG_EKF2_GNSS_YAW)
-
-		if (PX4_ISFINITE(gps.yaw)) {
-			_time_last_gps_yaw_buffer_push = _time_latest_us;
-			gps_sample_new.yaw = gps.yaw;
-			gps_sample_new.yaw_acc = PX4_ISFINITE(gps.yaw_accuracy) ? gps.yaw_accuracy : 0.f;
-
-		} else {
-			gps_sample_new.yaw = NAN;
-			gps_sample_new.yaw_acc = 0.f;
-		}
-
-		if (PX4_ISFINITE(gps.yaw_offset)) {
-			_gps_yaw_offset = gps.yaw_offset;
-
-		} else {
-			_gps_yaw_offset = 0.0f;
-		}
-
-#endif // CONFIG_EKF2_GNSS_YAW
-
-		// Only calculate the relative position if the WGS-84 location of the origin is set
-		if (collect_gps(gps)) {
-			gps_sample_new.pos = _pos_ref.project((gps.lat / 1.0e7), (gps.lon / 1.0e7));
-
-		} else {
-			gps_sample_new.pos(0) = 0.0f;
-			gps_sample_new.pos(1) = 0.0f;
-		}
-
-		_gps_buffer->push(gps_sample_new);
-		_time_last_gps_buffer_push = _time_latest_us;
-
-
-	} else {
-		ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us);
-	}
-}
-
-#if defined(CONFIG_EKF2_BAROMETER)
-void EstimatorInterface::setBaroData(const baroSample &baro_sample)
-{
-	if (!_initialised) {
-		return;
-	}
-
-	// Allocate the required buffer size if not previously done
-	if (_baro_buffer == nullptr) {
-		_baro_buffer = new RingBuffer<baroSample>(_obs_buffer_length);
-
-		if (_baro_buffer == nullptr || !_baro_buffer->valid()) {
-			delete _baro_buffer;
-			_baro_buffer = nullptr;
-			printBufferAllocationFailed("baro");
-			return;
-		}
-	}
-
-	const int64_t time_us = baro_sample.time_us
-				- static_cast<int64_t>(_params.baro_delay_ms * 1000)
-				- 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>(_baro_buffer->get_newest().time_us + _min_obs_interval_us)) {
-
-		baroSample baro_sample_new{baro_sample};
-		baro_sample_new.time_us = time_us;
-
-		_baro_buffer->push(baro_sample_new);
-		_time_last_baro_buffer_push = _time_latest_us;
-
-	} else {
-		ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _baro_buffer->get_newest().time_us, _min_obs_interval_us);
-	}
-}
-#endif // CONFIG_EKF2_BAROMETER
-
-#if defined(CONFIG_EKF2_AIRSPEED)
-void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
-{
-	if (!_initialised) {
-		return;
-	}
-
-	// Allocate the required buffer size if not previously done
-	if (_airspeed_buffer == nullptr) {
-		_airspeed_buffer = new RingBuffer<airspeedSample>(_obs_buffer_length);
-
-		if (_airspeed_buffer == nullptr || !_airspeed_buffer->valid()) {
-			delete _airspeed_buffer;
-			_airspeed_buffer = nullptr;
-			printBufferAllocationFailed("airspeed");
-			return;
-		}
-	}
-
-	const int64_t time_us = airspeed_sample.time_us
-				- static_cast<int64_t>(_params.airspeed_delay_ms * 1000)
-				- 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>(_airspeed_buffer->get_newest().time_us + _min_obs_interval_us)) {
-
-		airspeedSample airspeed_sample_new{airspeed_sample};
-		airspeed_sample_new.time_us = time_us;
-
-		_airspeed_buffer->push(airspeed_sample_new);
-
-	} else {
-		ECL_WARN("airspeed data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _airspeed_buffer->get_newest().time_us, _min_obs_interval_us);
-	}
-}
-#endif // CONFIG_EKF2_AIRSPEED
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-void EstimatorInterface::setRangeData(const rangeSample &range_sample)
-{
-	if (!_initialised) {
-		return;
-	}
-
-	// Allocate the required buffer size if not previously done
-	if (_range_buffer == nullptr) {
-		_range_buffer = new RingBuffer<rangeSample>(_obs_buffer_length);
-
-		if (_range_buffer == nullptr || !_range_buffer->valid()) {
-			delete _range_buffer;
-			_range_buffer = nullptr;
-			printBufferAllocationFailed("range");
-			return;
-		}
-	}
-
-	const int64_t time_us = range_sample.time_us
-				- static_cast<int64_t>(_params.range_delay_ms * 1000)
-				- 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>(_range_buffer->get_newest().time_us + _min_obs_interval_us)) {
-
-		rangeSample range_sample_new{range_sample};
-		range_sample_new.time_us = time_us;
-
-		_range_buffer->push(range_sample_new);
-		_time_last_range_buffer_push = _time_latest_us;
-
-	} else {
-		ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us, _min_obs_interval_us);
-	}
-}
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
-{
-	if (!_initialised) {
-		return;
-	}
-
-	// Allocate the required buffer size if not previously done
-	if (_flow_buffer == nullptr) {
-		_flow_buffer = new RingBuffer<flowSample>(_imu_buffer_length);
-
-		if (_flow_buffer == nullptr || !_flow_buffer->valid()) {
-			delete _flow_buffer;
-			_flow_buffer = nullptr;
-			printBufferAllocationFailed("flow");
-			return;
-		}
-	}
-
-	const int64_t time_us = flow.time_us
-				- static_cast<int64_t>(_params.flow_delay_ms * 1000)
-				- 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>(_flow_buffer->get_newest().time_us + _min_obs_interval_us)) {
-
-		flowSample optflow_sample_new{flow};
-		optflow_sample_new.time_us = time_us;
-
-		_flow_buffer->push(optflow_sample_new);
-
-	} else {
-		ECL_WARN("optical flow data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _flow_buffer->get_newest().time_us, _min_obs_interval_us);
-	}
-}
-#endif // CONFIG_EKF2_OPTICAL_FLOW
-
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
-{
-	if (!_initialised) {
-		return;
-	}
-
-	// Allocate the required buffer size if not previously done
-	if (_ext_vision_buffer == nullptr) {
-		_ext_vision_buffer = new RingBuffer<extVisionSample>(_obs_buffer_length);
-
-		if (_ext_vision_buffer == nullptr || !_ext_vision_buffer->valid()) {
-			delete _ext_vision_buffer;
-			_ext_vision_buffer = nullptr;
-			printBufferAllocationFailed("vision");
-			return;
-		}
-	}
-
-	// calculate the system time-stamp for the mid point of the integration period
-	const int64_t time_us = evdata.time_us
-				- static_cast<int64_t>(_params.ev_delay_ms * 1000)
-				- 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>(_ext_vision_buffer->get_newest().time_us + _min_obs_interval_us)) {
-
-		extVisionSample ev_sample_new{evdata};
-		ev_sample_new.time_us = time_us;
-
-		_ext_vision_buffer->push(ev_sample_new);
-		_time_last_ext_vision_buffer_push = _time_latest_us;
-
-	} else {
-		ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us, _min_obs_interval_us);
-	}
-}
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-
-#if defined(CONFIG_EKF2_AUXVEL)
-void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
-{
-	if (!_initialised) {
-		return;
-	}
-
-	// Allocate the required buffer size if not previously done
-	if (_auxvel_buffer == nullptr) {
-		_auxvel_buffer = new RingBuffer<auxVelSample>(_obs_buffer_length);
-
-		if (_auxvel_buffer == nullptr || !_auxvel_buffer->valid()) {
-			delete _auxvel_buffer;
-			_auxvel_buffer = nullptr;
-			printBufferAllocationFailed("aux vel");
-			return;
-		}
-	}
-
-	const int64_t time_us = auxvel_sample.time_us
-				- static_cast<int64_t>(_params.auxvel_delay_ms * 1000)
-				- 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>(_auxvel_buffer->get_newest().time_us + _min_obs_interval_us)) {
-
-		auxVelSample auxvel_sample_new{auxvel_sample};
-		auxvel_sample_new.time_us = time_us;
-
-		_auxvel_buffer->push(auxvel_sample_new);
-
-	} else {
-		ECL_WARN("aux velocity data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _auxvel_buffer->get_newest().time_us, _min_obs_interval_us);
-	}
-}
-#endif // CONFIG_EKF2_AUXVEL
-
-void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
+void Ekf::setSystemFlagData(const systemFlagUpdate &system_flags)
 {
 	if (!_initialised) {
 		return;
@@ -479,75 +138,12 @@ void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
 		_system_flag_buffer->push(system_flags_new);
 
 	} else {
-		ECL_DEBUG("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us);
+		ECL_DEBUG("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d",
+			  time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
 
-#if defined(CONFIG_EKF2_DRAG_FUSION)
-void EstimatorInterface::setDragData(const imuSample &imu)
-{
-	// down-sample the drag specific force data by accumulating and calculating the mean when
-	// sufficient samples have been collected
-	if (_params.drag_ctrl > 0) {
-
-		// Allocate the required buffer size if not previously done
-		if (_drag_buffer == nullptr) {
-			_drag_buffer = new RingBuffer<dragSample>(_obs_buffer_length);
-
-			if (_drag_buffer == nullptr || !_drag_buffer->valid()) {
-				delete _drag_buffer;
-				_drag_buffer = nullptr;
-				printBufferAllocationFailed("drag");
-				return;
-			}
-		}
-
-		// don't use any accel samples that are clipping
-		if (imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]) {
-			// reset accumulators
-			_drag_sample_count = 0;
-			_drag_down_sampled.accelXY.zero();
-			_drag_down_sampled.time_us = 0;
-			_drag_sample_time_dt = 0.0f;
-
-			return;
-		}
-
-		_drag_sample_count++;
-		// note acceleration is accumulated as a delta velocity
-		_drag_down_sampled.accelXY(0) += imu.delta_vel(0);
-		_drag_down_sampled.accelXY(1) += imu.delta_vel(1);
-		_drag_down_sampled.time_us += imu.time_us;
-		_drag_sample_time_dt += imu.delta_vel_dt;
-
-		// calculate the downsample ratio for drag specific force data
-		uint8_t min_sample_ratio = (uint8_t) ceilf((float)_imu_buffer_length / _obs_buffer_length);
-
-		if (min_sample_ratio < 5) {
-			min_sample_ratio = 5;
-		}
-
-		// calculate and store means from accumulated values
-		if (_drag_sample_count >= min_sample_ratio) {
-			// note conversion from accumulated delta velocity to acceleration
-			_drag_down_sampled.accelXY(0) /= _drag_sample_time_dt;
-			_drag_down_sampled.accelXY(1) /= _drag_sample_time_dt;
-			_drag_down_sampled.time_us /= _drag_sample_count;
-
-			// write to buffer
-			_drag_buffer->push(_drag_down_sampled);
-
-			// reset accumulators
-			_drag_sample_count = 0;
-			_drag_down_sampled.accelXY.zero();
-			_drag_down_sampled.time_us = 0;
-			_drag_sample_time_dt = 0.0f;
-		}
-	}
-}
-#endif // CONFIG_EKF2_DRAG_FUSION
-
-bool EstimatorInterface::initialise_interface(uint64_t timestamp)
+bool Ekf::initialise_interface(uint64_t timestamp)
 {
 	// find the maximum time delay the buffers are required to handle
 
@@ -565,10 +161,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	}
 
 #if defined(CONFIG_EKF2_AIRSPEED)
+
 	// using airspeed
 	if (_params.arsp_thr > FLT_EPSILON) {
 		max_time_delay_ms = math::max(_params.airspeed_delay_ms, max_time_delay_ms);
 	}
+
 #endif // CONFIG_EKF2_AIRSPEED
 
 	// mag mode
@@ -577,10 +175,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	}
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
+
 	// using range finder
 	if ((_params.rng_ctrl != RngCtrl::DISABLED)) {
 		max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms);
 	}
+
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 	if (_params.gnss_ctrl > 0) {
@@ -588,15 +188,19 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	}
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
+
 	if (_params.flow_ctrl > 0) {
 		max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
 	}
+
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
+
 	if (_params.ev_ctrl > 0) {
 		max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms);
 	}
+
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 	const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f;
@@ -629,18 +233,13 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	return true;
 }
 
-bool EstimatorInterface::isOnlyActiveSourceOfHorizontalAiding(const bool aiding_flag) const
-{
-	return aiding_flag && !isOtherSourceOfHorizontalAidingThan(aiding_flag);
-}
-
-bool EstimatorInterface::isOtherSourceOfHorizontalAidingThan(const bool aiding_flag) const
+bool Ekf::isOtherSourceOfHorizontalAidingThan(const bool aiding_flag) const
 {
 	const int nb_sources = getNumberOfActiveHorizontalAidingSources();
 	return aiding_flag ? nb_sources > 1 : nb_sources > 0;
 }
 
-int EstimatorInterface::getNumberOfActiveHorizontalAidingSources() const
+int Ekf::getNumberOfActiveHorizontalAidingSources() const
 {
 	return int(_control_status.flags.gps)
 	       + int(_control_status.flags.opt_flow)
@@ -651,28 +250,13 @@ int EstimatorInterface::getNumberOfActiveHorizontalAidingSources() const
 	       + int(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta);
 }
 
-bool EstimatorInterface::isHorizontalAidingActive() const
-{
-	return getNumberOfActiveHorizontalAidingSources() > 0;
-}
-
-bool EstimatorInterface::isOtherSourceOfVerticalPositionAidingThan(const bool aiding_flag) const
+bool Ekf::isOtherSourceOfVerticalPositionAidingThan(const bool aiding_flag) const
 {
 	const int nb_sources = getNumberOfActiveVerticalPositionAidingSources();
 	return aiding_flag ? nb_sources > 1 : nb_sources > 0;
 }
 
-bool EstimatorInterface::isVerticalPositionAidingActive() const
-{
-	return getNumberOfActiveVerticalPositionAidingSources() > 0;
-}
-
-bool EstimatorInterface::isOnlyActiveSourceOfVerticalPositionAiding(const bool aiding_flag) const
-{
-	return aiding_flag && !isOtherSourceOfVerticalPositionAidingThan(aiding_flag);
-}
-
-int EstimatorInterface::getNumberOfActiveVerticalPositionAidingSources() const
+int Ekf::getNumberOfActiveVerticalPositionAidingSources() const
 {
 	return int(_control_status.flags.gps_hgt)
 	       + int(_control_status.flags.baro_hgt)
@@ -680,30 +264,14 @@ int EstimatorInterface::getNumberOfActiveVerticalPositionAidingSources() const
 	       + int(_control_status.flags.ev_hgt);
 }
 
-bool EstimatorInterface::isVerticalAidingActive() const
-{
-	return isVerticalPositionAidingActive() || isVerticalVelocityAidingActive();
-}
-
-bool EstimatorInterface::isVerticalVelocityAidingActive() const
-{
-	return getNumberOfActiveVerticalVelocityAidingSources() > 0;
-}
-
-int EstimatorInterface::getNumberOfActiveVerticalVelocityAidingSources() const
-{
-	return int(_control_status.flags.gps)
-	       + int(_control_status.flags.ev_vel);
-}
-
-void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name)
+void Ekf::printBufferAllocationFailed(const char *buffer_name)
 {
 	if (buffer_name) {
 		ECL_ERR("%s buffer allocation failed", buffer_name);
 	}
 }
 
-void EstimatorInterface::print_status()
+void Ekf::print_status()
 {
 	printf("EKF average dt: %.6f seconds\n", (double)_dt_ekf_avg);
 
@@ -712,49 +280,71 @@ void EstimatorInterface::print_status()
 	printf("minimum observation interval %d us\n", _min_obs_interval_us);
 
 	if (_gps_buffer) {
-		printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size());
+		printf("gps buffer: %d/%d (%d Bytes)\n",
+		       _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size());
 	}
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
+
 	if (_mag_buffer) {
-		printf("mag buffer: %d/%d (%d Bytes)\n", _mag_buffer->entries(), _mag_buffer->get_length(), _mag_buffer->get_total_size());
+		printf("mag buffer: %d/%d (%d Bytes)\n",
+		       _mag_buffer->entries(), _mag_buffer->get_length(), _mag_buffer->get_total_size());
 	}
+
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_BAROMETER)
+
 	if (_baro_buffer) {
-		printf("baro buffer: %d/%d (%d Bytes)\n", _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size());
+		printf("baro buffer: %d/%d (%d Bytes)\n",
+		       _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size());
 	}
+
 #endif // CONFIG_EKF2_BAROMETER
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
+
 	if (_range_buffer) {
-		printf("range buffer: %d/%d (%d Bytes)\n", _range_buffer->entries(), _range_buffer->get_length(), _range_buffer->get_total_size());
+		printf("range buffer: %d/%d (%d Bytes)\n", _range_buffer->entries(), _range_buffer->get_length(),
+		       _range_buffer->get_total_size());
 	}
+
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_AIRSPEED)
+
 	if (_airspeed_buffer) {
-		printf("airspeed buffer: %d/%d (%d Bytes)\n", _airspeed_buffer->entries(), _airspeed_buffer->get_length(), _airspeed_buffer->get_total_size());
+		printf("airspeed buffer: %d/%d (%d Bytes)\n",
+		       _airspeed_buffer->entries(), _airspeed_buffer->get_length(), _airspeed_buffer->get_total_size());
 	}
+
 #endif // CONFIG_EKF2_AIRSPEED
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
+
 	if (_flow_buffer) {
-		printf("flow buffer: %d/%d (%d Bytes)\n", _flow_buffer->entries(), _flow_buffer->get_length(), _flow_buffer->get_total_size());
+		printf("flow buffer: %d/%d (%d Bytes)\n",
+		       _flow_buffer->entries(), _flow_buffer->get_length(), _flow_buffer->get_total_size());
 	}
+
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
+
 	if (_ext_vision_buffer) {
-		printf("vision buffer: %d/%d (%d Bytes)\n", _ext_vision_buffer->entries(), _ext_vision_buffer->get_length(), _ext_vision_buffer->get_total_size());
+		printf("vision buffer: %d/%d (%d Bytes)\n",
+		       _ext_vision_buffer->entries(), _ext_vision_buffer->get_length(), _ext_vision_buffer->get_total_size());
 	}
+
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_DRAG_FUSION)
+
 	if (_drag_buffer) {
-		printf("drag buffer: %d/%d (%d Bytes)\n", _drag_buffer->entries(), _drag_buffer->get_length(), _drag_buffer->get_total_size());
+		printf("drag buffer: %d/%d (%d Bytes)\n",
+		       _drag_buffer->entries(), _drag_buffer->get_length(), _drag_buffer->get_total_size());
 	}
+
 #endif // CONFIG_EKF2_DRAG_FUSION
 
 	_output_predictor.print_status();

src/modules/ekf2/EKF/estimator_interface.h

@@ -1,490 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file estimator_interface.h
- * Definition of base class for attitude estimators
- *
- * @author Roman Bast <bapstroman@gmail.com>
- *
- */
-
-#ifndef EKF_ESTIMATOR_INTERFACE_H
-#define EKF_ESTIMATOR_INTERFACE_H
-
-#if defined(MODULE_NAME)
-#include <px4_platform_common/log.h>
-# define ECL_INFO PX4_DEBUG
-# define ECL_WARN PX4_DEBUG
-# define ECL_ERR  PX4_DEBUG
-# define ECL_DEBUG PX4_DEBUG
-#else
-# define ECL_INFO(X, ...) printf(X "\n", ##__VA_ARGS__)
-# define ECL_WARN(X, ...) fprintf(stderr, X "\n", ##__VA_ARGS__)
-# define ECL_ERR(X, ...) fprintf(stderr, X "\n", ##__VA_ARGS__)
-
-# if defined(DEBUG_BUILD)
-#  define ECL_DEBUG(X, ...) fprintf(stderr, X "\n", ##__VA_ARGS__)
-# else
-#  define ECL_DEBUG(X, ...)
-#endif
-
-#endif
-
-#include "common.h"
-#include "RingBuffer.h"
-#include "imu_down_sampler.hpp"
-#include "output_predictor.h"
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-# include "range_finder_consistency_check.hpp"
-# include "sensor_range_finder.hpp"
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-#include <lib/geo/geo.h>
-#include <matrix/math.hpp>
-#include <mathlib/mathlib.h>
-#include <mathlib/math/filter/AlphaFilter.hpp>
-
-using namespace estimator;
-
-class EstimatorInterface
-{
-public:
-	// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
-	virtual bool collect_gps(const gpsMessage &gps) = 0;
-
-	void setIMUData(const imuSample &imu_sample);
-
-#if defined(CONFIG_EKF2_MAGNETOMETER)
-	void setMagData(const magSample &mag_sample);
-#endif // CONFIG_EKF2_MAGNETOMETER
-
-	void setGpsData(const gpsMessage &gps);
-
-#if defined(CONFIG_EKF2_BAROMETER)
-	void setBaroData(const baroSample &baro_sample);
-#endif // CONFIG_EKF2_BAROMETER
-
-#if defined(CONFIG_EKF2_AIRSPEED)
-	void setAirspeedData(const airspeedSample &airspeed_sample);
-#endif // CONFIG_EKF2_AIRSPEED
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	void setRangeData(const rangeSample &range_sample);
-
-	// set sensor limitations reported by the rangefinder
-	void set_rangefinder_limits(float min_distance, float max_distance)
-	{
-		_range_sensor.setLimits(min_distance, max_distance);
-	}
-
-	const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	// if optical flow sensor gyro delta angles are not available, set gyro_xyz vector fields to NaN and the EKF will use its internal delta angle data instead
-	void setOpticalFlowData(const flowSample &flow);
-
-	// set sensor limitations reported by the optical flow sensor
-	void set_optical_flow_limits(float max_flow_rate, float min_distance, float max_distance)
-	{
-		_flow_max_rate = max_flow_rate;
-		_flow_min_distance = min_distance;
-		_flow_max_distance = max_distance;
-	}
-#endif // CONFIG_EKF2_OPTICAL_FLOW
-
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	// set external vision position and attitude data
-	void setExtVisionData(const extVisionSample &evdata);
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-
-#if defined(CONFIG_EKF2_AUXVEL)
-	void setAuxVelData(const auxVelSample &auxvel_sample);
-#endif // CONFIG_EKF2_AUXVEL
-
-	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; }
-
-	// set vehicle landed status data
-	void set_in_air_status(bool in_air)
-	{
-		if (!in_air) {
-			if (_control_status.flags.in_air) {
-				ECL_DEBUG("no longer in air");
-			}
-
-			_time_last_on_ground_us = _time_delayed_us;
-
-		} else {
-			if (!_control_status.flags.in_air) {
-				ECL_DEBUG("in air");
-			}
-
-			_time_last_in_air = _time_delayed_us;
-		}
-
-		_control_status.flags.in_air = in_air;
-	}
-
-	void set_vehicle_at_rest(bool at_rest)
-	{
-		if (!_control_status.flags.vehicle_at_rest && at_rest) {
-			ECL_DEBUG("at rest");
-
-		} else if (_control_status.flags.vehicle_at_rest && !at_rest) {
-			ECL_DEBUG("no longer at rest");
-		}
-
-		_control_status.flags.vehicle_at_rest = at_rest;
-	}
-
-	// return true if the attitude is usable
-	bool attitude_valid() const { return _control_status.flags.tilt_align; }
-
-	// get vehicle landed status data
-	bool get_in_air_status() const { return _control_status.flags.in_air; }
-
-	// get wind estimation status
-	bool get_wind_status() const { return _control_status.flags.wind; }
-
-	// set vehicle is fixed wing status
-	void set_is_fixed_wing(bool is_fixed_wing) { _control_status.flags.fixed_wing = is_fixed_wing; }
-
-	// set flag if static pressure rise due to ground effect is expected
-	// use _params.gnd_effect_deadzone to adjust for expected rise in static pressure
-	// flag will clear after GNDEFFECT_TIMEOUT uSec
-	void set_gnd_effect()
-	{
-		_control_status.flags.gnd_effect = true;
-		_time_last_gnd_effect_on = _time_delayed_us;
-	}
-
-	// set air density used by the multi-rotor specific drag force fusion
-	void set_air_density(float air_density) { _air_density = air_density; }
-
-	// the flags considered are opt_flow, gps, ev_vel and ev_pos
-	bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const;
-
-	/*
-	 * Check if there are any other active source of horizontal aiding
-	 * Warning: does not tell if the selected source is
-	 * active, use isOnlyActiveSourceOfHorizontalAiding() for this
-	 *
-	 * The flags considered are opt_flow, gps, ev_vel and ev_pos
-	 *
-	 * @param aiding_flag a flag in _control_status.flags
-	 * @return true if an other source than aiding_flag is active
-	 */
-	bool isOtherSourceOfHorizontalAidingThan(bool aiding_flag) const;
-
-	// Return true if at least one source of horizontal aiding is active
-	// the flags considered are opt_flow, gps, ev_vel and ev_pos
-	bool isHorizontalAidingActive() const;
-	bool isVerticalAidingActive() const;
-
-	int getNumberOfActiveHorizontalAidingSources() const;
-
-	bool isOtherSourceOfVerticalPositionAidingThan(bool aiding_flag) const;
-	bool isVerticalPositionAidingActive() const;
-	bool isOnlyActiveSourceOfVerticalPositionAiding(const bool aiding_flag) const;
-	int getNumberOfActiveVerticalPositionAidingSources() const;
-
-	bool isVerticalVelocityAidingActive() const;
-	int getNumberOfActiveVerticalVelocityAidingSources() const;
-
-	const matrix::Quatf &getQuaternion() const { return _output_predictor.getQuaternion(); }
-	float getUnaidedYaw() const { return _output_predictor.getUnaidedYaw(); }
-	Vector3f getVelocity() const { return _output_predictor.getVelocity(); }
-	const Vector3f &getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); }
-	float getVerticalPositionDerivative() const { return _output_predictor.getVerticalPositionDerivative(); }
-	Vector3f getPosition() const { return _output_predictor.getPosition(); }
-	const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); }
-
-#if defined(CONFIG_EKF2_MAGNETOMETER)
-	// Get the value of magnetic declination in degrees to be saved for use at the next startup
-	// Returns true when the declination can be saved
-	// At the next startup, set param.mag_declination_deg to the value saved
-	bool get_mag_decl_deg(float &val) const
-	{
-		if (_NED_origin_initialised && (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)) {
-			val = math::degrees(_mag_declination_gps);
-			return true;
-
-		} else {
-			return false;
-		}
-	}
-
-	bool get_mag_inc_deg(float &val) const
-	{
-		if (_NED_origin_initialised) {
-			val = math::degrees(_mag_inclination_gps);
-			return true;
-
-		} else {
-			return false;
-		}
-	}
-
-	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;
-	}
-#endif // CONFIG_EKF2_MAGNETOMETER
-
-	// 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; }
-
-	const filter_control_status_u &control_status_prev() const { return _control_status_prev; }
-	const decltype(filter_control_status_u::flags) &control_status_prev_flags() const { return _control_status_prev.flags; }
-
-	// get EKF internal fault status
-	const fault_status_u &fault_status() const { return _fault_status; }
-	const decltype(fault_status_u::flags) &fault_status_flags() const { return _fault_status.flags; }
-
-	const innovation_fault_status_u &innov_check_fail_status() const { return _innov_check_fail_status; }
-	const decltype(innovation_fault_status_u::flags) &innov_check_fail_status_flags() const { return _innov_check_fail_status.flags; }
-
-	const warning_event_status_u &warning_event_status() const { return _warning_events; }
-	const decltype(warning_event_status_u::flags) &warning_event_flags() const { return _warning_events.flags; }
-	void clear_warning_events() { _warning_events.value = 0; }
-
-	const information_event_status_u &information_event_status() const { return _information_events; }
-	const decltype(information_event_status_u::flags) &information_event_flags() const { return _information_events.flags; }
-	void clear_information_events() { _information_events.value = 0; }
-
-	// Getter for the average EKF update period in s
-	float get_dt_ekf_avg() const { return _dt_ekf_avg; }
-
-	// Getters for samples on the delayed time horizon
-	const imuSample &get_imu_sample_delayed() const { return _imu_buffer.get_oldest(); }
-	const uint64_t &time_delayed_us() const { return _time_delayed_us; }
-
-	const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
-
-	const bool &global_origin_valid() const { return _NED_origin_initialised; }
-	const MapProjection &global_origin() const { return _pos_ref; }
-
-	void print_status();
-
-	float gps_horizontal_position_drift_rate_m_s() const { return _gps_horizontal_position_drift_rate_m_s; }
-	float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; }
-	float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; }
-
-	OutputPredictor &output_predictor() { return _output_predictor; };
-
-protected:
-
-	EstimatorInterface() = default;
-	virtual ~EstimatorInterface();
-
-	virtual bool init(uint64_t timestamp) = 0;
-
-	parameters _params{};		// filter parameters
-
-	/*
-	 OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer
-	 which sets the maximum frequency at which we can process non-IMU measurements. Measurements that
-	 arrive too soon after the previous measurement will not be processed.
-	 max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_S)
-	 This can be adjusted to match the max sensor data rate plus some margin for jitter.
-	*/
-	uint8_t _obs_buffer_length{0};
-
-	/*
-	IMU_BUFFER_LENGTH defines how many IMU samples we buffer which sets the time delay from current time to the
-	EKF fusion time horizon and therefore the maximum sensor time offset relative to the IMU that we can compensate for.
-	max sensor time offet (msec) =  IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS
-	This can be adjusted to a value that is FILTER_UPDATE_PERIOD_MS longer than the maximum observation time delay.
-	*/
-	uint8_t _imu_buffer_length{0};
-
-	float _dt_ekf_avg{0.010f}; ///< average update rate of the ekf in s
-
-	uint64_t _time_delayed_us{0}; // captures the imu sample on the delayed time horizon
-	uint64_t _time_latest_us{0}; // imu sample capturing the newest imu data
-
-	OutputPredictor _output_predictor{};
-
-	// measurement samples capturing measurements on the delayed time horizon
-	gpsSample _gps_sample_delayed{};
-
-
-#if defined(CONFIG_EKF2_AIRSPEED)
-	airspeedSample _airspeed_sample_delayed{};
-#endif // CONFIG_EKF2_AIRSPEED
-
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	extVisionSample _ev_sample_prev{};
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	RingBuffer<rangeSample> *_range_buffer{nullptr};
-	uint64_t _time_last_range_buffer_push{0};
-
-	sensor::SensorRangeFinder _range_sensor{};
-	RangeFinderConsistencyCheck _rng_consistency_check;
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	RingBuffer<flowSample> 	*_flow_buffer{nullptr};
-
-	flowSample _flow_sample_delayed{};
-
-	// Sensor limitations
-	float _flow_max_rate{1.0f}; ///< maximum angular flow rate that the optical flow sensor can measure (rad/s)
-	float _flow_min_distance{0.0f};	///< minimum distance that the optical flow sensor can operate at (m)
-	float _flow_max_distance{10.f};	///< maximum distance that the optical flow sensor can operate at (m)
-#endif // CONFIG_EKF2_OPTICAL_FLOW
-
-	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};		// air density (kg/m**3)
-
-	bool _imu_updated{false};      // true if the ekf should update (completed downsampling process)
-	bool _initialised{false};      // true if the ekf interface instance (data buffering) is initialized
-
-	bool _NED_origin_initialised{false};
-	float _gpos_origin_eph{0.0f}; // horizontal position uncertainty of the global origin
-	float _gpos_origin_epv{0.0f}; // vertical position uncertainty of the global origin
-	MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin
-	MapProjection _gps_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message
-	float _gps_alt_prev{0.0f};	// height from the previous GPS message (m)
-#if defined(CONFIG_EKF2_GNSS_YAW)
-	float _gps_yaw_offset{0.0f};	// Yaw offset angle for dual GPS antennas used for yaw estimation (radians).
-	// innovation consistency check monitoring ratios
-	AlphaFilter<float> _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
-	uint64_t _time_last_gps_yaw_buffer_push{0};
-#endif // CONFIG_EKF2_GNSS_YAW
-
-#if defined(CONFIG_EKF2_DRAG_FUSION)
-	RingBuffer<dragSample> *_drag_buffer{nullptr};
-	dragSample _drag_down_sampled{};	// down sampled drag specific force data (filter prediction rate -> observation rate)
-	Vector2f _drag_test_ratio{};		// drag innovation consistency check ratio
-#endif // CONFIG_EKF2_DRAG_FUSION
-
-	innovation_fault_status_u _innov_check_fail_status{};
-
-	bool _horizontal_deadreckon_time_exceeded{true};
-	bool _vertical_position_deadreckon_time_exceeded{true};
-	bool _vertical_velocity_deadreckon_time_exceeded{true};
-
-	float _gps_horizontal_position_drift_rate_m_s{NAN}; // Horizontal position drift rate (m/s)
-	float _gps_vertical_position_drift_rate_m_s{NAN};   // Vertical position drift rate (m/s)
-	float _gps_filtered_horizontal_velocity_m_s{NAN};   // Filtered horizontal velocity (m/s)
-
-	uint64_t _time_last_on_ground_us{0};	///< last time we were on the ground (uSec)
-	uint64_t _time_last_in_air{0};		///< last time we were in air (uSec)
-
-	// data buffer instances
-	static constexpr uint8_t kBufferLengthDefault = 12;
-	RingBuffer<imuSample> _imu_buffer{kBufferLengthDefault};
-
-	RingBuffer<gpsSample> *_gps_buffer{nullptr};
-
-#if defined(CONFIG_EKF2_MAGNETOMETER)
-	RingBuffer<magSample> *_mag_buffer{nullptr};
-	uint64_t _time_last_mag_buffer_push{0};
-#endif // CONFIG_EKF2_MAGNETOMETER
-
-#if defined(CONFIG_EKF2_AIRSPEED)
-	RingBuffer<airspeedSample> *_airspeed_buffer{nullptr};
-#endif // CONFIG_EKF2_AIRSPEED
-
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	RingBuffer<extVisionSample> *_ext_vision_buffer{nullptr};
-	uint64_t _time_last_ext_vision_buffer_push{0};
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-#if defined(CONFIG_EKF2_AUXVEL)
-	RingBuffer<auxVelSample> *_auxvel_buffer{nullptr};
-#endif // CONFIG_EKF2_AUXVEL
-	RingBuffer<systemFlagUpdate> *_system_flag_buffer{nullptr};
-
-	uint64_t _time_last_gps_buffer_push{0};
-
-#if defined(CONFIG_EKF2_BAROMETER)
-	RingBuffer<baroSample> *_baro_buffer{nullptr};
-	uint64_t _time_last_baro_buffer_push{0};
-#endif // CONFIG_EKF2_BAROMETER
-
-	uint64_t _time_last_gnd_effect_on{0};
-
-	fault_status_u _fault_status{};
-
-	// allocate data buffers and initialize interface variables
-	bool initialise_interface(uint64_t timestamp);
-
-	uint64_t _wmm_gps_time_last_checked{0};  // time WMM last checked
-	uint64_t _wmm_gps_time_last_set{0};      // time WMM last set
-	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{};
-
-	// this is the previous status of the filter control modes - used to detect mode transitions
-	filter_control_status_u _control_status_prev{};
-
-	// these are used to record single frame events for external monitoring and should NOT be used for
-	// state logic becasue they will be cleared externally after being read.
-	warning_event_status_u _warning_events{};
-	information_event_status_u _information_events{};
-
-private:
-
-#if defined(CONFIG_EKF2_DRAG_FUSION)
-	void setDragData(const imuSample &imu);
-
-	// Used by the multi-rotor specific drag force fusion
-	uint8_t _drag_sample_count{0};	// number of drag specific force samples assumulated at the filter prediction rate
-	float _drag_sample_time_dt{0.0f};	// time integral across all samples used to form _drag_down_sampled (sec)
-#endif // CONFIG_EKF2_DRAG_FUSION
-
-	void printBufferAllocationFailed(const char *buffer_name);
-
-	ImuDownSampler _imu_down_sampler{_params.filter_update_interval_us};
-
-	unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)
-};
-#endif // !EKF_ESTIMATOR_INTERFACE_H

src/modules/ekf2/EKF/ev_control.cpp

@@ -38,6 +38,43 @@
 
 #include "ekf.h"
 
+void Ekf::setExtVisionData(const extVisionSample &evdata)
+{
+	if (!_initialised) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	if (_ext_vision_buffer == nullptr) {
+		_ext_vision_buffer = new RingBuffer<extVisionSample>(_obs_buffer_length);
+
+		if (_ext_vision_buffer == nullptr || !_ext_vision_buffer->valid()) {
+			delete _ext_vision_buffer;
+			_ext_vision_buffer = nullptr;
+			printBufferAllocationFailed("vision");
+			return;
+		}
+	}
+
+	// calculate the system time-stamp for the mid point of the integration period
+	const int64_t time_us = evdata.time_us
+				- static_cast<int64_t>(_params.ev_delay_ms * 1000)
+				- 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>(_ext_vision_buffer->get_newest().time_us + _min_obs_interval_us)) {
+
+		extVisionSample ev_sample_new{evdata};
+		ev_sample_new.time_us = time_us;
+
+		_ext_vision_buffer->push(ev_sample_new);
+		_time_last_ext_vision_buffer_push = _time_latest_us;
+
+	} else {
+		ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us, _min_obs_interval_us);
+	}
+}
+
 void Ekf::controlExternalVisionFusion()
 {
 	_ev_pos_b_est.predict(_dt_ekf_avg);

src/modules/ekf2/EKF/gps_control.cpp

@@ -39,6 +39,85 @@
 #include "ekf.h"
 #include <mathlib/mathlib.h>
 
+void Ekf::setGpsData(const gpsMessage &gps)
+{
+	if (!_initialised) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	if (_gps_buffer == nullptr) {
+		_gps_buffer = new RingBuffer<gpsSample>(_obs_buffer_length);
+
+		if (_gps_buffer == nullptr || !_gps_buffer->valid()) {
+			delete _gps_buffer;
+			_gps_buffer = nullptr;
+			printBufferAllocationFailed("GPS");
+			return;
+		}
+	}
+
+	const int64_t time_us = gps.time_usec
+				- static_cast<int64_t>(_params.gps_delay_ms * 1000)
+				- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
+
+	if (time_us >= static_cast<int64_t>(_gps_buffer->get_newest().time_us + _min_obs_interval_us)) {
+
+		if (!gps.vel_ned_valid || (gps.fix_type == 0)) {
+			return;
+		}
+
+		gpsSample gps_sample_new;
+
+		gps_sample_new.time_us = time_us;
+
+		gps_sample_new.vel = gps.vel_ned;
+
+		gps_sample_new.sacc = gps.sacc;
+		gps_sample_new.hacc = gps.eph;
+		gps_sample_new.vacc = gps.epv;
+
+		gps_sample_new.hgt = (float)gps.alt * 1e-3f;
+
+#if defined(CONFIG_EKF2_GNSS_YAW)
+
+		if (PX4_ISFINITE(gps.yaw)) {
+			_time_last_gps_yaw_buffer_push = _time_latest_us;
+			gps_sample_new.yaw = gps.yaw;
+			gps_sample_new.yaw_acc = PX4_ISFINITE(gps.yaw_accuracy) ? gps.yaw_accuracy : 0.f;
+
+		} else {
+			gps_sample_new.yaw = NAN;
+			gps_sample_new.yaw_acc = 0.f;
+		}
+
+		if (PX4_ISFINITE(gps.yaw_offset)) {
+			_gps_yaw_offset = gps.yaw_offset;
+
+		} else {
+			_gps_yaw_offset = 0.0f;
+		}
+
+#endif // CONFIG_EKF2_GNSS_YAW
+
+		// Only calculate the relative position if the WGS-84 location of the origin is set
+		if (collect_gps(gps)) {
+			gps_sample_new.pos = _pos_ref.project((gps.lat / 1.0e7), (gps.lon / 1.0e7));
+
+		} else {
+			gps_sample_new.pos(0) = 0.0f;
+			gps_sample_new.pos(1) = 0.0f;
+		}
+
+		_gps_buffer->push(gps_sample_new);
+		_time_last_gps_buffer_push = _time_latest_us;
+
+
+	} else {
+		ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us);
+	}
+}
+
 void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 {
 	if (!_gps_buffer || (_params.gnss_ctrl == 0)) {

src/modules/ekf2/EKF/mag_control.cpp

@@ -39,6 +39,42 @@
 #include "ekf.h"
 #include <mathlib/mathlib.h>
 
+void Ekf::setMagData(const magSample &mag_sample)
+{
+	if (!_initialised) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	if (_mag_buffer == nullptr) {
+		_mag_buffer = new RingBuffer<magSample>(_obs_buffer_length);
+
+		if (_mag_buffer == nullptr || !_mag_buffer->valid()) {
+			delete _mag_buffer;
+			_mag_buffer = nullptr;
+			printBufferAllocationFailed("mag");
+			return;
+		}
+	}
+
+	const int64_t time_us = mag_sample.time_us
+				- static_cast<int64_t>(_params.mag_delay_ms * 1000)
+				- 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>(_mag_buffer->get_newest().time_us + _min_obs_interval_us)) {
+
+		magSample mag_sample_new{mag_sample};
+		mag_sample_new.time_us = time_us;
+
+		_mag_buffer->push(mag_sample_new);
+		_time_last_mag_buffer_push = _time_latest_us;
+
+	} else {
+		ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, _min_obs_interval_us);
+	}
+}
+
 void Ekf::controlMagFusion()
 {
 	// reset the flight alignment flag so that the mag fields will be

src/modules/ekf2/EKF/optical_flow_control.cpp

@@ -38,6 +38,41 @@
 
 #include "ekf.h"
 
+void Ekf::setOpticalFlowData(const flowSample &flow)
+{
+	if (!_initialised) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	if (_flow_buffer == nullptr) {
+		_flow_buffer = new RingBuffer<flowSample>(_imu_buffer_length);
+
+		if (_flow_buffer == nullptr || !_flow_buffer->valid()) {
+			delete _flow_buffer;
+			_flow_buffer = nullptr;
+			printBufferAllocationFailed("flow");
+			return;
+		}
+	}
+
+	const int64_t time_us = flow.time_us
+				- static_cast<int64_t>(_params.flow_delay_ms * 1000)
+				- 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>(_flow_buffer->get_newest().time_us + _min_obs_interval_us)) {
+
+		flowSample optflow_sample_new{flow};
+		optflow_sample_new.time_us = time_us;
+
+		_flow_buffer->push(optflow_sample_new);
+
+	} else {
+		ECL_WARN("optical flow data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _flow_buffer->get_newest().time_us, _min_obs_interval_us);
+	}
+}
+
 void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 {
 	if (_flow_buffer) {

src/modules/ekf2/EKF/range_height_control.cpp

@@ -38,6 +38,42 @@
 
 #include "ekf.h"
 
+void Ekf::setRangeData(const rangeSample &range_sample)
+{
+	if (!_initialised) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	if (_range_buffer == nullptr) {
+		_range_buffer = new RingBuffer<rangeSample>(_obs_buffer_length);
+
+		if (_range_buffer == nullptr || !_range_buffer->valid()) {
+			delete _range_buffer;
+			_range_buffer = nullptr;
+			printBufferAllocationFailed("range");
+			return;
+		}
+	}
+
+	const int64_t time_us = range_sample.time_us
+				- static_cast<int64_t>(_params.range_delay_ms * 1000)
+				- 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>(_range_buffer->get_newest().time_us + _min_obs_interval_us)) {
+
+		rangeSample range_sample_new{range_sample};
+		range_sample_new.time_us = time_us;
+
+		_range_buffer->push(range_sample_new);
+		_time_last_range_buffer_push = _time_latest_us;
+
+	} else {
+		ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us, _min_obs_interval_us);
+	}
+}
+
 void Ekf::controlRangeHeightFusion()
 {
 	static constexpr const char *HGT_SRC_NAME = "RNG";

src/modules/ekf2/EKF2.cpp

@@ -1002,6 +1002,11 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
 	PublishAidSourceStatus(_ekf.aid_src_baro_hgt(), _status_baro_hgt_pub_last, _estimator_aid_src_baro_hgt_pub);
 #endif // CONFIG_EKF2_BAROMETER
 
+#if defined(CONFIG_EKF2_DRAG_FUSION)
+	// drag
+	PublishAidSourceStatus(_ekf.aid_src_drag(), _status_drag_pub_last, _estimator_aid_src_drag_pub);
+#endif // CONFIG_EKF2_DRAG_FUSION
+
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	// RNG height
 	PublishAidSourceStatus(_ekf.aid_src_rng_hgt(), _status_rng_hgt_pub_last, _estimator_aid_src_rng_hgt_pub);
@@ -1344,49 +1349,89 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 	// publish estimator innovation data
 	estimator_innovations_s innovations{};
 	innovations.timestamp_sample = _ekf.time_delayed_us();
-	_ekf.getGpsVelPosInnov(innovations.gps_hvel, innovations.gps_vvel, innovations.gps_hpos, innovations.gps_vpos);
+
+	// GPS
+	innovations.gps_hvel[0] = _ekf.aid_src_gnss_vel().innovation[0];
+	innovations.gps_hvel[1] = _ekf.aid_src_gnss_vel().innovation[1];
+	innovations.gps_vvel    = _ekf.aid_src_gnss_vel().innovation[2];
+	innovations.gps_hpos[0] = _ekf.aid_src_gnss_pos().innovation[0];
+	innovations.gps_hpos[1] = _ekf.aid_src_gnss_pos().innovation[1];
+	innovations.gps_vpos    = _ekf.aid_src_gnss_hgt().innovation;
+
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	_ekf.getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos);
+	// External Vision
+	innovations.ev_hvel[0] = _ekf.aid_src_ev_vel().innovation[0];
+	innovations.ev_hvel[1] = _ekf.aid_src_ev_vel().innovation[1];
+	innovations.ev_vvel    = _ekf.aid_src_ev_vel().innovation[2];
+	innovations.ev_hpos[0] = _ekf.aid_src_ev_pos().innovation[0];
+	innovations.ev_hpos[1] = _ekf.aid_src_ev_pos().innovation[1];
+	innovations.ev_vpos    = _ekf.aid_src_ev_hgt().innovation;
 #endif // CONFIG_EKF2_EXTERNAL_VISION
-#if defined(CONFIG_EKF2_BAROMETER)
-	_ekf.getBaroHgtInnov(innovations.baro_vpos);
-#endif // CONFIG_EKF2_BAROMETER
+
+	// Height sensors
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	_ekf.getRngHgtInnov(innovations.rng_vpos);
+	innovations.rng_vpos = _ekf.aid_src_rng_hgt().innovation;
 #endif // CONFIG_EKF2_RANGE_FINDER
+#if defined(CONFIG_EKF2_BAROMETER)
+	innovations.baro_vpos = _ekf.aid_src_baro_hgt().innovation;
+#endif // CONFIG_EKF2_BAROMETER
+
 #if defined(CONFIG_EKF2_AUXVEL)
-	_ekf.getAuxVelInnov(innovations.aux_hvel);
+	// Auxiliary velocity
+	innovations.aux_hvel[0] = _ekf.aid_src_aux_vel().innovation[0];
+	innovations.aux_hvel[1] = _ekf.aid_src_aux_vel().innovation[1];
 #endif // CONFIG_EKF2_AUXVEL
+
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	_ekf.getFlowInnov(innovations.flow);
+	// Optical flow
+	innovations.flow[0] = _ekf.aid_src_optical_flow().innovation[0];
+	innovations.flow[1] = _ekf.aid_src_optical_flow().innovation[1];
+# if defined(CONFIG_EKF2_TERRAIN)
+	innovations.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().innovation[0];
+	innovations.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().innovation[1];
+# endif // CONFIG_EKF2_TERRAIN
 #endif // CONFIG_EKF2_OPTICAL_FLOW
-	_ekf.getHeadingInnov(innovations.heading);
+
+	// heading
+	innovations.heading = _ekf.getHeadingInnov();
+
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	_ekf.getMagInnov(innovations.mag_field);
+	// mag_field
+	innovations.mag_field[0] = _ekf.aid_src_mag().innovation[0];
+	innovations.mag_field[1] = _ekf.aid_src_mag().innovation[1];
+	innovations.mag_field[2] = _ekf.aid_src_mag().innovation[2];
 #endif // CONFIG_EKF2_MAGNETOMETER
+
+	// gravity
+	innovations.gravity[0] = _ekf.aid_src_gravity().innovation[0];
+	innovations.gravity[1] = _ekf.aid_src_gravity().innovation[1];
+	innovations.gravity[2] = _ekf.aid_src_gravity().innovation[2];
+
 #if defined(CONFIG_EKF2_DRAG_FUSION)
-	_ekf.getDragInnov(innovations.drag);
+	// drag
+	innovations.drag[0] = _ekf.aid_src_drag().innovation[0];
+	innovations.drag[1] = _ekf.aid_src_drag().innovation[1];
 #endif // CONFIG_EKF2_DRAG_FUSION
+
 #if defined(CONFIG_EKF2_AIRSPEED)
-	_ekf.getAirspeedInnov(innovations.airspeed);
+	// airspeed
+	innovations.airspeed = _ekf.aid_src_airspeed().innovation;
 #endif // CONFIG_EKF2_AIRSPEED
+
 #if defined(CONFIG_EKF2_SIDESLIP)
-	_ekf.getBetaInnov(innovations.beta);
+	// beta
+	innovations.beta = _ekf.aid_src_sideslip().innovation;
 #endif // CONFIG_EKF2_SIDESLIP
 
-	_ekf.getGravityInnov(innovations.gravity);
+#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER)
+	// hagl
+	innovations.hagl = _ekf.aid_src_terrain_range_finder().innovation;
+#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
 
-#if defined(CONFIG_EKF2_TERRAIN)
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	_ekf.getHaglInnov(innovations.hagl);
+	// hagl_rate
+	innovations.hagl_rate = _ekf.getHaglRateInnov();
 #endif // CONFIG_EKF2_RANGE_FINDER
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	_ekf.getTerrainFlowInnov(innovations.terr_flow);
-# endif // CONFIG_EKF2_OPTICAL_FLOW
-#endif // CONFIG_EKF2_TERRAIN
-
-	// Not yet supported
-	innovations.aux_vvel = NAN;
 
 	innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_innovations_pub.publish(innovations);
@@ -1434,51 +1479,89 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
 	// publish estimator innovation test ratio data
 	estimator_innovations_s test_ratios{};
 	test_ratios.timestamp_sample = _ekf.time_delayed_us();
-	_ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0],
-				    test_ratios.gps_vpos);
+
+	// GPS
+	test_ratios.gps_hvel[0] = _ekf.aid_src_gnss_vel().test_ratio[0];
+	test_ratios.gps_hvel[1] = _ekf.aid_src_gnss_vel().test_ratio[1];
+	test_ratios.gps_vvel    = _ekf.aid_src_gnss_vel().test_ratio[2];
+	test_ratios.gps_hpos[0] = _ekf.aid_src_gnss_pos().test_ratio[0];
+	test_ratios.gps_hpos[1] = _ekf.aid_src_gnss_pos().test_ratio[1];
+	test_ratios.gps_vpos    = _ekf.aid_src_gnss_hgt().test_ratio;
+
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	_ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0], test_ratios.ev_vpos);
+	// External Vision
+	test_ratios.ev_hvel[0] = _ekf.aid_src_ev_vel().test_ratio[0];
+	test_ratios.ev_hvel[1] = _ekf.aid_src_ev_vel().test_ratio[1];
+	test_ratios.ev_vvel    = _ekf.aid_src_ev_vel().test_ratio[2];
+	test_ratios.ev_hpos[0] = _ekf.aid_src_ev_pos().test_ratio[0];
+	test_ratios.ev_hpos[1] = _ekf.aid_src_ev_pos().test_ratio[1];
+	test_ratios.ev_vpos    = _ekf.aid_src_ev_hgt().test_ratio;
 #endif // CONFIG_EKF2_EXTERNAL_VISION
-#if defined(CONFIG_EKF2_BAROMETER)
-	_ekf.getBaroHgtInnovRatio(test_ratios.baro_vpos);
-#endif // CONFIG_EKF2_BAROMETER
+
+	// Height sensors
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	_ekf.getRngHgtInnovRatio(test_ratios.rng_vpos);
-	_ekf.getHaglRateInnovRatio(test_ratios.hagl_rate);
+	test_ratios.rng_vpos = _ekf.aid_src_rng_hgt().test_ratio;
 #endif // CONFIG_EKF2_RANGE_FINDER
+#if defined(CONFIG_EKF2_BAROMETER)
+	test_ratios.baro_vpos = _ekf.aid_src_baro_hgt().test_ratio;
+#endif // CONFIG_EKF2_BAROMETER
+
 #if defined(CONFIG_EKF2_AUXVEL)
-	_ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]);
+	// Auxiliary velocity
+	test_ratios.aux_hvel[0] = _ekf.aid_src_aux_vel().test_ratio[0];
+	test_ratios.aux_hvel[1] = _ekf.aid_src_aux_vel().test_ratio[1];
 #endif // CONFIG_EKF2_AUXVEL
+
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	_ekf.getFlowInnovRatio(test_ratios.flow[0]);
+	// Optical flow
+	test_ratios.flow[0] = _ekf.aid_src_optical_flow().test_ratio[0];
+	test_ratios.flow[1] = _ekf.aid_src_optical_flow().test_ratio[1];
+# if defined(CONFIG_EKF2_TERRAIN)
+	test_ratios.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().test_ratio[0];
+	test_ratios.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().test_ratio[1];
+# endif // CONFIG_EKF2_TERRAIN
 #endif // CONFIG_EKF2_OPTICAL_FLOW
-	_ekf.getHeadingInnovRatio(test_ratios.heading);
+
+	// heading
+	test_ratios.heading = _ekf.getHeadingInnovRatio();
+
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	_ekf.getMagInnovRatio(test_ratios.mag_field[0]);
+	// mag_field
+	test_ratios.mag_field[0] = _ekf.aid_src_mag().test_ratio[0];
+	test_ratios.mag_field[1] = _ekf.aid_src_mag().test_ratio[1];
+	test_ratios.mag_field[2] = _ekf.aid_src_mag().test_ratio[2];
 #endif // CONFIG_EKF2_MAGNETOMETER
+
+	// gravity
+	test_ratios.gravity[0] = _ekf.aid_src_gravity().test_ratio[0];
+	test_ratios.gravity[1] = _ekf.aid_src_gravity().test_ratio[1];
+	test_ratios.gravity[2] = _ekf.aid_src_gravity().test_ratio[2];
+
 #if defined(CONFIG_EKF2_DRAG_FUSION)
-	_ekf.getDragInnovRatio(&test_ratios.drag[0]);
+	// drag
+	test_ratios.drag[0] = _ekf.aid_src_drag().test_ratio[0];
+	test_ratios.drag[1] = _ekf.aid_src_drag().test_ratio[1];
 #endif // CONFIG_EKF2_DRAG_FUSION
+
 #if defined(CONFIG_EKF2_AIRSPEED)
-	_ekf.getAirspeedInnovRatio(test_ratios.airspeed);
+	// airspeed
+	test_ratios.airspeed = _ekf.aid_src_airspeed().test_ratio;
 #endif // CONFIG_EKF2_AIRSPEED
+
 #if defined(CONFIG_EKF2_SIDESLIP)
-	_ekf.getBetaInnovRatio(test_ratios.beta);
+	// beta
+	test_ratios.beta = _ekf.aid_src_sideslip().test_ratio;
 #endif // CONFIG_EKF2_SIDESLIP
 
-	_ekf.getGravityInnovRatio(test_ratios.gravity[0]);
+#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER)
+	// hagl
+	test_ratios.hagl = _ekf.aid_src_terrain_range_finder().test_ratio;
+#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
 
-#if defined(CONFIG_EKF2_TERRAIN)
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	_ekf.getHaglInnovRatio(test_ratios.hagl);
-# endif
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	_ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]);
-# endif // CONFIG_EKF2_OPTICAL_FLOW
-#endif // CONFIG_EKF2_TERRAIN
-
-	// Not yet supported
-	test_ratios.aux_vvel = NAN;
+	// hagl_rate
+	test_ratios.hagl_rate = _ekf.getHaglRateInnovRatio();
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 	test_ratios.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_innovation_test_ratios_pub.publish(test_ratios);
@@ -1489,50 +1572,89 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
 	// publish estimator innovation variance data
 	estimator_innovations_s variances{};
 	variances.timestamp_sample = _ekf.time_delayed_us();
-	_ekf.getGpsVelPosInnovVar(variances.gps_hvel, variances.gps_vvel, variances.gps_hpos, variances.gps_vpos);
+
+	// GPS
+	variances.gps_hvel[0] = _ekf.aid_src_gnss_vel().innovation_variance[0];
+	variances.gps_hvel[1] = _ekf.aid_src_gnss_vel().innovation_variance[1];
+	variances.gps_vvel    = _ekf.aid_src_gnss_vel().innovation_variance[2];
+	variances.gps_hpos[0] = _ekf.aid_src_gnss_pos().innovation_variance[0];
+	variances.gps_hpos[1] = _ekf.aid_src_gnss_pos().innovation_variance[1];
+	variances.gps_vpos    = _ekf.aid_src_gnss_hgt().innovation_variance;
+
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	_ekf.getEvVelPosInnovVar(variances.ev_hvel, variances.ev_vvel, variances.ev_hpos, variances.ev_vpos);
+	// External Vision
+	variances.ev_hvel[0] = _ekf.aid_src_ev_vel().innovation_variance[0];
+	variances.ev_hvel[1] = _ekf.aid_src_ev_vel().innovation_variance[1];
+	variances.ev_vvel    = _ekf.aid_src_ev_vel().innovation_variance[2];
+	variances.ev_hpos[0] = _ekf.aid_src_ev_pos().innovation_variance[0];
+	variances.ev_hpos[1] = _ekf.aid_src_ev_pos().innovation_variance[1];
+	variances.ev_vpos    = _ekf.aid_src_ev_hgt().innovation_variance;
 #endif // CONFIG_EKF2_EXTERNAL_VISION
-#if defined(CONFIG_EKF2_BAROMETER)
-	_ekf.getBaroHgtInnovVar(variances.baro_vpos);
-#endif // CONFIG_EKF2_BAROMETER
+
+	// Height sensors
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	_ekf.getRngHgtInnovVar(variances.rng_vpos);
-	_ekf.getHaglRateInnovVar(variances.hagl_rate);
+	variances.rng_vpos = _ekf.aid_src_rng_hgt().innovation_variance;
 #endif // CONFIG_EKF2_RANGE_FINDER
+#if defined(CONFIG_EKF2_BAROMETER)
+	variances.baro_vpos = _ekf.aid_src_baro_hgt().innovation_variance;
+#endif // CONFIG_EKF2_BAROMETER
+
 #if defined(CONFIG_EKF2_AUXVEL)
-	_ekf.getAuxVelInnovVar(variances.aux_hvel);
+	// Auxiliary velocity
+	variances.aux_hvel[0] = _ekf.aid_src_aux_vel().innovation_variance[0];
+	variances.aux_hvel[1] = _ekf.aid_src_aux_vel().innovation_variance[1];
 #endif // CONFIG_EKF2_AUXVEL
+
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	_ekf.getFlowInnovVar(variances.flow);
+	// Optical flow
+	variances.flow[0] = _ekf.aid_src_optical_flow().innovation_variance[0];
+	variances.flow[1] = _ekf.aid_src_optical_flow().innovation_variance[1];
+# if defined(CONFIG_EKF2_TERRAIN)
+	variances.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().innovation_variance[0];
+	variances.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().innovation_variance[1];
+# endif // CONFIG_EKF2_TERRAIN
 #endif // CONFIG_EKF2_OPTICAL_FLOW
-	_ekf.getHeadingInnovVar(variances.heading);
+
+	// heading
+	variances.heading = _ekf.getHeadingInnovVar();
+
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	_ekf.getMagInnovVar(variances.mag_field);
+	// mag_field
+	variances.mag_field[0] = _ekf.aid_src_mag().innovation_variance[0];
+	variances.mag_field[1] = _ekf.aid_src_mag().innovation_variance[1];
+	variances.mag_field[2] = _ekf.aid_src_mag().innovation_variance[2];
 #endif // CONFIG_EKF2_MAGNETOMETER
+
+	// gravity
+	variances.gravity[0] = _ekf.aid_src_gravity().innovation_variance[0];
+	variances.gravity[1] = _ekf.aid_src_gravity().innovation_variance[1];
+	variances.gravity[2] = _ekf.aid_src_gravity().innovation_variance[2];
+
 #if defined(CONFIG_EKF2_DRAG_FUSION)
-	_ekf.getDragInnovVar(variances.drag);
+	// drag
+	variances.drag[0] = _ekf.aid_src_drag().innovation_variance[0];
+	variances.drag[1] = _ekf.aid_src_drag().innovation_variance[1];
 #endif // CONFIG_EKF2_DRAG_FUSION
+
 #if defined(CONFIG_EKF2_AIRSPEED)
-	_ekf.getAirspeedInnovVar(variances.airspeed);
+	// airspeed
+	variances.airspeed = _ekf.aid_src_airspeed().innovation_variance;
 #endif // CONFIG_EKF2_AIRSPEED
+
 #if defined(CONFIG_EKF2_SIDESLIP)
-	_ekf.getBetaInnovVar(variances.beta);
+	// beta
+	variances.beta = _ekf.aid_src_sideslip().innovation_variance;
 #endif // CONFIG_EKF2_SIDESLIP
 
-#if defined(CONFIG_EKF2_TERRAIN)
-# if defined(CONFIG_EKF2_RANGE_FINDER)
-	_ekf.getHaglInnovVar(variances.hagl);
-# endif // CONFIG_EKF2_RANGE_FINDER
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	_ekf.getTerrainFlowInnovVar(variances.terr_flow);
-# endif // CONFIG_EKF2_OPTICAL_FLOW
+#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER)
+	// hagl
+	variances.hagl = _ekf.aid_src_terrain_range_finder().innovation_variance;
 #endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
 
-	_ekf.getGravityInnovVar(variances.gravity);
-
-	// Not yet supported
-	variances.aux_vvel = NAN;
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	// hagl_rate
+	variances.hagl_rate = _ekf.getHaglRateInnovVar();
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 	variances.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_innovation_variances_pub.publish(variances);
@@ -1955,7 +2077,7 @@ void EKF2::PublishYawEstimatorStatus(const hrt_abstime &timestamp)
 
 void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
 {
-	if (_ekf.get_wind_status()) {
+	if (_ekf.control_status_flags().wind) {
 		// Publish wind estimate only if ekf declares them valid
 		wind_s wind{};
 		wind.timestamp_sample = _ekf.time_delayed_us();
@@ -1964,12 +2086,12 @@ void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
 		const Vector2f wind_vel_var = _ekf.getWindVelocityVariance();
 
 #if defined(CONFIG_EKF2_AIRSPEED)
-		_ekf.getAirspeedInnov(wind.tas_innov);
-		_ekf.getAirspeedInnovVar(wind.tas_innov_var);
+		wind.tas_innov = _ekf.aid_src_airspeed().innovation;
+		wind.tas_innov_var = _ekf.aid_src_airspeed().innovation_variance;
 #endif // CONFIG_EKF2_AIRSPEED
 #if defined(CONFIG_EKF2_SIDESLIP)
-		_ekf.getBetaInnov(wind.beta_innov);
-		_ekf.getBetaInnovVar(wind.beta_innov_var);
+		wind.beta_innov = _ekf.aid_src_sideslip().innovation;
+		wind.beta_innov = _ekf.aid_src_sideslip().innovation_variance;
 #endif // CONFIG_EKF2_SIDESLIP
 
 		wind.windspeed_north = wind_vel(0);

src/modules/ekf2/EKF2.hpp

@@ -404,6 +404,11 @@ private:
 	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_baro_hgt_pub {ORB_ID(estimator_aid_src_baro_hgt)};
 #endif // CONFIG_EKF2_BAROMETER
 
+#if defined(CONFIG_EKF2_DRAG_FUSION)
+	uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_drag_pub {ORB_ID(estimator_aid_src_drag)};
+	hrt_abstime _status_drag_pub_last{0};
+#endif // CONFIG_EKF2_DRAG_FUSION
+
 	float _last_gnss_hgt_bias_published{};
 
 #if defined(CONFIG_EKF2_AIRSPEED)

src/modules/ekf2/test/sensor_simulator/ekf_logger.h

@@ -39,7 +39,6 @@
 #define EKF_EKF_LOGGER_H
 
 #include "EKF/ekf.h"
-#include "EKF/estimator_interface.h"
 #include "ekf_wrapper.h"
 #include <fstream>
 #include <iostream>

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h

@@ -40,7 +40,6 @@
 
 #include <memory>
 #include "EKF/ekf.h"
-#include "EKF/estimator_interface.h"
 
 class EkfWrapper
 {

src/modules/ekf2/test/test_EKF_basics.cpp

@@ -245,22 +245,16 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized)
 
 	_sensor_simulator.runSeconds(1);
 
-	float hpos = 0.f;
-	float vpos = 0.f;
-	float hvel = 0.f;
-	float vvel = 0.f;
-	float baro_vpos = 0.f;
-
 	// After the change of origin, the pos and vel innovations should stay small
-	_ekf->getGpsVelPosInnovRatio(hvel, vvel, hpos, vpos);
-	_ekf->getBaroHgtInnovRatio(baro_vpos);
+	EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[0], 0.f, 0.05f);
+	EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[1], 0.f, 0.05f);
+	EXPECT_NEAR(_ekf->aid_src_gnss_hgt().test_ratio, 0.f, 0.05f);
 
-	EXPECT_NEAR(hpos, 0.f, 0.05f);
-	EXPECT_NEAR(vpos, 0.f, 0.05f);
-	EXPECT_NEAR(baro_vpos, 0.f, 0.05f);
+	EXPECT_NEAR(_ekf->aid_src_baro_hgt().test_ratio, 0.f, 0.05f);
 
-	EXPECT_NEAR(hvel, 0.f, 0.02f);
-	EXPECT_NEAR(vvel, 0.f, 0.02f);
+	EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[0], 0.f, 0.02f);
+	EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[1], 0.f, 0.02f);
+	EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[2], 0.f, 0.02f);
 }
 
 TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized)
@@ -291,19 +285,14 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized)
 
 	_sensor_simulator.runSeconds(1);
 
-	float hpos = 0.f;
-	float vpos = 0.f;
-	float hvel = 0.f;
-	float vvel = 0.f;
-
 	// After the change of origin, the pos and vel innovations should stay small
-	_ekf->getGpsVelPosInnovRatio(hvel, vvel, hpos, vpos);
+	EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[0], 0.f, 0.05f);
+	EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[1], 0.f, 0.05f);
+	EXPECT_NEAR(_ekf->aid_src_gnss_hgt().test_ratio, 0.f, 0.05f);
 
-	EXPECT_NEAR(hpos, 0.f, 0.05f);
-	EXPECT_NEAR(vpos, 0.f, 0.05f);
-
-	EXPECT_NEAR(hvel, 0.f, 0.02f);
-	EXPECT_NEAR(vvel, 0.f, 0.02f);
+	EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[0], 0.f, 0.02f);
+	EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[1], 0.f, 0.02f);
+	EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[2], 0.f, 0.02f);
 }
 
 TEST_F(EkfBasicsTest, global_position_from_local_ev)

src/modules/ekf2/test/test_EKF_gps.cpp

@@ -222,8 +222,7 @@ TEST_F(EkfGpsTest, altitudeDrift)
 		_sensor_simulator.runSeconds(dt);
 	}
 
-	float baro_innov;
-	_ekf->getBaroHgtInnov(baro_innov);
+	float baro_innov = _ekf->aid_src_baro_hgt().innovation;
 	BiasEstimator::status status = _ekf->getBaroBiasEstimatorStatus();
 
 	printf("baro innov = %f\n", (double)baro_innov);

src/modules/ekf2/test/test_EKF_height_fusion.cpp

@@ -198,13 +198,8 @@ TEST_F(EkfHeightFusionTest, gpsRef)
 	EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_initial + baro_increment - gps_step, 0.2f);
 
 	// and the innovations are close to zero
-	float baro_innov = NAN;
-	_ekf->getBaroHgtInnov(baro_innov);
-	EXPECT_NEAR(baro_innov, 0.f, 0.2f);
-
-	float rng_innov = NAN;
-	_ekf->getRngHgtInnov(rng_innov);
-	EXPECT_NEAR(rng_innov, 0.f, 0.2f);
+	EXPECT_NEAR(_ekf->aid_src_baro_hgt().innovation, 0.f, 0.2f);
+	EXPECT_NEAR(_ekf->aid_src_rng_hgt().innovation, 0.f, 0.2f);
 }
 
 TEST_F(EkfHeightFusionTest, baroRefFailOver)