ekf2-flow: refactor control logic

Use flow rates instead of integrals in backend. This allows us to delay
the data to the mitpoint integration time and simplifies the code in
general.
Gyro compensation can still be done in EKF2 if needed, but the
flow module normally already appends the correct gyro data to the flow
message.

src/modules/ekf2/test/sensor_simulator/flow.cpp

@@ -28,9 +28,8 @@ void Flow::setData(const flowSample &flow)
 flowSample Flow::dataAtRest()
 {
 	flowSample flow_at_rest;
-	flow_at_rest.dt = static_cast<float>(_update_period) * 1e-6f;
-	flow_at_rest.flow_xy_rad = Vector2f{0.0f, 0.0f};
-	flow_at_rest.gyro_xyz = Vector3f{0.0f, 0.0f, 0.0f};
+	flow_at_rest.flow_rate = Vector2f{0.0f, 0.0f};
+	flow_at_rest.gyro_rate = Vector3f{0.0f, 0.0f, 0.0f};
 	flow_at_rest.quality = 255;
 	return flow_at_rest;
 }

src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp

@@ -266,11 +266,11 @@ void SensorSimulator::setSingleReplaySample(const sensor_info &sample)
 
 	} else if (sample.sensor_type == sensor_info::measurement_t::FLOW) {
 		flowSample flow_sample;
-		flow_sample.flow_xy_rad = Vector2f(sample.sensor_data[0],
-						   sample.sensor_data[1]);
-		flow_sample.gyro_xyz = Vector3f(sample.sensor_data[2],
-						sample.sensor_data[3],
-						sample.sensor_data[4]);
+		flow_sample.flow_rate = Vector2f(sample.sensor_data[0],
+						 sample.sensor_data[1]);
+		flow_sample.gyro_rate = Vector3f(sample.sensor_data[2],
+						 sample.sensor_data[3],
+						 sample.sensor_data[4]);
 		flow_sample.quality = sample.sensor_data[5];
 		_flow.setData(flow_sample);
 
@@ -373,9 +373,9 @@ void SensorSimulator::setSensorDataFromTrajectory()
 	if (_flow.isRunning()) {
 		flowSample flow_sample = _flow.dataAtRest();
 		const Vector3f vel_body = R_world_to_body * vel_world;
-		flow_sample.flow_xy_rad =
-			Vector2f(vel_body(1) * flow_sample.dt / distance_to_ground,
-				 -vel_body(0) * flow_sample.dt / distance_to_ground);
+		flow_sample.flow_rate =
+			Vector2f(vel_body(1) / distance_to_ground,
+				 -vel_body(0) / distance_to_ground);
 		_flow.setData(flow_sample);
 	}
 

src/modules/ekf2/test/test_EKF_flow.cpp

@@ -102,9 +102,9 @@ void EkfFlowTest::startZeroFlowFusion()
 void EkfFlowTest::setFlowFromHorizontalVelocityAndDistance(flowSample &flow_sample,
 		const Vector2f &simulated_horz_velocity, float estimated_distance_to_ground)
 {
-	flow_sample.flow_xy_rad =
-		Vector2f(simulated_horz_velocity(1) * flow_sample.dt / estimated_distance_to_ground,
-			 -simulated_horz_velocity(0) * flow_sample.dt / estimated_distance_to_ground);
+	flow_sample.flow_rate =
+		Vector2f(simulated_horz_velocity(1) / estimated_distance_to_ground,
+			 -simulated_horz_velocity(0) / estimated_distance_to_ground);
 }
 
 TEST_F(EkfFlowTest, resetToFlowVelocityInAir)
@@ -140,6 +140,8 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir)
 
 	// THEN: estimated velocity should match simulated velocity
 	const Vector3f estimated_velocity = _ekf->getVelocity();
+	estimated_velocity.print();
+	simulated_velocity.print();
 	EXPECT_TRUE(isEqual(estimated_velocity, simulated_velocity))
 			<< "estimated vel = " << estimated_velocity(0) << ", "
 			<< estimated_velocity(1);
@@ -163,11 +165,11 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround)
 
 	// WHEN: start fusing flow data
 	flowSample flow_sample = _sensor_simulator._flow.dataAtRest();
-	flow_sample.dt = 0.f; // some sensors force dt to zero when quality is low
 	flow_sample.quality = 0;
 	_sensor_simulator._flow.setData(flow_sample);
 	_ekf_wrapper.enableFlowFusion();
 	_sensor_simulator.startFlow();
+	_sensor_simulator.startRangeFinder();
 	_sensor_simulator.runSeconds(1.0);
 
 	// THEN: estimated velocity should match simulated velocity
@@ -248,7 +250,7 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithAutopilotGyroData)
 	setFlowFromHorizontalVelocityAndDistance(flow_sample, simulated_horz_velocity, simulated_distance_to_ground);
 
 	// use autopilot gyro data
-	flow_sample.gyro_xyz.setAll(NAN);
+	flow_sample.gyro_rate.setAll(NAN);
 
 	_sensor_simulator._flow.setData(flow_sample);
 	_sensor_simulator._imu.setGyroData(body_rate);
@@ -286,7 +288,7 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithFlowGyroData)
 
 	// use flow sensor gyro data
 	// for clarification of the sign, see definition of flowSample
-	flow_sample.gyro_xyz = -body_rate * flow_sample.dt;
+	flow_sample.gyro_rate = -body_rate;
 
 	_sensor_simulator._flow.setData(flow_sample);
 	_sensor_simulator._imu.setGyroData(body_rate);

src/modules/ekf2/test/test_EKF_fusionLogic.cpp

@@ -183,6 +183,7 @@ TEST_F(EkfFusionLogicTest, fallbackFromGpsToFlow)
 	const float max_ground_distance = 50.f;
 	_ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance);
 	_sensor_simulator.startFlow();
+	_sensor_simulator.startRangeFinder();
 	_ekf_wrapper.enableFlowFusion();
 
 	_ekf->set_in_air_status(true);

src/modules/ekf2/test/test_EKF_gps.cpp

@@ -192,6 +192,7 @@ TEST_F(EkfGpsTest, gpsHgtToBaroFallback)
 	_sensor_simulator._flow.setData(_sensor_simulator._flow.dataAtRest());
 	_ekf_wrapper.enableFlowFusion();
 	_sensor_simulator.startFlow();
+	_sensor_simulator.startRangeFinder();
 
 	_ekf_wrapper.enableGpsHeightFusion();
 

src/modules/ekf2/test/test_EKF_terrain_estimator.cpp

@@ -98,9 +98,9 @@ public:
 
 		// Configure optical flow simulator data
 		flowSample flow_sample = _sensor_simulator._flow.dataAtRest();
-		flow_sample.flow_xy_rad =
-			Vector2f(simulated_velocity(1) * flow_sample.dt / flow_height,
-				 -simulated_velocity(0) * flow_sample.dt / flow_height);
+		flow_sample.flow_rate =
+			Vector2f(simulated_velocity(1) / flow_height,
+				 -simulated_velocity(0) / flow_height);
 		_sensor_simulator._flow.setData(flow_sample);
 		const float max_flow_rate = 5.f;
 		const float min_ground_distance = 0.f;