fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-07 04:11:35 +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/ekf2/EKF/ekf.h
+5
View File
@@ -180,6 +180,9 @@ public:
const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); }
const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; }
const Vector3f &getFlowGyroBias() const { return _flow_gyro_bias; }
const Vector3f &getRefBodyRate() const { return _ref_body_rate; }
const Vector3f &getMeasuredBodyRate() const { return _measured_body_rate; }
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_AUXVEL)
@@ -656,6 +659,8 @@ private:
Vector2f _flow_vel_body{}; ///< velocity from corrected flow measurement (body frame)(m/s)
Vector2f _flow_vel_ne{}; ///< velocity from corrected flow measurement (local frame) (m/s)
Vector3f _imu_del_ang_of{}; ///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad)
Vector3f _ref_body_rate{};
Vector3f _measured_body_rate{};
float _delta_time_of{0.0f}; ///< time in sec that _imu_del_ang_of was accumulated over (sec)
uint64_t _time_bad_motion_us{0}; ///< last system time that on-ground motion exceeded limits (uSec)