dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Add EKF2_OF_QMIN_GND to handle 0 optical flow quality when on ground

This commit is contained in:
alexklimaj 2023-06-27 17:07:12 -06:00 committed by Daniel Agar
parent 694501b782
commit 36ec1fa811
5 changed files with 21 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/ekf2/EKF/common.h
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)

7
src/modules/ekf2/EKF/optical_flow_control.cpp
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);

1
src/modules/ekf2/EKF2.cpp
View File

@ -154,6 +154,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_of_n_min(_params->flow_noise),
_param_ekf2_of_n_max(_params->flow_noise_qual_min),
_param_ekf2_of_qmin(_params->flow_qual_min),
_param_ekf2_of_qmin_gnd(_params->flow_qual_min_gnd),
_param_ekf2_of_gate(_params->flow_innov_gate),
_param_ekf2_of_pos_x(_params->flow_pos_body(0)),
_param_ekf2_of_pos_y(_params->flow_pos_body(1)),

4
src/modules/ekf2/EKF2.hpp
View File

@ -662,7 +662,9 @@ private:
(ParamExtFloat<px4::params::EKF2_OF_N_MAX>)
_param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec)
(ParamExtInt<px4::params::EKF2_OF_QMIN>)
_param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor
_param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor when in air
(ParamExtInt<px4::params::EKF2_OF_QMIN_GND>)
_param_ekf2_of_qmin_gnd, ///< minimum acceptable quality integer from the flow sensor when on ground
(ParamExtFloat<px4::params::EKF2_OF_GATE>)
_param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_OF_POS_X>)

11
src/modules/ekf2/ekf2_params.c
View File

@ -901,7 +901,7 @@ PARAM_DEFINE_FLOAT(EKF2_OF_N_MIN, 0.15f);
PARAM_DEFINE_FLOAT(EKF2_OF_N_MAX, 0.5f);
/**
* Optical Flow data will only be used if the sensor reports a quality metric >= EKF2_OF_QMIN.
* Optical Flow data will only be used in air if the sensor reports a quality metric >= EKF2_OF_QMIN.
*
* @group EKF2
* @min 0
@ -909,6 +909,15 @@ PARAM_DEFINE_FLOAT(EKF2_OF_N_MAX, 0.5f);
*/
PARAM_DEFINE_INT32(EKF2_OF_QMIN, 1);
/**
* Optical Flow data will only be used on the ground if the sensor reports a quality metric >= EKF2_OF_QMIN_GND.
*
* @group EKF2
* @min 0
* @max 255
*/
PARAM_DEFINE_INT32(EKF2_OF_QMIN_GND, 0);
/**
* Gate size for optical flow fusion
*