fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 23:37:35 +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
Download Patch File Download Diff File Expand all files Collapse all files
EKF/gps_yaw_fusion.cpp
+2 -3
View File
@@ -53,8 +53,6 @@ void Ekf::fuseGpsAntYaw()
const float q2 = _state.quat_nominal(2); const float q2 = _state.quat_nominal(2);
const float q3 = _state.quat_nominal(3); 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 // 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); 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 t34 = q0*t2*2.0f;
float t35 = t33+t34; 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[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[1] = -t30*(t26*(t27-q2*t3*2.0f)+t16*t22*t25);
H_YAW[2] = t30*(t25*t26-t16*t22*(t27-q2*t3*2.0f)); 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)); const float R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
// calculate the innovation and define the innovation gate // 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; _heading_innov = predicted_hdg - measured_hdg;
// wrap the innovation to the interval between +-pi // wrap the innovation to the interval between +-pi