fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 15:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: move flow fusion activation from AID_MASK to OF_CTRL

Browse Source
This commit is contained in:
bresch
2023-04-26 14:32:53 +02:00
committed by Daniel Agar
parent 39c2f95669
commit 2a2e43b620
7 changed files with 45 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/common.h
+2 -1
View File
@@ -158,7 +158,7 @@ enum class EvCtrl : uint8_t {
enum SensorFusionMask : uint16_t {
// Bit locations for fusion_mode
DEPRECATED_USE_GPS = (1<<0), ///< set to true to use GPS data (DEPRECATED, use gnss_ctrl)
USE_OPT_FLOW = (1<<1), ///< set to true to use optical flow data
DEPRECATED_USE_OPT_FLOW = (1<<1), ///< set to true to use optical flow data
DEPRECATED_INHIBIT_ACC_BIAS = (1<<2), ///< set to true to inhibit estimation of accelerometer delta velocity bias
DEPRECATED_USE_EXT_VIS_POS = (1<<3), ///< set to true to use external vision position data
DEPRECATED_USE_EXT_VIS_YAW = (1<<4), ///< set to true to use external vision quaternion data for yaw
@@ -423,6 +423,7 @@ struct parameters {
float gravity_noise{1.0f}; ///< accelerometer measurement gaussian noise (m/s**2)
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
int32_t flow_ctrl{0};
float flow_delay_ms{5.0f}; ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
// optical flow fusion
src/modules/ekf2/EKF/estimator_interface.cpp
+1 -1
View File
@@ -569,7 +569,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
}
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
if (_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) {
if (_params.flow_ctrl > 0) {
max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
}
#endif // CONFIG_EKF2_OPTICAL_FLOW
src/modules/ekf2/EKF/optical_flow_control.cpp
+2 -2
View File
@@ -158,7 +158,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
// Handle cases where we are using optical flow but we should not use it anymore
if (_control_status.flags.opt_flow) {
if (!(_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW)
if (!(_params.flow_ctrl == 1)
|| inhibit_flow_use) {
stopFlowFusion();
@@ -167,7 +167,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
}
// optical flow fusion mode selection logic
if ((_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) // optical flow has been selected by the user
if ((_params.flow_ctrl == 1) // optical flow has been selected by the user
&& !_control_status.flags.opt_flow // we are not yet using flow data
&& !inhibit_flow_use) {