AP_NavEKF3: Add push to buffer for external yaw sensor data

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -871,7 +871,20 @@ void NavEKF3_core::readRngBcnData()
 
 void NavEKF3_core::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type)
 {
-//TODO
+    // limit update rate to maximum allowed by sensor buffers and fusion process
+    // don't try to write to buffer until the filter has been initialised
+    if (((timeStamp_ms - yawMeasTime_ms) < frontend->sensorIntervalMin_ms) || !statesInitialised) {
+        return;
+    }
+
+    yawAngDataNew.yawAng = yawAngle;
+    yawAngDataNew.yawAngErr = yawAngleErr;
+    yawAngDataNew.type = type;
+    yawAngDataNew.time_ms = timeStamp_ms;
+
+    storedYawAng.push(yawAngDataNew);
+
+    yawMeasTime_ms = timeStamp_ms;
 }
 
 

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -367,6 +367,11 @@ void NavEKF3_core::InitialiseVariables()
     usingWheelSensors = false;
     wheelOdmMeasTime_ms = 0;
 
+    // yaw sensor fusion
+    yawMeasTime_ms = 0;
+    memset(&yawAngDataNew, 0, sizeof(yawAngDataNew));
+    memset(&yawAngDataDelayed, 0, sizeof(yawAngDataDelayed));
+
     // zero data buffers
     storedIMU.reset();
     storedGPS.reset();

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -1130,6 +1130,7 @@ private:
     wheel_odm_elements wheelOdmDataDelayed;   // Body  frame odometry data at the fusion time horizon
 
     // yaw sensor fusion
+    uint32_t yawMeasTime_ms;
     obs_ring_buffer_t<yaw_elements> storedYawAng;
     yaw_elements yawAngDataNew;
     yaw_elements yawAngDataDelayed;