diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 14a18932b7..9d5323765e 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -112,6 +112,8 @@ typedef struct { float alitutudeStepCm; float maxPitchStep; float absErrorAngle; + float groundspeedPitchAttenuator; + float imuYawCogGain; } rescueSensorData_s; typedef struct { @@ -233,7 +235,8 @@ static void rescueAttainPosition(void) throttleI = 0.0f; previousAltitudeError = 0.0f; throttleAdjustment = 0; - rescueState.intent.disarmThreshold = gpsRescueConfig()->disarmThreshold / 10.0f; + rescueState.intent.disarmThreshold = gpsRescueConfig()->disarmThreshold * 0.1f; + rescueState.sensor.imuYawCogGain = 1.0f; return; case RESCUE_DO_NOTHING: // 20s of slow descent for switch induced sanity failures to allow time to recover @@ -453,7 +456,7 @@ static void performSanityChecks(void) if (rescueState.phase == RESCUE_FLY_HOME) { const float velocityToHomeCmS = previousDistanceToHomeCm - rescueState.sensor.distanceToHomeCm; // cm/s previousDistanceToHomeCm = rescueState.sensor.distanceToHomeCm; - rescueState.intent.secondsFailing += (velocityToHomeCmS < 0.5f * rescueState.intent.targetVelocityCmS) ? 1 : -1; + rescueState.intent.secondsFailing += (velocityToHomeCmS < 0.1f * rescueState.intent.targetVelocityCmS) ? 1 : -1; rescueState.intent.secondsFailing = constrain(rescueState.intent.secondsFailing, 0, 15); if (rescueState.intent.secondsFailing == 15) { #ifdef USE_MAG @@ -477,7 +480,6 @@ static void performSanityChecks(void) rescueState.failure = RESCUE_LOWSATS; } - // These conditions ignore sanity mode settings, and apply in all rescues, to handle getting stuck in a climb or descend const float actualAltitudeChange = rescueState.sensor.currentAltitudeCm - prevAltitudeCm; @@ -567,6 +569,25 @@ static void sensorUpdate(void) rescueState.sensor.distanceToHomeM = rescueState.sensor.distanceToHomeCm / 100.0f; rescueState.sensor.groundSpeedCmS = gpsSol.groundSpeed; // cm/s + // when there is significant velocity error, increase IMU COG Gain for yaw to a higher value, and reduce max pitch angle + if (gpsRescueConfig()->rescueGroundspeed) { + const float rescueGroundspeed = 1000.0f; // in cm/s, fixed 10 m/s groundspeed + // sets the slope of the increase in IMU COG Gain as velocity error increases; at 10, we get max CoG gain around 25m/s drift, approx the limit of what we can fix + const float groundspeedErrorRatio = fabsf(rescueState.sensor.groundSpeedCmS - rescueState.sensor.velocityToHomeCmS) / rescueGroundspeed; + // 0 if groundspeed = velocity to home, + // 1 if moving sideways at 10m/s but zero home velocity, + // 2 if moving backwards (away from home) at 10 m/s + // 4 if moving backwards (away from home) at 20 m/s + if ((rescueState.phase == RESCUE_ATTAIN_ALT) || (rescueState.phase == RESCUE_ROTATE)) { + // zero IMU CoG yaw during climb or rotate, to stop IMU error accumulation arising from drift during long climbs + rescueState.sensor.imuYawCogGain = 0; + } else { + // up to 6x increase in CoG IMU Yaw gain + rescueState.sensor.imuYawCogGain = constrainf(1.0f + (2.0f * groundspeedErrorRatio), 1.0f, 6.0f); + } + rescueState.sensor.groundspeedPitchAttenuator = 1.0f / constrainf(1.0f + groundspeedErrorRatio, 1.0f, 3.0f); // cut pitch angle by up to one third + } + rescueState.sensor.gpsDataIntervalSeconds = getGpsDataIntervalSeconds(); // Range from 10ms (100hz) to 1000ms (1Hz). Intended to cover common GPS data rates and exclude unusual values. @@ -648,10 +669,13 @@ void descend(void) rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed * rescueState.intent.proximityToLandingArea; // reduce target velocity as we get closer to home. Zero within 2m of home, reducing risk of overshooting. // if quad drifts further than 2m away from home, should by then have rotated towards home, so pitch is allowed - rescueState.intent.rollAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.intent.proximityToLandingArea; + rescueState.intent.rollAngleLimitDeg = 0.5f * gpsRescueConfig()->maxRescueAngle * rescueState.intent.proximityToLandingArea; // reduce roll capability when closer to home, none within final 2m + rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.sensor.groundspeedPitchAttenuator; } + rescueState.intent.yawAttenuator = 1.0f; // just in case entered descend phase before yaw authority was complete + // configure altitude step for descent, considering interval between altitude readings rescueState.intent.altitudeStep = -1.0f * rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate; @@ -758,7 +782,8 @@ void gpsRescueUpdate(void) rescueState.intent.yawAttenuator += rescueState.sensor.gpsRescueTaskIntervalSeconds; } if (rescueState.sensor.absErrorAngle < 30.0f) { - rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle; // allow pitch + // allow pitch, limiting allowed angle if we are drifting away from home + rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.sensor.groundspeedPitchAttenuator; rescueState.phase = RESCUE_FLY_HOME; // enter fly home phase rescueState.intent.secondsFailing = 0; // reset sanity timer for flight home rescueState.intent.proximityToLandingArea = 1.0f; // velocity iTerm activated, initialise proximity for descent phase at 1.0 @@ -771,7 +796,6 @@ void gpsRescueUpdate(void) if (rescueState.intent.yawAttenuator < 1.0f) { // be sure to accumulate full yaw authority rescueState.intent.yawAttenuator += rescueState.sensor.gpsRescueTaskIntervalSeconds; } - // velocity PIDs are now active // update target velocity gradually, aiming for rescueGroundspeed with a time constant of 1.0s const float targetVelocityError = gpsRescueConfig()->rescueGroundspeed - rescueState.intent.targetVelocityCmS; @@ -794,6 +818,8 @@ void gpsRescueUpdate(void) // gradually gain roll capability to max of half of max pitch angle if (newGPSData) { + // cut back on allowed angle if there is a high groundspeed error + rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.sensor.groundspeedPitchAttenuator; if (rescueState.sensor.distanceToHomeM <= rescueState.intent.descentDistanceM) { rescueState.phase = RESCUE_DESCENT; rescueState.intent.secondsFailing = 0; // reset sanity timer for descent @@ -853,6 +879,11 @@ float gpsRescueGetYawRate(void) return rescueYaw; } +float gpsRescueGetImuYawCogGain(void) +{ + return rescueState.sensor.imuYawCogGain; +} + float gpsRescueGetThrottle(void) { // Calculated a desired commanded throttle scaled from 0.0 to 1.0 for use in the mixer. diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index 7e77e7121c..9fb2897dfc 100644 --- a/src/main/flight/gps_rescue.h +++ b/src/main/flight/gps_rescue.h @@ -57,3 +57,4 @@ bool gpsRescueIsConfigured(void); bool gpsRescueIsAvailable(void); bool gpsRescueIsDisabled(void); bool gpsRescueDisableMag(void); +float gpsRescueGetImuYawCogGain(void); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 613c801cbf..eda9eb7b4c 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -201,7 +201,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, - bool useCOG, float courseOverGround, const float dcmKpGain) + float cogYawGain, float courseOverGround, const float dcmKpGain) { static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki @@ -210,17 +210,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 (useCOG) { + if (cogYawGain != 0.0f) { + // Used in a GPS Rescue to boost IMU yaw gain when course over ground and velocity to home differ significantly 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]); - + const float ez_ef = cogYawGain * (- 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; @@ -263,7 +261,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz, // Use measured acceleration vector float recipAccNorm = sq(ax) + sq(ay) + sq(az); if (useAcc && recipAccNorm > 0.01f) { - // Normalise accelerometer measurement + // Normalise accelerometer measurement; useAcc is true when all smoothed acc axes are within 20% of 1G recipAccNorm = invSqrt(recipAccNorm); ax *= recipAccNorm; ay *= recipAccNorm; @@ -415,10 +413,10 @@ static float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAvera if (attitudeResetActive) { ret = ATTITUDE_RESET_KP_GAIN; } else { - ret = imuRuntimeConfig.dcm_kp; - if (!armState) { - ret = ret * 10.0f; // Scale the kP to generally converge faster when disarmed. - } + ret = imuRuntimeConfig.dcm_kp; + if (!armState) { + ret *= 10.0f; // Scale the kP to generally converge faster when disarmed. + } } return ret; @@ -476,8 +474,8 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) static timeUs_t previousIMUUpdateTime; bool useAcc = false; bool useMag = 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 + float cogYawGain = 0.0f; // IMU yaw gain to be applied in imuMahonyAHRSupdate from ground course, default to no correction from CoG + float courseOverGround = 0; // To be used when cogYawGain is non-zero, in radians const timeDelta_t deltaT = currentTimeUs - previousIMUUpdateTime; previousIMUUpdateTime = currentTimeUs; @@ -495,14 +493,15 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) if (!useMag && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat > GPS_MIN_SAT_COUNT && gpsSol.groundSpeed >= GPS_COG_MIN_GROUNDSPEED) { // Use GPS course over ground to correct attitude.values.yaw courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse); - useCOG = true; - + if (FLIGHT_MODE(GPS_RESCUE_MODE)) { + cogYawGain = gpsRescueGetImuYawCogGain(); // do not modify IMU yaw gain unless in a rescue + } if (shouldInitializeGPSHeading()) { - // Reset our reference and reinitialize quaternion. This will likely ideally happen more than once per flight, but for now, + // Reset our reference and reinitialize quaternion. // 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. + cogYawGain = 0.0f; // Don't use the COG when we first initialize } } #endif @@ -512,7 +511,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) UNUSED(imuIsAccelerometerHealthy); UNUSED(useAcc); UNUSED(useMag); - UNUSED(useCOG); + UNUSED(cogYawGain); UNUSED(canUseGPSHeading); UNUSED(courseOverGround); UNUSED(deltaT); @@ -528,13 +527,13 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) gyroAverage[axis] = gyroGetFilteredDownsampled(axis); } - useAcc = imuIsAccelerometerHealthy(acc.accADC); + useAcc = imuIsAccelerometerHealthy(acc.accADC); // all smoothed accADC values are within 20% of 1G imuMahonyAHRSupdate(deltaT * 1e-6f, DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]), useAcc, acc.accADC[X], acc.accADC[Y], acc.accADC[Z], useMag, - useCOG, courseOverGround, imuCalcKpGain(currentTimeUs, useAcc, gyroAverage)); + cogYawGain, courseOverGround, imuCalcKpGain(currentTimeUs, useAcc, gyroAverage)); imuUpdateEulerAngles(); #endif diff --git a/src/main/pg/gps_rescue.c b/src/main/pg/gps_rescue.c index 525c32ca43..f7893135ce 100644 --- a/src/main/pg/gps_rescue.c +++ b/src/main/pg/gps_rescue.c @@ -40,7 +40,7 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, .initialAltitudeM = 30, .rescueGroundspeed = 750, - .maxRescueAngle = 70, + .maxRescueAngle = 45, .rollMix = 150, .pitchCutoffHz = 75, diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index a78daf0421..2ec9304f47 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -256,6 +256,7 @@ extern "C" { float baroUpsampleAltitude() { return 0.0f; } float pt2FilterGain(float, float) { return 0.0f; } float getBaroAltitude(void) { return 3000.0f; } + float gpsRescueGetImuYawCogGain(void) { return 1.0f; } void pt2FilterInit(pt2Filter_t *baroDerivativeLpf, float) { UNUSED(baroDerivativeLpf);