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

GPS Yaw: add consts and remove fusion starting message

Browse Source 
The fusion status is captured in the status flags; the message is
superfluous, uses flash space and can impact real-time performance
This commit is contained in:
bresch
2020-06-23 07:43:31 +02:00
committed by Mathieu Bresciani
parent 2bafe9df08
commit 4b746a3fca
2 changed files with 5 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
-3
View File
@@ -534,10 +534,7 @@ void Ekf::controlGpsFusion()
// Activate GPS yaw fusion
if (resetGpsAntYaw()) {
_control_status.flags.yaw_align = true;
startGpsYawFusion();
ECL_INFO_TIMESTAMPED("starting GPS yaw fusion");
}
}
EKF/gps_yaw_fusion.cpp
+5 -11
View File
@@ -53,17 +53,14 @@ void Ekf::fuseGpsAntYaw()
const float q2 = _state.quat_nominal(2);
const float q3 = _state.quat_nominal(3);
float R_YAW = 1.0f;
float predicted_hdg;
float H_YAW[4];
float measured_hdg;
// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
measured_hdg = _gps_sample_delayed.yaw + _gps_yaw_offset;
const float measured_hdg = wrap_pi(_gps_sample_delayed.yaw + _gps_yaw_offset);
// define the predicted antenna array vector and rotate into earth frame
Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) {
@@ -71,7 +68,7 @@ void Ekf::fuseGpsAntYaw()
}
// calculate predicted antenna yaw angle
predicted_hdg = atan2f(ant_vec_ef(1),ant_vec_ef(0));
const float predicted_hdg = atan2f(ant_vec_ef(1),ant_vec_ef(0));
// calculate observation jacobian
float t2 = sinf(_gps_yaw_offset);
@@ -126,10 +123,7 @@ void Ekf::fuseGpsAntYaw()
H_YAW[3] = t30*(t26*t32+t16*t22*t35);
// using magnetic heading tuning parameter
R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
// wrap the heading to the interval between +-pi
measured_hdg = wrap_pi(measured_hdg);
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);