imu changes to allow for GPS rescue mode & fix home error

src/main/flight/imu.c

@@ -81,12 +81,14 @@ static bool imuUpdated = false;
 #define ATTITUDE_RESET_GYRO_LIMIT 15       // 15 deg/sec - gyro limit for quiet period
 #define ATTITUDE_RESET_KP_GAIN    25.0     // dcmKpGain value to use during attitude reset
 #define ATTITUDE_RESET_ACTIVE_TIME 500000  // 500ms - Time to wait for attitude to converge at high gain
+#define GPS_COG_MIN_GROUNDSPEED 500        // 500cm/s minimum groundspeed for a gps heading to be considered valid
 
 int32_t accSum[XYZ_AXIS_COUNT];
 
 uint32_t accTimeSum = 0;        // keep track for integration of acc
 int accSumCount = 0;
 float accVelScale;
+bool canUseGPSHeading = true;
 
 static float throttleAngleScale;
 static float fc_acc;
@@ -166,6 +168,12 @@ void imuInit(void)
     smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle));
     accVelScale = 9.80665f / acc.dev.acc_1G / 10000.0f;
 
+#ifdef USE_GPS
+    canUseGPSHeading = true;
+#else
+    canUseGPSHeading = false;
+#endif
+
     imuComputeRotationMatrix();
 
 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
@@ -244,7 +252,7 @@ static float invSqrt(float x)
 static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
                                 bool useAcc, float ax, float ay, float az,
                                 bool useMag, float mx, float my, float mz,
-                                bool useYaw, float yawError, const float dcmKpGain)
+                                bool useCOG, float courseOverGround, const float dcmKpGain)
 {
     static float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;    // integral error terms scaled by Ki
 
@@ -253,10 +261,15 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
 
     // Use raw heading error (from GPS or whatever else)
     float ex = 0, ey = 0, ez = 0;
-    if (useYaw) {
+    if (useCOG) {
-        while (yawError >  M_PIf) yawError -= (2.0f * M_PIf);
+        while (courseOverGround >  M_PIf) courseOverGround -= (2.0f * M_PIf);
-        while (yawError < -M_PIf) yawError += (2.0f * M_PIf);
+        while (courseOverGround < -M_PIf) courseOverGround += (2.0f * M_PIf);
-        ez += sin_approx(yawError / 2.0f);
+
+        const float ez_ef = (- sin_approx(courseOverGround) * rMat[0][0] - cos_approx(courseOverGround) * rMat[1][0]);
+
+        ex = rMat[2][0] * ez_ef;
+        ey = rMat[2][1] * ez_ef;
+        ez = rMat[2][2] * ez_ef;
     }
 
 #ifdef USE_MAG
@@ -464,10 +477,10 @@ float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAverage)
 static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
 {
     static timeUs_t previousIMUUpdateTime;
-    float rawYawError = 0;
     bool useAcc = false;
     bool useMag = false;
-    bool useYaw = false;
+    bool useCOG = false; // Whether or not correct yaw via imuMahonyAHRSupdate from our ground course
+    float courseOverGround = 0; // To be used when useCOG is true.  Stored in Radians
 
     const timeDelta_t deltaT = currentTimeUs - previousIMUUpdateTime;
     previousIMUUpdateTime = currentTimeUs;
@@ -478,10 +491,29 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
     }
 #endif
 #if defined(USE_GPS)
-    if (!useMag && STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= 300) {
+    if (!useMag && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= GPS_COG_MIN_GROUNDSPEED) {
-        // In case of a fixed-wing aircraft we can use GPS course over ground to correct heading
+        // Use GPS course over ground to correct attitude.values.yaw
-        rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - gpsSol.groundCourse);
+        if (STATE(FIXED_WING)) {
-        useYaw = true;
+            courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
+            useCOG = true;
+        } else {
+            // If GPS rescue mode is active and we can use it, go for it.  When we're close to home we will 
+            // probably stop re calculating GPS heading data.  Other future modes can also use this extern
+
+            if (canUseGPSHeading) {
+                courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
+
+                useCOG = true;
+            }
+        }
+
+        if (useCOG && shouldInitializeGPSHeading()) {
+            // Reset our reference and reinitialize quaternion.  This will likely ideally happen more than once per flight, but for now,
+            // shouldInitializeGPSHeading() returns true only once.
+            imuComputeQuaternionFromRPY(&qP, attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
+
+            useCOG = false; // Don't use the COG when we first reinitialize.  Next time around though, yes.
+        }
     }
 #endif
 
@@ -490,8 +522,8 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
     UNUSED(imuIsAccelerometerHealthy);
     UNUSED(useAcc);
     UNUSED(useMag);
-    UNUSED(useYaw);
+    UNUSED(useCOG);
-    UNUSED(rawYawError);
+    UNUSED(canUseGPSHeading);
 #else
 
 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
@@ -509,7 +541,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
                         DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]),
                         useAcc, accAverage[X], accAverage[Y], accAverage[Z],
                         useMag, mag.magADC[X], mag.magADC[Y], mag.magADC[Z],
-                        useYaw, rawYawError, imuCalcKpGain(currentTimeUs, useAcc, gyroAverage));
+                        useCOG, courseOverGround,  imuCalcKpGain(currentTimeUs, useAcc, gyroAverage));
 
     imuUpdateEulerAngles();
 #endif
@@ -538,11 +570,32 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
     }
 }
 
+bool shouldInitializeGPSHeading()
+{
+    static bool initialized = false;
+
+    if (!initialized) {
+        initialized = true;
+
+        return true;
+    }
+    
+    return false;
+}
+
 float getCosTiltAngle(void)
 {
     return rMat[2][2];
 }
 
+void getQuaternion(quaternion *quat)
+{
+   quat->w = q.w;
+   quat->x = q.x;
+   quat->y = q.y;
+   quat->z = q.z;
+}
+
 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
 {
     /*
@@ -559,6 +612,49 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
     return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f)));
 }
 
+void imuComputeQuaternionFromRPY(quaternionProducts *quatProd, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw)
+{
+    if (initialRoll > 1800) {
+        initialRoll -= 3600;
+    }
+
+    if (initialPitch > 1800) {
+        initialPitch -= 3600;
+    }
+
+    if (initialYaw > 1800) {
+        initialYaw -= 3600;
+    }
+
+    const float cosRoll = cos_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
+    const float sinRoll = sin_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
+
+    const float cosPitch = cos_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
+    const float sinPitch = sin_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
+
+    const float cosYaw = cos_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
+    const float sinYaw = sin_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
+
+    const float q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
+    const float q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
+    const float q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
+    const float q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
+
+    quatProd->xx = sq(q1);
+    quatProd->yy = sq(q2);
+    quatProd->zz = sq(q3);
+
+    quatProd->xy = q1 * q2;
+    quatProd->xz = q1 * q3;
+    quatProd->yz = q2 * q3;
+
+    quatProd->wx = q0 * q1;
+    quatProd->wy = q0 * q2;
+    quatProd->wz = q0 * q3;
+
+    imuComputeRotationMatrix();
+}
+
 #ifdef SIMULATOR_BUILD
 void imuSetAttitudeRPY(float roll, float pitch, float yaw)
 {

src/main/flight/imu.h

@@ -29,6 +29,7 @@ extern uint32_t accTimeSum;
 extern int accSumCount;
 extern float accVelScale;
 extern int32_t accSum[XYZ_AXIS_COUNT];
+extern bool canUseGPSHeading;
 
 typedef struct {
     float w,x,y,z;
@@ -79,6 +80,7 @@ typedef struct imuRuntimeConfig_s {
 void imuConfigure(uint16_t throttle_correction_angle);
 
 float getCosTiltAngle(void);
+void getQuaternion(quaternion * q);
 void imuUpdateAttitude(timeUs_t currentTimeUs);
 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
 
@@ -96,3 +98,5 @@ void imuSetHasNewData(uint32_t dt);
 void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd);
 bool imuQuaternionHeadfreeOffsetSet(void);
 void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def * v);
+void imuComputeQuaternionFromRPY(quaternionProducts *qP, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw);
+bool shouldInitializeGPSHeading(void);