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

ekf2: optical flow remove _flow_data_ready flag

Browse Source
This commit is contained in:
Daniel Agar
2024-07-09 10:10:01 -04:00
parent 7047f9441c
commit 9dfd82ab06
2 changed files with 8 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
+8 -8
View File
@@ -40,12 +40,15 @@
void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
{ {
if (_flow_buffer) { if (!_flow_buffer || (_params.flow_ctrl != 1)) {
_flow_data_ready = _flow_buffer->pop_first_older_than(imu_delayed.time_us, &_flow_sample_delayed); stopFlowFusion();
return;
} }
bool flow_data_ready = false;
// New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon // New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon
if (_flow_data_ready) { if (_flow_buffer->pop_first_older_than(imu_delayed.time_us, &_flow_sample_delayed)) {
// flow gyro has opposite sign convention // flow gyro has opposite sign convention
_ref_body_rate = -(imu_delayed.delta_ang / imu_delayed.delta_ang_dt - getGyroBias()); _ref_body_rate = -(imu_delayed.delta_ang / imu_delayed.delta_ang_dt - getGyroBias());
@@ -82,10 +85,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
&& is_magnitude_good && is_magnitude_good
&& is_tilt_good) { && is_tilt_good) {
flow_data_ready = true;
} else {
// don't use this flow data and wait for the next data to arrive
_flow_data_ready = false;
} }
updateOptFlow(_aid_src_optical_flow, flow_sample); updateOptFlow(_aid_src_optical_flow, flow_sample);
@@ -95,7 +95,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
_flow_rate_compensated = flow_sample.flow_rate - flow_gyro_corrected.xy(); _flow_rate_compensated = flow_sample.flow_rate - flow_gyro_corrected.xy();
} }
if (_flow_data_ready) { if (flow_data_ready) {
// Check if we are in-air and require optical flow to control position drift // Check if we are in-air and require optical flow to control position drift
bool is_flow_required = _control_status.flags.in_air bool is_flow_required = _control_status.flags.in_air
src/modules/ekf2/EKF/ekf.h
-2
View File
@@ -603,8 +603,6 @@ private:
Vector3f _ref_body_rate{}; Vector3f _ref_body_rate{};
Vector2f _flow_rate_compensated{}; ///< measured angular rate of the image about the X and Y body axes after removal of body rotation (rad/s), RH rotation is positive Vector2f _flow_rate_compensated{}; ///< measured angular rate of the image about the X and Y body axes after removal of body rotation (rad/s), RH rotation is positive
bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
#endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_AIRSPEED) #if defined(CONFIG_EKF2_AIRSPEED)