fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 03:23: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/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp
+7 -12
View File
@@ -290,12 +290,12 @@ void VehicleOpticalFlow::Run()
// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate
// is produced by a RH rotation of the image about the sensor axis.
const Vector2f flow_xy_rad{-vehicle_optical_flow.pixel_flow[0], -vehicle_optical_flow.pixel_flow[1]};
const Vector3f gyro_xyz{-vehicle_optical_flow.delta_angle[0], -vehicle_optical_flow.delta_angle[1], -vehicle_optical_flow.delta_angle[2]};
const Vector3f gyro_rate_integral{-vehicle_optical_flow.delta_angle[0], -vehicle_optical_flow.delta_angle[1], -vehicle_optical_flow.delta_angle[2]};
const float flow_dt = 1e-6f * vehicle_optical_flow.integration_timespan_us;
// compensate for body motion to give a LOS rate
const Vector2f flow_compensated_XY_rad = flow_xy_rad - gyro_xyz.xy();
const Vector2f flow_compensated_XY_rad = flow_xy_rad - gyro_rate_integral.xy();
Vector3f vel_optflow_body;
vel_optflow_body(0) = - range * flow_compensated_XY_rad(1) / flow_dt;
@@ -320,24 +320,19 @@ void VehicleOpticalFlow::Run()
flow_vel.vel_ne[1] = flow_vel_ne(1);
}
// flow_uncompensated_integral
flow_xy_rad.copyTo(flow_vel.flow_uncompensated_integral);
const Vector2f flow_rate(flow_xy_rad * (1.f / flow_dt));
flow_rate.copyTo(flow_vel.flow_rate_uncompensated);
// flow_compensated_integral
flow_compensated_XY_rad.copyTo(flow_vel.flow_compensated_integral);
const Vector2f flow_rate_compensated(flow_compensated_XY_rad * (1.f / flow_dt));
flow_rate_compensated.copyTo(flow_vel.flow_rate_compensated);
const Vector3f measured_body_rate(gyro_xyz * (1.f / flow_dt));
const Vector3f measured_body_rate(gyro_rate_integral * (1.f / flow_dt));
// gyro_rate
flow_vel.gyro_rate[0] = measured_body_rate(0);
flow_vel.gyro_rate[1] = measured_body_rate(1);
flow_vel.gyro_rate[2] = measured_body_rate(2);
// gyro_rate_integral
flow_vel.gyro_rate_integral[0] = gyro_xyz(0);
flow_vel.gyro_rate_integral[1] = gyro_xyz(1);
flow_vel.gyro_rate_integral[2] = gyro_xyz(2);
flow_vel.timestamp = hrt_absolute_time();
_vehicle_optical_flow_vel_pub.publish(flow_vel);