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

Add const modifier

Browse Source
This commit is contained in:
kamilritz 2020-06-21 13:56:23 +02:00 committed by Mathieu Bresciani
parent 48f0eb16da
commit c3653e68ca
1 changed files with 2 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
EKF/gps_yaw_fusion.cpp
View File

@ -53,8 +53,6 @@ void Ekf::fuseGpsAntYaw()
const float q2 = _state.quat_nominal(2);
const float q3 = _state.quat_nominal(3);
float H_YAW[4];
// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
const float measured_hdg = wrap_pi(_gps_sample_delayed.yaw + _gps_yaw_offset);
@ -117,6 +115,7 @@ void Ekf::fuseGpsAntYaw()
float t34 = q0*t2*2.0f;
float t35 = t33+t34;
float H_YAW[4];
H_YAW[0] = (t35/(t11-t2*(t4-q1*q2*2.0f))-t16*t18*t32)/(t18*t21+1.0f);
H_YAW[1] = -t30*(t26*(t27-q2*t3*2.0f)+t16*t22*t25);
H_YAW[2] = t30*(t25*t26-t16*t22*(t27-q2*t3*2.0f));
@ -126,7 +125,7 @@ void Ekf::fuseGpsAntYaw()
const float R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
// calculate the innovation and define the innovation gate
float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
const float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
_heading_innov = predicted_hdg - measured_hdg;
// wrap the innovation to the interval between +-pi