dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Add const modifier and increase matrix library usage

This commit is contained in:
kamilritz 2020-06-21 14:11:12 +02:00 committed by Mathieu Bresciani
parent d9afc2f1e8
commit c2801eb488
1 changed files with 1 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/optflow_fusion.cpp
View File

@ -101,9 +101,7 @@ void Ekf::fuseOptFlow()
// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
// 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) = _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);
const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt + Vector2f(_flow_gyro_bias);
if (opt_flow_rate.norm() < _flow_max_rate) {
_flow_innov[0] = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis