fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 09:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: pass gpsSample around where required

Browse Source 
- this minimizes potential misuse accessing _gps_sample_delayed and
makes the dependency clear
This commit is contained in:
Daniel Agar
2022-09-05 10:27:19 -04:00
parent 0c860fa227
commit d996af4647
9 changed files with 75 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/gps_yaw_fusion.cpp
+5 -5
View File
@@ -45,7 +45,7 @@
#include <mathlib/mathlib.h>
#include <cstdlib>
void Ekf::fuseGpsYaw()
void Ekf::fuseGpsYaw(const gpsSample& gps_sample)
{
// assign intermediate state variables
const float q0 = _state.quat_nominal(0);
@@ -54,7 +54,7 @@ void Ekf::fuseGpsYaw()
const float q3 = _state.quat_nominal(3);
// 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.yaw + _gps_yaw_offset);
// define the predicted antenna array vector and rotate into earth frame
const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
@@ -70,7 +70,7 @@ void Ekf::fuseGpsYaw()
// TODO extend interface to use yaw uncertainty provided by GPS if available
const float R_YAW = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f));
_aid_src_gnss_yaw.timestamp_sample = _gps_sample_delayed.time_us;
_aid_src_gnss_yaw.timestamp_sample = gps_sample.time_us;
_aid_src_gnss_yaw.observation = measured_hdg;
_aid_src_gnss_yaw.observation_variance = R_YAW;
_aid_src_gnss_yaw.innovation = heading_innov;
@@ -201,7 +201,7 @@ void Ekf::fuseGpsYaw()
}
}
bool Ekf::resetYawToGps()
bool Ekf::resetYawToGps(const float gnss_yaw)
{
// define the predicted antenna array vector and rotate into earth frame
const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
@@ -213,7 +213,7 @@ bool Ekf::resetYawToGps()
}
// GPS yaw measurement is alreday compensated for antenna offset in the driver
const float measured_yaw = _gps_sample_delayed.yaw;
const float measured_yaw = gnss_yaw;
const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f));
resetQuatStateYaw(measured_yaw, yaw_variance);