AP_NavEKF2: make IMU buffer length depend on main loop rate

this fixes a problem with Replay and baro data

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -392,14 +392,14 @@ void NavEKF2_core::StoreIMU()
 {
     // increment the index and write new data
     fifoIndexNow = fifoIndexNow + 1;
-    if (fifoIndexNow >= IMU_BUFFER_LENGTH) {
+    if (fifoIndexNow >= imu_buffer_length) {
         fifoIndexNow = 0;
     }
     storedIMU[fifoIndexNow] = imuDataDownSampledNew;
     // set the index required to access the oldest data, applying an offset to the fusion time horizon that is used to
     // prevent the same fusion operation being performed on the same frame across multiple EKF's
     fifoIndexDelayed = fifoIndexNow + 1;
-    if (fifoIndexDelayed >= IMU_BUFFER_LENGTH) {
+    if (fifoIndexDelayed >= imu_buffer_length) {
         fifoIndexDelayed = 0;
     }
 }
@@ -408,12 +408,12 @@ void NavEKF2_core::StoreIMU()
 void NavEKF2_core::StoreIMU_reset()
 {
     // write current measurement to entire table
-    for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
+    for (uint8_t i=0; i<imu_buffer_length; i++) {
         storedIMU[i] = imuDataNew;
     }
     imuDataDelayed = imuDataNew;
     fifoIndexDelayed = fifoIndexNow+1;
-    if (fifoIndexDelayed >= IMU_BUFFER_LENGTH) {
+    if (fifoIndexDelayed >= imu_buffer_length) {
         fifoIndexDelayed = 0;
     }
 }

libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -32,7 +32,7 @@ void NavEKF2_core::ResetVelocity(void)
         stateStruct.velocity.x  = gpsDataNew.vel.x; // north velocity from blended accel data
         stateStruct.velocity.y  = gpsDataNew.vel.y; // east velocity from blended accel data
     }
-    for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
+    for (uint8_t i=0; i<imu_buffer_length; i++) {
         storedOutput[i].velocity.x = stateStruct.velocity.x;
         storedOutput[i].velocity.y = stateStruct.velocity.y;
     }
@@ -65,7 +65,7 @@ void NavEKF2_core::ResetPosition(void)
         stateStruct.position.x = gpsDataNew.pos.x  + 0.001f*gpsDataNew.vel.x*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));
         stateStruct.position.y = gpsDataNew.pos.y  + 0.001f*gpsDataNew.vel.y*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));
     }
-    for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
+    for (uint8_t i=0; i<imu_buffer_length; i++) {
         storedOutput[i].position.x = stateStruct.position.x;
         storedOutput[i].position.y = stateStruct.position.y;
     }
@@ -90,7 +90,7 @@ void NavEKF2_core::ResetHeight(void)
     // write to the state vector
     stateStruct.position.z = -baroDataNew.hgt; // down position from blended accel data
     terrainState = stateStruct.position.z + rngOnGnd;
-    for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
+    for (uint8_t i=0; i<imu_buffer_length; i++) {
         storedOutput[i].position.z = stateStruct.position.z;
     }
     outputDataNew.position.z = stateStruct.position.z;

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -61,6 +61,27 @@ void NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _c
     imu_index = _imu_index;
     core_index = _core_index;
     _ahrs = frontend->_ahrs;
+
+    /*
+      the imu_buffer_length needs to cope with a 260ms delay at a
+      maximum fusion rate of 100Hz. Non-imu data coming in at faster
+      than 100Hz is downsampled. For 50Hz main loop rate we need a
+      shorter buffer.
+     */
+    switch (_ahrs->get_ins().get_sample_rate()) {
+    case AP_InertialSensor::RATE_50HZ:
+        imu_buffer_length = 13;
+        break;
+    case AP_InertialSensor::RATE_100HZ:
+    case AP_InertialSensor::RATE_200HZ:
+    case AP_InertialSensor::RATE_400HZ:
+        // maximum 260 msec delay at 100 Hz fusion rate
+        imu_buffer_length = 26;
+        break;
+    }
+
+    storedIMU    = new imu_elements[imu_buffer_length];
+    storedOutput = new output_elements[imu_buffer_length];
 }
     
 
@@ -1128,7 +1149,7 @@ void NavEKF2_core::StoreOutputReset()
     outputDataNew.velocity = stateStruct.velocity;
     outputDataNew.position = stateStruct.position;
     // write current measurement to entire table
-    for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
+    for (uint8_t i=0; i<imu_buffer_length; i++) {
         storedOutput[i] = outputDataNew;
     }
     outputDataDelayed = outputDataNew;
@@ -1142,7 +1163,7 @@ void NavEKF2_core::StoreQuatReset()
 {
     outputDataNew.quat = stateStruct.quat;
     // write current measurement to entire table
-    for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
+    for (uint8_t i=0; i<imu_buffer_length; i++) {
         storedOutput[i].quat = outputDataNew.quat;
     }
     outputDataDelayed.quat = outputDataNew.quat;
@@ -1153,7 +1174,7 @@ void NavEKF2_core::StoreQuatRotate(Quaternion deltaQuat)
 {
     outputDataNew.quat = outputDataNew.quat*deltaQuat;
     // write current measurement to entire table
-    for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
+    for (uint8_t i=0; i<imu_buffer_length; i++) {
         storedOutput[i].quat = storedOutput[i].quat*deltaQuat;
     }
     outputDataDelayed.quat = outputDataDelayed.quat*deltaQuat;

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -42,23 +42,6 @@
 #define MASK_GPS_VERT_SPD   (1<<6)
 #define MASK_GPS_HORIZ_SPD  (1<<7)
 
-/*
- * IMU FIFO buffer length depends on the IMU update rate being used and the maximum sensor delay
- * Samples*delta_time must be > max sensor delay
-*/
-#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
-// Note that if using more than 2 instances of the EKF, as set by EK2_IMU_MASK, this delay should be increased by 2 samples
-// for each additional instance to allow for the need to offset the fusion time horizon for each instance to avoid simultaneous fusion
-// of measurements by each instance
-#define IMU_BUFFER_LENGTH       26 // maximum 260 msec delay at 100 Hz fusion rate
-#elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
-#define IMU_BUFFER_LENGTH       13 // maximum 260 msec delay at 50 Hz
-#elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
-#define IMU_BUFFER_LENGTH       13 // maximum 260 msec delay at 50 Hz
-#else
-#define IMU_BUFFER_LENGTH       104 // unknown so use max buffer length
-#endif
-
 class AP_AHRS;
 
 class NavEKF2_core
@@ -282,6 +265,7 @@ private:
     NavEKF2 *frontend;
     uint8_t imu_index;
     uint8_t core_index;
+    uint8_t imu_buffer_length;
 
     typedef float ftype;
 #if defined(MATH_CHECK_INDEXES) && (MATH_CHECK_INDEXES == 1)
@@ -657,12 +641,12 @@ private:
     Matrix24 KH;                    // intermediate result used for covariance updates
     Matrix24 KHP;                   // intermediate result used for covariance updates
     Matrix24 P;                     // covariance matrix
-    imu_elements storedIMU[IMU_BUFFER_LENGTH];      // IMU data buffer
+    imu_elements *storedIMU;        // IMU data buffer [imu_buffer_length]
     gps_elements storedGPS[OBS_BUFFER_LENGTH];      // GPS data buffer
     mag_elements storedMag[OBS_BUFFER_LENGTH];      // Magnetometer data buffer
     baro_elements storedBaro[OBS_BUFFER_LENGTH];    // Baro data buffer
     tas_elements storedTAS[OBS_BUFFER_LENGTH];      // TAS data buffer
-    output_elements storedOutput[IMU_BUFFER_LENGTH];// output state buffer
+    output_elements *storedOutput;  // output state buffer [imu_buffer_length]
     Vector3f correctedDelAng;       // delta angles about the xyz body axes corrected for errors (rad)
     Quaternion correctedDelAngQuat; // quaternion representation of correctedDelAng
     Vector3f correctedDelVel;       // delta velocities along the XYZ body axes for weighted average of IMU1 and IMU2 corrected for errors (m/s)