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

ekf2: fully disable yaw estimator EKFGSF_yaw with CONFIG_EKF2_GNSS (#22233)

Browse Source
This commit is contained in:
Daniel Agar
2023-10-18 13:30:17 -04:00
committed by GitHub
parent fefdad83bf
commit bdaf0acfca
7 changed files with 94 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_control.cpp
+5
View File
@@ -155,7 +155,9 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
* Vector3f(_mag_strength_gps, 0, 0);
// mag_B: reset
#if defined(CONFIG_EKF2_GNSS)
if (isYawEmergencyEstimateAvailable()) {
const Dcmf R_to_earth = updateYawInRotMat(_yawEstimator.getYaw(), _R_to_earth);
const Dcmf R_to_body = R_to_earth.transpose();
@@ -165,6 +167,9 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
ECL_INFO("resetMagStates using yaw estimator");
} else if (!reset_heading && _control_status.flags.yaw_align) {
#else
if (!reset_heading && _control_status.flags.yaw_align) {
#endif
// mag_B: reset using WMM
const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
_state.mag_B = mag - (R_to_body * mag_earth_pred);