From e64c92b322b62b9b190bccfea696984c1cf6ffc0 Mon Sep 17 00:00:00 2001 From: chobits Date: Thu, 4 Jun 2020 10:53:00 +0800 Subject: [PATCH] AP_NavEKF2: use err info from ext nav interface --- libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp | 2 ++ libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 6 +----- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index e7442495f8..9577283c00 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -788,6 +788,7 @@ void NavEKF2_core::fuseEulerYaw() measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination()); } else if (extNavUsedForYaw) { // Get the yaw angle from the external vision data + R_YAW = sq(extNavDataDelayed.angErr); extNavDataDelayed.quat.to_euler(euler321.x, euler321.y, euler321.z); measured_yaw = euler321.z; } else { @@ -847,6 +848,7 @@ void NavEKF2_core::fuseEulerYaw() measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination()); } else if (extNavUsedForYaw) { // Get the yaw angle from the external vision data + R_YAW = sq(extNavDataDelayed.angErr); euler312 = extNavDataDelayed.quat.to_vector312(); measured_yaw = euler312.z; } else { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index c2037f16f8..3109e64e7b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -909,11 +909,7 @@ void NavEKF2_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, extNavDataNew.pos = pos; extNavDataNew.quat = quat; - if (posErr > 0) { - extNavDataNew.posErr = posErr; - } else { - extNavDataNew.posErr = frontend->_gpsHorizPosNoise; - } + extNavDataNew.posErr = posErr; extNavDataNew.angErr = angErr; timeStamp_ms = timeStamp_ms - delay_ms; // Correct for the average intersampling delay due to the filter updaterate