From c49ab760c78fc6dc4cb174d6f9b7b39b01e19a7b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 20 Jun 2018 14:06:41 +1000 Subject: [PATCH] EKF: Allow EKF to start without using mag data when not using earth frame data --- EKF/ekf_helper.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 0bdfed2892..3ddd7fdaba 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -595,6 +595,10 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // the angle of the projection onto the horizontal gives the yaw angle euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; + } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) { + // we are operating without knowing the earth frame yaw angle + return true; + } else { // there is no yaw observation return false; @@ -649,6 +653,10 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // the angle of the projection onto the horizontal gives the yaw angle euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; + } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) { + // we are operating without knowing the earth frame yaw angle + return true; + } else { // there is no yaw observation return false;