AP_NavEKF: use published delta velocities and delta angles if available

3drobotics · Feb 25, 2015 · bd5dadb · bd5dadb
1 parent 8b2f4a8
commit bd5dadb
Show file tree

Hide file tree

Showing 2 changed files with 43 additions and 31 deletions.
diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -1110,7 +1110,11 @@ void NavEKF::UpdateStrapdownEquationsNED()
 
     // apply corrections for earths rotation rate and coning errors
     // % * - and + operators have been overloaded
-    correctedDelAng   = correctedDelAng - prevTnb * earthRateNED*dtIMU + (prevDelAng % correctedDelAng) * 8.333333e-2f;
+    if (haveDeltaAngles) {
+        correctedDelAng   = correctedDelAng - prevTnb * earthRateNED*dtIMU;
+    } else {
+        correctedDelAng   = correctedDelAng - prevTnb * earthRateNED*dtIMU + (prevDelAng % correctedDelAng) * 8.333333e-2f;
+    }
 
     // save current measurements
     prevDelAng = correctedDelAng;
@@ -3925,45 +3929,50 @@ void NavEKF::ConstrainStates()
 // update IMU delta angle and delta velocity measurements
 void NavEKF::readIMUData()
 {
-    Vector3f angRate;   // angular rate vector in XYZ body axes measured by the IMU (rad/s)
-    Vector3f accel1;    // acceleration vector in XYZ body axes measured by IMU1 (m/s^2)
-    Vector3f accel2;    // acceleration vector in XYZ body axes measured by IMU2 (m/s^2)
+    const AP_InertialSensor &ins = _ahrs->get_ins();
+
+    // limit IMU delta time to prevent numerical problems elsewhere
+    dtIMU = constrain_float(ins.get_delta_time(), 0.001f, 1.0f);
 
     // the imu sample time is sued as a common time reference throughout the filter
     imuSampleTime_ms = hal.scheduler->millis();
 
-    // limit IMU delta time to prevent numerical problems elsewhere
-    dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(), 0.001f, 1.0f);
+    bool dual_ins = ins.get_accel_health(0) && ins.get_accel_health(1);
+    haveDeltaAngles = true;
 
-    // get accel and gyro data from dual sensors if healthy
-    if (_ahrs->get_ins().get_accel_health(0) && _ahrs->get_ins().get_accel_health(1)) {
-        accel1 = _ahrs->get_ins().get_accel(0);
-        accel2 = _ahrs->get_ins().get_accel(1);
+    if (dual_ins) {
+        Vector3f dAngIMU1;
+        Vector3f dAngIMU2;
+
+        if(!ins.get_delta_velocity(0,dVelIMU1)) {
+            dVelIMU1 = ins.get_accel(0) * dtIMU;
+        }
+
+        if(!ins.get_delta_velocity(1,dVelIMU2)) {
+            dVelIMU2 = ins.get_accel(1) * dtIMU;
+        }
+
+        if(!ins.get_delta_angle(0, dAngIMU1)) {
+            haveDeltaAngles = false;
+            dAngIMU1 = ins.get_gyro(0) * dtIMU;
+        }
+
+        if(!ins.get_delta_angle(1, dAngIMU2)) {
+            haveDeltaAngles = false;
+            dAngIMU2 = ins.get_gyro(1) * dtIMU;
+        }
+        dAngIMU = (dAngIMU1+dAngIMU2) * 0.5f;
     } else {
-        accel1 = _ahrs->get_ins().get_accel();
-        accel2 = accel1;
-    }
+        if(!ins.get_delta_velocity(dVelIMU1)) {
+            dVelIMU1 = ins.get_accel() * dtIMU;
+        }
+        dVelIMU2 = dVelIMU1;
 
-    // average the available gyro sensors
-    angRate.zero();
-    uint8_t gyro_count = 0;
-    for (uint8_t i = 0; i<_ahrs->get_ins().get_gyro_count(); i++) {
-        if (_ahrs->get_ins().get_gyro_health(i)) {
-            angRate += _ahrs->get_ins().get_gyro(i);
-            gyro_count++;
+        if(!ins.get_delta_angle(dAngIMU)) {
+            haveDeltaAngles = false;
+            dAngIMU = ins.get_gyro() * dtIMU;
         }
     }
-    if (gyro_count != 0) {
-        angRate /= gyro_count;
-    }
-
-    // trapezoidal integration
-    dAngIMU     = (angRate + lastAngRate) * dtIMU * 0.5f;
-    lastAngRate = angRate;
-    dVelIMU1    = (accel1 + lastAccel1) * dtIMU * 0.5f;
-    lastAccel1  = accel1;
-    dVelIMU2    = (accel2 + lastAccel2) * dtIMU * 0.5f;
-    lastAccel2  = accel2;
 }
 
 // check for new valid GPS data and update stored measurement if available
@@ -4427,6 +4436,7 @@ void NavEKF::InitialiseVariables()
     inhibitMagStates = true;
     gndOffsetValid =  false;
     flowXfailed = false;
+    haveDeltaAngles = false;
     validOrigin = false;
 }
 

diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -682,6 +682,8 @@ class NavEKF
     bool gndOffsetValid;            // true when the ground offset state can still be considered valid
     bool flowXfailed;               // true when the X optical flow measurement has failed the innovation consistency check
 
+    bool haveDeltaAngles;
+
     // states held by optical flow fusion across time steps
     // optical flow X,Y motion compensated rate measurements are fused across two time steps
     // to level computational load as this can be an expensive operation