fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-06-26 16:21:17 -04:00
parent 7047f9441c
commit 9dfd82ab06
2 changed files with 8 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
View File

@ -40,12 +40,15 @@
void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
{
if (_flow_buffer) {
_flow_data_ready = _flow_buffer->pop_first_older_than(imu_delayed.time_us, &_flow_sample_delayed);
if (!_flow_buffer || (_params.flow_ctrl != 1)) {
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
if (_flow_data_ready) {
if (_flow_buffer->pop_first_older_than(imu_delayed.time_us, &_flow_sample_delayed)) {
// flow gyro has opposite sign convention
_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_tilt_good) {
} else {
// don't use this flow data and wait for the next data to arrive
_flow_data_ready = false;
flow_data_ready = true;
}
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();
}
if (_flow_data_ready) {
if (flow_data_ready) {
// Check if we are in-air and require optical flow to control position drift
bool is_flow_required = _control_status.flags.in_air

2
src/modules/ekf2/EKF/ekf.h
View File

@ -603,8 +603,6 @@ private:
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
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
#if defined(CONFIG_EKF2_AIRSPEED)