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

Update optical flow interface

Browse Source
This commit is contained in:
kamilritz
2020-01-23 17:46:21 +01:00
committed by Roman Bapst
parent cafb1400fb
commit f3d790a664
12 changed files with 46 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/optflow_fusion.cpp
+7 -7
View File
@@ -73,8 +73,8 @@ void Ekf::fuseOptFlow()
const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
// calculate the velocity of the sensor relative to the imu in body frame
// Note: _flow_sample_delayed.gyroXYZ is the negative of the body angular velocity, thus use minus sign
const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt) % pos_offset_body;
// Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign
const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body;
// calculate the velocity of the sensor in the earth frame
const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;
@@ -102,8 +102,8 @@ void Ekf::fuseOptFlow()
// correct for gyro bias errors in the data used to do the motion compensation
// Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
Vector2f opt_flow_rate;
opt_flow_rate(0) = _flowRadXYcomp(0) / _flow_sample_delayed.dt + _flow_gyro_bias(0);
opt_flow_rate(1) = _flowRadXYcomp(1) / _flow_sample_delayed.dt + _flow_gyro_bias(1);
opt_flow_rate(0) = _flow_compensated_XY_rad(0) / _flow_sample_delayed.dt + _flow_gyro_bias(0);
opt_flow_rate(1) = _flow_compensated_XY_rad(1) / _flow_sample_delayed.dt + _flow_gyro_bias(1);
if (opt_flow_rate.norm() < _flow_max_rate) {
_flow_innov[0] = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis
@@ -522,7 +522,7 @@ bool Ekf::calcOptFlowBodyRateComp()
return false;
}
const bool use_flow_sensor_gyro = ISFINITE(_flow_sample_delayed.gyroXYZ(0)) && ISFINITE(_flow_sample_delayed.gyroXYZ(1)) && ISFINITE(_flow_sample_delayed.gyroXYZ(2));
const bool use_flow_sensor_gyro = ISFINITE(_flow_sample_delayed.gyro_xyz(0)) && ISFINITE(_flow_sample_delayed.gyro_xyz(1)) && ISFINITE(_flow_sample_delayed.gyro_xyz(2));
if (use_flow_sensor_gyro) {
@@ -532,7 +532,7 @@ bool Ekf::calcOptFlowBodyRateComp()
const Vector3f reference_body_rate(_imu_del_ang_of * (1.0f / _delta_time_of));
const Vector3f measured_body_rate(_flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt));
const Vector3f measured_body_rate(_flow_sample_delayed.gyro_xyz * (1.0f / _flow_sample_delayed.dt));
// 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;
@@ -541,7 +541,7 @@ bool Ekf::calcOptFlowBodyRateComp()
} 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.gyroXYZ = -_imu_del_ang_of;
_flow_sample_delayed.gyro_xyz = -_imu_del_ang_of;
_flow_gyro_bias.zero();
}