fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 22:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2-flow: refactor control logic

Browse Source 
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.
This commit is contained in:
bresch
2023-11-21 21:56:34 -05:00
committed by Daniel Agar
parent 5f8ecd6b40
commit 1c3a1183c8
15 changed files with 201 additions and 312 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/test_EKF_flow.cpp
+8 -6
View File
@@ -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);