fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 05:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: fix flow gyro bias corrections (#22145)

Browse Source 
* ekf2-flow: fix flow gyro bias compensation
* ekf2-flow: apply flow gyro bias when used
* ekf2: log optical flow gyro bias
* ekf2: optical flow control always use provided flow gyro (with bias applied)
* ekf2-flow: log flow gyro and gyro reference
* ekf2-flow: support senrors with XY flow gyro

---------

Co-authored-by: Daniel Agar <daniel@agar.ca>
Co-authored-by: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)>
This commit is contained in:
Mathieu Bresciani
2023-10-05 10:51:30 -04:00
committed by GitHub
co-authored by Daniel Agar bresch
parent e00b35e142
commit d61743412c
6 changed files with 44 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp
+15 -2
View File
@@ -126,9 +126,20 @@ void VehicleOpticalFlow::Run()
// delta angle
// - from sensor_optical_flow if available, otherwise use synchronized sensor_gyro if available
if (sensor_optical_flow.delta_angle_available && Vector3f(sensor_optical_flow.delta_angle).isAllFinite()) {
if (sensor_optical_flow.delta_angle_available && Vector2f(sensor_optical_flow.delta_angle).isAllFinite()) {
// passthrough integrated gyro if available
_delta_angle += _flow_rotation * Vector3f{sensor_optical_flow.delta_angle};
Vector3f delta_angle(sensor_optical_flow.delta_angle);
if (!PX4_ISFINITE(delta_angle(2))) {
// Some sensors only provide X and Y angular rates, rotate them but place back the NAN on the Z axis
delta_angle(2) = 0.f;
_delta_angle += _flow_rotation * delta_angle;
_delta_angle(2) = NAN;
} else {
_delta_angle += _flow_rotation * delta_angle;
}
_delta_angle_available = true;
} else {
@@ -320,10 +331,12 @@ void VehicleOpticalFlow::Run()
// 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();