AP_NavEKF2: use err info from ext nav interface

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 {

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