Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

ekf2: improve optical flow angular rate compensation

Browse Source
This commit is contained in:
Mathieu Bresciani
2021-10-12 19:17:29 +02:00
committed by GitHub
parent fab053d33b
commit 56b0c46444
2 changed files with 12 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+1 -1
View File
@@ -388,7 +388,7 @@ void Ekf::controlOpticalFlowFusion()
// compensate for body motion to give a LOS rate
_flow_compensated_XY_rad = _flow_sample_delayed.flow_xy_rad - _flow_sample_delayed.gyro_xyz.xy();
} else if (!_control_status.flags.in_air && is_body_rate_comp_available) {
} else if (!_control_status.flags.in_air) {
if (!is_delta_time_good) {
// handle special case of SITL and PX4Flow where dt is forced to
src/modules/ekf2/EKF/optflow_fusion.cpp
+11 -3
View File
@@ -340,6 +340,7 @@ bool Ekf::calcOptFlowBodyRateComp()
return false;
}
bool is_body_rate_comp_available = false;
const bool use_flow_sensor_gyro = PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2));
if (use_flow_sensor_gyro) {
@@ -356,19 +357,26 @@ bool Ekf::calcOptFlowBodyRateComp()
// calculate the bias estimate using a combined LPF and spike filter
_flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(measured_body_rate - reference_body_rate, -0.1f, 0.1f) * 0.01f;
is_body_rate_comp_available = true;
}
} else {
// Use the EKF gyro data if optical flow sensor gyro data is not available
// for clarification of the sign see definition of flowSample and imuSample in common.h
_flow_sample_delayed.gyro_xyz = -_imu_del_ang_of;
_flow_gyro_bias.zero();
if ((_delta_time_of > FLT_EPSILON)
&& (_flow_sample_delayed.dt > FLT_EPSILON)) {
_flow_sample_delayed.gyro_xyz = -_imu_del_ang_of / _delta_time_of * _flow_sample_delayed.dt;
_flow_gyro_bias.zero();
is_body_rate_comp_available = true;
}
}
// reset the accumulators
_imu_del_ang_of.setZero();
_delta_time_of = 0.0f;
return true;
return is_body_rate_comp_available;
}
// calculate the measurement variance for the optical flow sensor (rad/sec)^2