terrain_estimator: add unit tests

test/sensor_simulator/ekf_wrapper.cpp

@@ -177,6 +177,40 @@ bool EkfWrapper::isWindVelocityEstimated() const
 	return control_status.flags.wind;
 }
 
+void EkfWrapper::enableTerrainRngFusion()
+{
+	_ekf_params->terrain_fusion_mode |= TerrainFusionMask::TerrainFuseRangeFinder;
+}
+
+void EkfWrapper::disableTerrainRngFusion()
+{
+	_ekf_params->terrain_fusion_mode &= ~TerrainFusionMask::TerrainFuseRangeFinder;
+}
+
+bool EkfWrapper::isIntendingTerrainRngFusion() const
+{
+	terrain_fusion_status_u terrain_status;
+	terrain_status.value = _ekf->getTerrainEstimateSensorBitfield();
+	return terrain_status.flags.range_finder;
+}
+
+void EkfWrapper::enableTerrainFlowFusion()
+{
+	_ekf_params->terrain_fusion_mode |= TerrainFusionMask::TerrainFuseOpticalFlow;
+}
+
+void EkfWrapper::disableTerrainFlowFusion()
+{
+	_ekf_params->terrain_fusion_mode &= ~TerrainFusionMask::TerrainFuseOpticalFlow;
+}
+
+bool EkfWrapper::isIntendingTerrainFlowFusion() const
+{
+	terrain_fusion_status_u terrain_status;
+	terrain_status.value = _ekf->getTerrainEstimateSensorBitfield();
+	return terrain_status.flags.flow;
+}
+
 Eulerf EkfWrapper::getEulerAngles() const
 {
 	return Eulerf(_ekf->getQuaternion());

test/sensor_simulator/ekf_wrapper.h

@@ -89,6 +89,14 @@ public:
 
 	bool isWindVelocityEstimated() const;
 
+	void enableTerrainRngFusion();
+	void disableTerrainRngFusion();
+	bool isIntendingTerrainRngFusion() const;
+
+	void enableTerrainFlowFusion();
+	void disableTerrainFlowFusion();
+	bool isIntendingTerrainFlowFusion() const;
+
 	Eulerf getEulerAngles() const;
 	float getYawAngle() const;
 	matrix::Vector<float, 4> getQuaternionVariance() const;

test/sensor_simulator/flow.cpp

@@ -15,7 +15,7 @@ Flow::~Flow()
 
 void Flow::send(uint64_t time)
 {
-	_flow_data.dt = time - _time_last_data_sent;
+	_flow_data.dt = static_cast<float>(time - _time_last_data_sent) * 1e-6f;
 	_flow_data.time_us = time;
 	_ekf->setOpticalFlowData(_flow_data);
 }