diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index f2b4ccc632..d44c4a945f 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -51,10 +51,69 @@ #include "gps_rescue.h" +typedef enum { + RESCUE_SANITY_OFF = 0, + RESCUE_SANITY_ON, + RESCUE_SANITY_FS_ONLY +} gpsRescueSanity_e; + +typedef enum { + RESCUE_IDLE, + RESCUE_INITIALIZE, + RESCUE_ATTAIN_ALT, + RESCUE_CROSSTRACK, + RESCUE_LANDING_APPROACH, + RESCUE_LANDING, + RESCUE_ABORT, + RESCUE_COMPLETE +} rescuePhase_e; + +typedef enum { + RESCUE_HEALTHY, + RESCUE_FLYAWAY, + RESCUE_CRASH_DETECTED, + RESCUE_TOO_CLOSE +} rescueFailureState_e; + +typedef struct { + int32_t targetAltitudeCm; + int32_t targetGroundspeed; + uint8_t minAngleDeg; + uint8_t maxAngleDeg; + bool crosstrack; +} rescueIntent_s; + +typedef struct { + int32_t maxAltitudeCm; + int32_t currentAltitudeCm; + uint16_t distanceToHomeM; + uint16_t maxDistanceToHomeM; + int16_t directionToHome; + uint16_t groundSpeed; + uint8_t numSat; + float zVelocity; // Up/down movement in cm/s + float zVelocityAvg; // Up/down average in cm/s + float accMagnitude; + float accMagnitudeAvg; +} rescueSensorData_s; + +typedef struct { + bool bumpDetection; + bool convergenceDetection; +} rescueSanityFlags; + +typedef struct { + rescuePhase_e phase; + rescueFailureState_e failure; + rescueSensorData_s sensor; + rescueIntent_s intent; + bool isFailsafe; +} rescueState_s; + #define GPS_RESCUE_MAX_YAW_RATE 360 // deg/sec max yaw rate #define GPS_RESCUE_RATE_SCALE_DEGREES 45 // Scale the commanded yaw rate when the error is less then this angle -PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 1); PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, .angle = 32, @@ -100,6 +159,220 @@ void rescueNewGpsData(void) newGPSData = true; } +static void rescueStart() +{ + rescueState.phase = RESCUE_INITIALIZE; +} + +static void rescueStop() +{ + rescueState.phase = RESCUE_IDLE; +} + +// Things that need to run regardless of GPS rescue mode being enabled or not +static void idleTasks() +{ + // Do not calculate any of the idle task values when we are not flying + if (!ARMING_FLAG(ARMED)) { + return; + } + + // Don't update any rescue flight statistics if we haven't applied a proper altitude offset yet + if (!isAltitudeOffset()) { + return; + } + + gpsRescueAngle[AI_PITCH] = 0; + gpsRescueAngle[AI_ROLL] = 0; + + // Store the max altitude we see not during RTH so we know our fly-back minimum alt + rescueState.sensor.maxAltitudeCm = MAX(rescueState.sensor.currentAltitudeCm, rescueState.sensor.maxAltitudeCm); + // Store the max distance to home during normal flight so we know if a flyaway is happening + rescueState.sensor.maxDistanceToHomeM = MAX(rescueState.sensor.distanceToHomeM, rescueState.sensor.maxDistanceToHomeM); + + rescueThrottle = rcCommand[THROTTLE]; + + //to do: have a default value for hoverThrottle + + // FIXME: GPS Rescue throttle handling should take into account min_check as the + // active throttle is from min_check through PWM_RANGE_MAX. Currently adjusting for this + // in gpsRescueGetThrottle() but it would be better handled here. + + float ct = getCosTiltAngle(); + if (ct > 0.5 && ct < 0.96 && throttleSamples < 1E6 && rescueThrottle > 1070) { //5 to 45 degrees tilt + //TO DO: only sample when acceleration is low + uint16_t adjustedThrottle = 1000 + (rescueThrottle - PWM_RANGE_MIN) * ct; + if (throttleSamples == 0) { + averageThrottle = adjustedThrottle; + } else { + averageThrottle += (adjustedThrottle - averageThrottle) / (throttleSamples + 1); + } + hoverThrottle = lrintf(averageThrottle); + throttleSamples++; + } +} + +// Very similar to maghold function on betaflight/cleanflight +static void setBearing(int16_t desiredHeading) +{ + float errorAngle = (attitude.values.yaw / 10.0f) - desiredHeading; + + // Determine the most efficient direction to rotate + if (errorAngle <= -180) { + errorAngle += 360; + } else if (errorAngle > 180) { + errorAngle -= 360; + } + + errorAngle *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed); + + // Calculate a desired yaw rate based on a maximum limit beyond + // an error window and then scale the requested rate down inside + // the window as error approaches 0. + rescueYaw = -constrainf(errorAngle / GPS_RESCUE_RATE_SCALE_DEGREES * GPS_RESCUE_MAX_YAW_RATE, -GPS_RESCUE_MAX_YAW_RATE, GPS_RESCUE_MAX_YAW_RATE); +} + +static void rescueAttainPosition() +{ + // Point to home if that is in our intent + if (rescueState.intent.crosstrack) { + setBearing(rescueState.sensor.directionToHome); + } + + if (!newGPSData) { + return; + } + + /** + Speed controller + */ + static float previousSpeedError = 0; + static int16_t speedIntegral = 0; + + const int16_t speedError = (rescueState.intent.targetGroundspeed - rescueState.sensor.groundSpeed) / 100; + const int16_t speedDerivative = speedError - previousSpeedError; + + speedIntegral = constrain(speedIntegral + speedError, -100, 100); + + previousSpeedError = speedError; + + int16_t angleAdjustment = gpsRescueConfig()->velP * speedError + (gpsRescueConfig()->velI * speedIntegral) / 100 + gpsRescueConfig()->velD * speedDerivative; + + gpsRescueAngle[AI_PITCH] = constrain(gpsRescueAngle[AI_PITCH] + MIN(angleAdjustment, 80), rescueState.intent.minAngleDeg * 100, rescueState.intent.maxAngleDeg * 100); + + float ct = cos(DECIDEGREES_TO_RADIANS(gpsRescueAngle[AI_PITCH] / 10)); + + /** + Altitude controller + */ + static float previousAltitudeError = 0; + static int16_t altitudeIntegral = 0; + + const int16_t altitudeError = (rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) / 100; // Error in meters + const int16_t altitudeDerivative = altitudeError - previousAltitudeError; + + // Only allow integral windup within +-15m absolute altitude error + if (ABS(altitudeError) < 25) { + altitudeIntegral = constrain(altitudeIntegral + altitudeError, -250, 250); + } else { + altitudeIntegral = 0; + } + + previousAltitudeError = altitudeError; + + int16_t altitudeAdjustment = (gpsRescueConfig()->throttleP * altitudeError + (gpsRescueConfig()->throttleI * altitudeIntegral) / 10 * + gpsRescueConfig()->throttleD * altitudeDerivative) / ct / 20; + int16_t hoverAdjustment = (hoverThrottle - 1000) / ct; + + rescueThrottle = constrain(1000 + altitudeAdjustment + hoverAdjustment, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax); + + DEBUG_SET(DEBUG_RTH, 0, rescueThrottle); + DEBUG_SET(DEBUG_RTH, 1, gpsRescueAngle[AI_PITCH]); + DEBUG_SET(DEBUG_RTH, 2, altitudeAdjustment); + DEBUG_SET(DEBUG_RTH, 3, rescueState.failure); +} + +static void performSanityChecks() +{ + if (rescueState.phase == RESCUE_IDLE) { + rescueState.failure = RESCUE_HEALTHY; + + return; + } + + // Do not abort until each of these items is fully tested + if (rescueState.failure != RESCUE_HEALTHY) { + if (gpsRescueConfig()->sanityChecks == RESCUE_SANITY_ON + || (gpsRescueConfig()->sanityChecks == RESCUE_SANITY_FS_ONLY && rescueState.isFailsafe == true)) { + rescueState.phase = RESCUE_ABORT; + } + } + + // Check if crash recovery mode is active, disarm if so. + if (crashRecoveryModeActive()) { + rescueState.failure = RESCUE_CRASH_DETECTED; + } + + // Things that should run at a low refresh rate (such as flyaway detection, etc) + // This runs at ~1hz + + static uint32_t previousTimeUs; + + const uint32_t currentTimeUs = micros(); + const uint32_t dTime = currentTimeUs - previousTimeUs; + + if (dTime < 1000000) { //1hz + return; + } + + previousTimeUs = currentTimeUs; + + // Stalled movement detection + static int8_t gsI = 0; + + gsI = constrain(gsI + (rescueState.sensor.groundSpeed < 150) ? 1 : -1, -10, 10); + + if (gsI == 10) { + rescueState.failure = RESCUE_CRASH_DETECTED; + } + + // Minimum sat detection + static int8_t msI = 0; + + msI = constrain(msI + (rescueState.sensor.numSat < gpsRescueConfig()->minSats) ? 1 : -1, -5, 5); + + if (msI == 5) { + rescueState.failure = RESCUE_FLYAWAY; + } +} + +static void sensorUpdate() +{ + rescueState.sensor.currentAltitudeCm = getEstimatedAltitudeCm(); + + // Calculate altitude velocity + static uint32_t previousTimeUs; + static int32_t previousAltitudeCm; + + const uint32_t currentTimeUs = micros(); + const float dTime = currentTimeUs - previousTimeUs; + + if (newGPSData) { // Calculate velocity at lowest common denominator + rescueState.sensor.distanceToHomeM = GPS_distanceToHome; + rescueState.sensor.directionToHome = GPS_directionToHome; + rescueState.sensor.numSat = gpsSol.numSat; + rescueState.sensor.groundSpeed = gpsSol.groundSpeed; + + rescueState.sensor.zVelocity = (rescueState.sensor.currentAltitudeCm - previousAltitudeCm) * 1000000.0f / dTime; + rescueState.sensor.zVelocityAvg = 0.8f * rescueState.sensor.zVelocityAvg + rescueState.sensor.zVelocity * 0.2f; + + rescueState.sensor.accMagnitude = (float) sqrtf(sq(acc.accADC[Z]) + sq(acc.accADC[X]) + sq(acc.accADC[Y])) * acc.dev.acc_1G_rec; + rescueState.sensor.accMagnitudeAvg = (rescueState.sensor.accMagnitudeAvg * 0.8f) + (rescueState.sensor.accMagnitude * 0.2f); + + previousAltitudeCm = rescueState.sensor.currentAltitudeCm; + previousTimeUs = currentTimeUs; + } +} + /* Determine what phase we are in, determine if all criteria are met to move to the next phase */ @@ -218,222 +491,6 @@ void updateGPSRescueState(void) newGPSData = false; } -void sensorUpdate() -{ - rescueState.sensor.currentAltitudeCm = getEstimatedAltitudeCm(); - - // Calculate altitude velocity - static uint32_t previousTimeUs; - static int32_t previousAltitudeCm; - - const uint32_t currentTimeUs = micros(); - const float dTime = currentTimeUs - previousTimeUs; - - if (newGPSData) { // Calculate velocity at lowest common denominator - rescueState.sensor.distanceToHomeM = GPS_distanceToHome; - rescueState.sensor.directionToHome = GPS_directionToHome; - rescueState.sensor.numSat = gpsSol.numSat; - rescueState.sensor.groundSpeed = gpsSol.groundSpeed; - - rescueState.sensor.zVelocity = (rescueState.sensor.currentAltitudeCm - previousAltitudeCm) * 1000000.0f / dTime; - rescueState.sensor.zVelocityAvg = 0.8f * rescueState.sensor.zVelocityAvg + rescueState.sensor.zVelocity * 0.2f; - - rescueState.sensor.accMagnitude = (float) sqrtf(sq(acc.accADC[Z]) + sq(acc.accADC[X]) + sq(acc.accADC[Y])) * acc.dev.acc_1G_rec; - rescueState.sensor.accMagnitudeAvg = (rescueState.sensor.accMagnitudeAvg * 0.8f) + (rescueState.sensor.accMagnitude * 0.2f); - - previousAltitudeCm = rescueState.sensor.currentAltitudeCm; - previousTimeUs = currentTimeUs; - } -} - -void performSanityChecks() -{ - if (rescueState.phase == RESCUE_IDLE) { - rescueState.failure = RESCUE_HEALTHY; - - return; - } - - // Do not abort until each of these items is fully tested - if (rescueState.failure != RESCUE_HEALTHY) { - if (gpsRescueConfig()->sanityChecks == RESCUE_SANITY_ON - || (gpsRescueConfig()->sanityChecks == RESCUE_SANITY_FS_ONLY && rescueState.isFailsafe == true)) { - rescueState.phase = RESCUE_ABORT; - } - } - - // Check if crash recovery mode is active, disarm if so. - if (crashRecoveryModeActive()) { - rescueState.failure = RESCUE_CRASH_DETECTED; - } - - // Things that should run at a low refresh rate (such as flyaway detection, etc) - // This runs at ~1hz - - static uint32_t previousTimeUs; - - const uint32_t currentTimeUs = micros(); - const uint32_t dTime = currentTimeUs - previousTimeUs; - - if (dTime < 1000000) { //1hz - return; - } - - previousTimeUs = currentTimeUs; - - // Stalled movement detection - static int8_t gsI = 0; - - gsI = constrain(gsI + (rescueState.sensor.groundSpeed < 150) ? 1 : -1, -10, 10); - - if (gsI == 10) { - rescueState.failure = RESCUE_CRASH_DETECTED; - } - - // Minimum sat detection - static int8_t msI = 0; - - msI = constrain(msI + (rescueState.sensor.numSat < gpsRescueConfig()->minSats) ? 1 : -1, -5, 5); - - if (msI == 5) { - rescueState.failure = RESCUE_FLYAWAY; - } -} - -void rescueStart() -{ - rescueState.phase = RESCUE_INITIALIZE; -} - -void rescueStop() -{ - rescueState.phase = RESCUE_IDLE; -} - -// Things that need to run regardless of GPS rescue mode being enabled or not -void idleTasks() -{ - // Do not calculate any of the idle task values when we are not flying - if (!ARMING_FLAG(ARMED)) { - return; - } - - // Don't update any rescue flight statistics if we haven't applied a proper altitude offset yet - if (!isAltitudeOffset()) { - return; - } - - gpsRescueAngle[AI_PITCH] = 0; - gpsRescueAngle[AI_ROLL] = 0; - - // Store the max altitude we see not during RTH so we know our fly-back minimum alt - rescueState.sensor.maxAltitudeCm = MAX(rescueState.sensor.currentAltitudeCm, rescueState.sensor.maxAltitudeCm); - // Store the max distance to home during normal flight so we know if a flyaway is happening - rescueState.sensor.maxDistanceToHomeM = MAX(rescueState.sensor.distanceToHomeM, rescueState.sensor.maxDistanceToHomeM); - - rescueThrottle = rcCommand[THROTTLE]; - - //to do: have a default value for hoverThrottle - - // FIXME: GPS Rescue throttle handling should take into account min_check as the - // active throttle is from min_check through PWM_RANGE_MAX. Currently adjusting for this - // in gpsRescueGetThrottle() but it would be better handled here. - - float ct = getCosTiltAngle(); - if (ct > 0.5 && ct < 0.96 && throttleSamples < 1E6 && rescueThrottle > 1070) { //5 to 45 degrees tilt - //TO DO: only sample when acceleration is low - uint16_t adjustedThrottle = 1000 + (rescueThrottle - PWM_RANGE_MIN) * ct; - if (throttleSamples == 0) { - averageThrottle = adjustedThrottle; - } else { - averageThrottle += (adjustedThrottle - averageThrottle) / (throttleSamples + 1); - } - hoverThrottle = lrintf(averageThrottle); - throttleSamples++; - } - -} - -void rescueAttainPosition() -{ - // Point to home if that is in our intent - if (rescueState.intent.crosstrack) { - setBearing(rescueState.sensor.directionToHome); - } - - if (!newGPSData) { - return; - } - - - /** - Speed controller - */ - static float previousSpeedError = 0; - static int16_t speedIntegral = 0; - - const int16_t speedError = (rescueState.intent.targetGroundspeed - rescueState.sensor.groundSpeed) / 100; - const int16_t speedDerivative = speedError - previousSpeedError; - - speedIntegral = constrain(speedIntegral + speedError, -100, 100); - - previousSpeedError = speedError; - - int16_t angleAdjustment = gpsRescueConfig()->velP * speedError + (gpsRescueConfig()->velI * speedIntegral) / 100 + gpsRescueConfig()->velD * speedDerivative; - - gpsRescueAngle[AI_PITCH] = constrain(gpsRescueAngle[AI_PITCH] + MIN(angleAdjustment, 80), rescueState.intent.minAngleDeg * 100, rescueState.intent.maxAngleDeg * 100); - - float ct = cos(DECIDEGREES_TO_RADIANS(gpsRescueAngle[AI_PITCH] / 10)); - - /** - Altitude controller - */ - static float previousAltitudeError = 0; - static int16_t altitudeIntegral = 0; - - const int16_t altitudeError = (rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) / 100; // Error in meters - const int16_t altitudeDerivative = altitudeError - previousAltitudeError; - - // Only allow integral windup within +-15m absolute altitude error - if (ABS(altitudeError) < 25) { - altitudeIntegral = constrain(altitudeIntegral + altitudeError, -250, 250); - } else { - altitudeIntegral = 0; - } - - previousAltitudeError = altitudeError; - - int16_t altitudeAdjustment = (gpsRescueConfig()->throttleP * altitudeError + (gpsRescueConfig()->throttleI * altitudeIntegral) / 10 * + gpsRescueConfig()->throttleD * altitudeDerivative) / ct / 20; - int16_t hoverAdjustment = (hoverThrottle - 1000) / ct; - - rescueThrottle = constrain(1000 + altitudeAdjustment + hoverAdjustment, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax); - - DEBUG_SET(DEBUG_RTH, 0, rescueThrottle); - DEBUG_SET(DEBUG_RTH, 1, gpsRescueAngle[AI_PITCH]); - DEBUG_SET(DEBUG_RTH, 2, altitudeAdjustment); - DEBUG_SET(DEBUG_RTH, 3, rescueState.failure); -} - -// Very similar to maghold function on betaflight/cleanflight -void setBearing(int16_t desiredHeading) -{ - float errorAngle = (attitude.values.yaw / 10.0f) - desiredHeading; - - // Determine the most efficient direction to rotate - if (errorAngle <= -180) { - errorAngle += 360; - } else if (errorAngle > 180) { - errorAngle -= 360; - } - - errorAngle *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed); - - // Calculate a desired yaw rate based on a maximum limit beyond - // an error window and then scale the requested rate down inside - // the window as error approaches 0. - rescueYaw = -constrainf(errorAngle / GPS_RESCUE_RATE_SCALE_DEGREES * GPS_RESCUE_MAX_YAW_RATE, -GPS_RESCUE_MAX_YAW_RATE, GPS_RESCUE_MAX_YAW_RATE); -} - float gpsRescueGetYawRate(void) { return rescueYaw; diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index dba47f39f8..d77de91da1 100644 --- a/src/main/flight/gps_rescue.h +++ b/src/main/flight/gps_rescue.h @@ -19,12 +19,6 @@ #include "pg/pg.h" -typedef enum { - RESCUE_SANITY_OFF = 0, - RESCUE_SANITY_ON, - RESCUE_SANITY_FS_ONLY -} gpsRescueSanity_e; - typedef struct gpsRescue_s { uint16_t angle; //degrees uint16_t initialAltitudeM; //meters @@ -38,77 +32,15 @@ typedef struct gpsRescue_s { uint16_t velP, velI, velD; uint8_t minSats; uint16_t minRescueDth; //meters - gpsRescueSanity_e sanityChecks; + uint8_t sanityChecks; } gpsRescueConfig_t; PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig); -typedef enum { - RESCUE_IDLE, - RESCUE_INITIALIZE, - RESCUE_ATTAIN_ALT, - RESCUE_CROSSTRACK, - RESCUE_LANDING_APPROACH, - RESCUE_LANDING, - RESCUE_ABORT, - RESCUE_COMPLETE -} rescuePhase_e; - -typedef enum { - RESCUE_HEALTHY, - RESCUE_FLYAWAY, - RESCUE_CRASH_DETECTED, - RESCUE_TOO_CLOSE -} rescueFailureState_e; - -typedef struct { - int32_t targetAltitudeCm; - int32_t targetGroundspeed; - uint8_t minAngleDeg; - uint8_t maxAngleDeg; - bool crosstrack; -} rescueIntent_s; - -typedef struct { - int32_t maxAltitudeCm; - int32_t currentAltitudeCm; - uint16_t distanceToHomeM; - uint16_t maxDistanceToHomeM; - int16_t directionToHome; - uint16_t groundSpeed; - uint8_t numSat; - float zVelocity; // Up/down movement in cm/s - float zVelocityAvg; // Up/down average in cm/s - float accMagnitude; - float accMagnitudeAvg; -} rescueSensorData_s; - -typedef struct { - bool bumpDetection; - bool convergenceDetection; -} rescueSanityFlags; - -typedef struct { - rescuePhase_e phase; - rescueFailureState_e failure; - rescueSensorData_s sensor; - rescueIntent_s intent; - bool isFailsafe; -} rescueState_s; - extern int32_t gpsRescueAngle[ANGLE_INDEX_COUNT]; //NOTE: ANGLES ARE IN CENTIDEGREES -extern rescueState_s rescueState; void updateGPSRescueState(void); void rescueNewGpsData(void); -void idleTasks(void); -void rescueStop(void); -void rescueStart(void); -void setBearing(int16_t deg); -void performSanityChecks(void); -void sensorUpdate(void); - -void rescueAttainPosition(void); float gpsRescueGetYawRate(void); float gpsRescueGetThrottle(void);