diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index af1044892..45288b4e2 100644 --- a/src/main/flight/imu.c +++ b/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) { - while (yawError > M_PIf) yawError -= (2.0f * M_PIf); - while (yawError < -M_PIf) yawError += (2.0f * M_PIf); - ez += sin_approx(yawError / 2.0f); + if (useCOG) { + while (courseOverGround > M_PIf) courseOverGround -= (2.0f * M_PIf); + while (courseOverGround < -M_PIf) courseOverGround += (2.0f * M_PIf); + + 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) { - // In case of a fixed-wing aircraft we can use GPS course over ground to correct heading - rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - gpsSol.groundCourse); - useYaw = true; + if (!useMag && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= GPS_COG_MIN_GROUNDSPEED) { + // Use GPS course over ground to correct attitude.values.yaw + if (STATE(FIXED_WING)) { + 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(rawYawError); + UNUSED(useCOG); + 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) { diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index fc06d6f6a..176ad76e3 100644 --- a/src/main/flight/imu.h +++ b/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);