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

Add EKF2_OF_QMIN_GND to handle 0 optical flow quality when on ground

Browse Source
This commit is contained in:
alexklimaj
2023-07-11 08:18:06 -07:00
committed by Ramon Roche
parent bca0be47bc
commit 03528a6200
5 changed files with 21 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/common.h
+1
View File
@@ -416,6 +416,7 @@ struct parameters {
float flow_noise{0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec)
float flow_noise_qual_min{0.5f}; ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
int32_t flow_qual_min{1}; ///< minimum acceptable quality integer from the flow sensor
int32_t flow_qual_min_gnd{0}; ///< minimum acceptable quality integer from the flow sensor when on ground
float flow_innov_gate{3.0f}; ///< optical flow fusion innovation consistency gate size (STD)
Vector3f flow_pos_body{}; ///< xyz position of range sensor focal point in body frame (m)
src/modules/ekf2/EKF/optical_flow_control.cpp
+6 -1
View File
@@ -70,7 +70,12 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
}
if (_flow_data_ready) {
const bool is_quality_good = (_flow_sample_delayed.quality >= _params.flow_qual_min);
int32_t min_quality = _params.flow_qual_min;
if (!_control_status.flags.in_air) {
min_quality = _params.flow_qual_min_gnd;
}
const bool is_quality_good = (_flow_sample_delayed.quality >= min_quality);
const bool is_magnitude_good = !_flow_sample_delayed.flow_xy_rad.longerThan(_flow_sample_delayed.dt * _flow_max_rate);
const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt);