From 8950eb1f68650550ab12633e77e6a615bc3b0bc7 Mon Sep 17 00:00:00 2001 From: Curtis Bangert Date: Wed, 13 Jun 2018 11:17:21 -0400 Subject: [PATCH 1/2] Moved updateGPSRescueState to imuUpdateAttitude. gps_rescue.c tidy. --- src/main/fc/fc_core.c | 8 +- src/main/flight/gps_rescue.c | 144 ++++++++++++++++++----------------- src/main/flight/imu.c | 12 +-- src/main/io/gps.c | 3 + 4 files changed, 84 insertions(+), 83 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 4d03c0ebb..96554a287 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -549,7 +549,7 @@ uint8_t calculateThrottlePercent(void) static bool airmodeIsActivated; -bool isAirmodeActivated() +bool isAirmodeActivated() { return airmodeIsActivated; } @@ -809,7 +809,7 @@ bool processRx(timeUs_t currentTimeUs) } } #endif - + if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) { ENABLE_FLIGHT_MODE(PASSTHRU_MODE); } else { @@ -924,10 +924,6 @@ static FAST_CODE_NOINLINE void subTaskMainSubprocesses(timeUs_t currentTimeUs) } #endif -#ifdef USE_GPS_RESCUE - updateGPSRescueState(); -#endif - #ifdef USE_SDCARD afatfs_poll(); #endif diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 5d6fa1e03..8ba62d210 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -109,87 +109,89 @@ void updateGPSRescueState(void) switch (rescueState.phase) { case RESCUE_IDLE: - idleTasks(); - break; + idleTasks(); + break; case RESCUE_INITIALIZE: - if (hoverThrottle == 0) { //no actual throttle data yet, let's use the default. - hoverThrottle = gpsRescueConfig()->throttleHover; - } - - rescueState.phase = RESCUE_ATTAIN_ALT; - FALLTHROUGH; + if (hoverThrottle == 0) { //no actual throttle data yet, let's use the default. + hoverThrottle = gpsRescueConfig()->throttleHover; + } + + rescueState.phase = RESCUE_ATTAIN_ALT; + FALLTHROUGH; case RESCUE_ATTAIN_ALT: - // Get to a safe altitude at a low velocity ASAP - if (ABS(rescueState.intent.targetAltitude - rescueState.sensor.currentAltitude) < 1000) { - rescueState.phase = RESCUE_CROSSTRACK; - } - - rescueState.intent.targetGroundspeed = 500; - rescueState.intent.targetAltitude = MAX(gpsRescueConfig()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500); - rescueState.intent.crosstrack = true; - rescueState.intent.minAngleDeg = 10; - rescueState.intent.maxAngleDeg = 15; - break; + // Get to a safe altitude at a low velocity ASAP + if (ABS(rescueState.intent.targetAltitude - rescueState.sensor.currentAltitude) < 1000) { + rescueState.phase = RESCUE_CROSSTRACK; + } + + rescueState.intent.targetGroundspeed = 500; + rescueState.intent.targetAltitude = MAX(gpsRescueConfig()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500); + rescueState.intent.crosstrack = true; + rescueState.intent.minAngleDeg = 10; + rescueState.intent.maxAngleDeg = 15; + break; case RESCUE_CROSSTRACK: - if (rescueState.sensor.distanceToHome < gpsRescueConfig()->descentDistance) { - rescueState.phase = RESCUE_LANDING_APPROACH; - } - - // We can assume at this point that we are at or above our RTH height, so we need to try and point to home and tilt while maintaining alt - // Is our altitude way off? We should probably kick back to phase RESCUE_ATTAIN_ALT - rescueState.intent.targetGroundspeed = gpsRescueConfig()->rescueGroundspeed; - rescueState.intent.targetAltitude = MAX(gpsRescueConfig()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500); - rescueState.intent.crosstrack = true; - rescueState.intent.minAngleDeg = 15; - rescueState.intent.maxAngleDeg = gpsRescueConfig()->angle; - break; + if (rescueState.sensor.distanceToHome < gpsRescueConfig()->descentDistance) { + rescueState.phase = RESCUE_LANDING_APPROACH; + } + + // We can assume at this point that we are at or above our RTH height, so we need to try and point to home and tilt while maintaining alt + // Is our altitude way off? We should probably kick back to phase RESCUE_ATTAIN_ALT + rescueState.intent.targetGroundspeed = gpsRescueConfig()->rescueGroundspeed; + rescueState.intent.targetAltitude = MAX(gpsRescueConfig()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500); + rescueState.intent.crosstrack = true; + rescueState.intent.minAngleDeg = 15; + rescueState.intent.maxAngleDeg = gpsRescueConfig()->angle; + break; case RESCUE_LANDING_APPROACH: - // We are getting close to home in the XY plane, get Z where it needs to be to move to landing phase - if (rescueState.sensor.distanceToHome < 10 && rescueState.sensor.currentAltitude <= 1000) { - rescueState.phase = RESCUE_LANDING; - } - - // Only allow new altitude and new speed to be equal or lower than the current values (to prevent parabolic movement on overshoot) - int32_t newAlt = gpsRescueConfig()->initialAltitude * 100 * rescueState.sensor.distanceToHome / gpsRescueConfig()->descentDistance; - int32_t newSpeed = gpsRescueConfig()->rescueGroundspeed * rescueState.sensor.distanceToHome / gpsRescueConfig()->descentDistance; - - rescueState.intent.targetAltitude = constrain(newAlt, 100, rescueState.intent.targetAltitude); - rescueState.intent.targetGroundspeed = constrain(newSpeed, 100, rescueState.intent.targetGroundspeed); - rescueState.intent.crosstrack = true; - rescueState.intent.minAngleDeg = 10; - rescueState.intent.maxAngleDeg = 20; - break; + // We are getting close to home in the XY plane, get Z where it needs to be to move to landing phase + if (rescueState.sensor.distanceToHome < 10 && rescueState.sensor.currentAltitude <= 1000) { + rescueState.phase = RESCUE_LANDING; + } + + // Only allow new altitude and new speed to be equal or lower than the current values (to prevent parabolic movement on overshoot) + int32_t newAlt = gpsRescueConfig()->initialAltitude * 100 * rescueState.sensor.distanceToHome / gpsRescueConfig()->descentDistance; + int32_t newSpeed = gpsRescueConfig()->rescueGroundspeed * rescueState.sensor.distanceToHome / gpsRescueConfig()->descentDistance; + + rescueState.intent.targetAltitude = constrain(newAlt, 100, rescueState.intent.targetAltitude); + rescueState.intent.targetGroundspeed = constrain(newSpeed, 100, rescueState.intent.targetGroundspeed); + rescueState.intent.crosstrack = true; + rescueState.intent.minAngleDeg = 10; + rescueState.intent.maxAngleDeg = 20; + break; case RESCUE_LANDING: - // We have reached the XYZ envelope to be considered at "home". We need to land gently and check our accelerometer for abnormal data. - // At this point, do not let the target altitude go up anymore, so if we overshoot, we dont' move in a parabolic trajectory - - // If we are over 120% of average magnitude, just disarm since we're pretty much home - if (rescueState.sensor.accMagnitude > rescueState.sensor.accMagnitudeAvg * 1.5) { - disarm(); - rescueState.phase = RESCUE_COMPLETE; - } - - rescueState.intent.targetGroundspeed = 0; - rescueState.intent.targetAltitude = 0; - rescueState.intent.crosstrack = true; - rescueState.intent.minAngleDeg = 0; - rescueState.intent.maxAngleDeg = 15; - break; + // We have reached the XYZ envelope to be considered at "home". We need to land gently and check our accelerometer for abnormal data. + // At this point, do not let the target altitude go up anymore, so if we overshoot, we dont' move in a parabolic trajectory + + // If we are over 120% of average magnitude, just disarm since we're pretty much home + if (rescueState.sensor.accMagnitude > rescueState.sensor.accMagnitudeAvg * 1.5) { + disarm(); + rescueState.phase = RESCUE_COMPLETE; + } + + rescueState.intent.targetGroundspeed = 0; + rescueState.intent.targetAltitude = 0; + rescueState.intent.crosstrack = true; + rescueState.intent.minAngleDeg = 0; + rescueState.intent.maxAngleDeg = 15; + break; case RESCUE_COMPLETE: - rescueStop(); - break; + rescueStop(); + break; case RESCUE_ABORT: - disarm(); - rescueStop(); - break; + disarm(); + rescueStop(); + break; + default: + break; } - + performSanityChecks(); - + if (rescueState.phase != RESCUE_IDLE) { rescueAttainPosition(); } - + newGPSData = false; } @@ -225,7 +227,7 @@ void performSanityChecks() { if (rescueState.phase == RESCUE_IDLE) { rescueState.failure = RESCUE_HEALTHY; - + return; } @@ -335,7 +337,7 @@ void rescueAttainPosition() return; } - + /** Speed controller */ diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 8a579ec45..7785ec47d 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -394,11 +394,11 @@ float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAverage) if ((fabsf(gyroAverage[X]) > ATTITUDE_RESET_GYRO_LIMIT) || (fabsf(gyroAverage[Y]) > ATTITUDE_RESET_GYRO_LIMIT) || (fabsf(gyroAverage[Z]) > ATTITUDE_RESET_GYRO_LIMIT) - || (!useAcc)) { + || (!useAcc)) { gyroQuietPeriodTimeEnd = currentTimeUs + ATTITUDE_RESET_QUIET_TIME; attitudeResetTimeEnd = 0; - } + } } if (attitudeResetTimeEnd > 0) { // Resetting the attitude estimation if (currentTimeUs >= attitudeResetTimeEnd) { @@ -408,14 +408,14 @@ float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAverage) } else { attitudeResetActive = true; } - } else if ((gyroQuietPeriodTimeEnd > 0) && (currentTimeUs >= gyroQuietPeriodTimeEnd)) { + } else if ((gyroQuietPeriodTimeEnd > 0) && (currentTimeUs >= gyroQuietPeriodTimeEnd)) { // Start the high gain period to bring the estimation into convergence attitudeResetTimeEnd = currentTimeUs + ATTITUDE_RESET_ACTIVE_TIME; gyroQuietPeriodTimeEnd = 0; } } lastArmState = armState; - + if (attitudeResetActive) { ret = ATTITUDE_RESET_KP_GAIN; } else { @@ -451,7 +451,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) 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 + // 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) { @@ -532,7 +532,7 @@ bool shouldInitializeGPSHeading() return true; } - + return false; } diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 90bedf39a..b70bcdaf0 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -531,6 +531,9 @@ void gpsUpdate(timeUs_t currentTimeUs) if (sensors(SENSOR_GPS)) { updateGpsIndicator(currentTimeUs); } +#if defined(USE_GPS_RESCUE) + updateGPSRescueState(); +#endif } static void gpsNewData(uint16_t c) From f129f7913c5db4d14bd859df503890169c14e9b7 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 17 Jun 2018 02:35:53 +1200 Subject: [PATCH 2/2] Added injection for GPS rescue commands. --- src/main/fc/fc_core.c | 8 +++++++- src/main/fc/fc_rc.c | 5 ----- src/main/flight/gps_rescue.c | 11 ++++++++++- src/main/flight/gps_rescue.h | 4 ++-- 4 files changed, 19 insertions(+), 9 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index ff46f0f34..28cbeac0a 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -974,6 +974,7 @@ static FAST_CODE void subTaskMotorUpdate(timeUs_t currentTimeUs) static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); // If we're armed, at minimum throttle, and we do arming via the // sticks, do not process yaw input from the rx. We do this so the @@ -996,7 +997,12 @@ static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs) } processRcCommand(); - UNUSED(currentTimeUs); + +#if defined(USE_GPS_RESCUE) + if (FLIGHT_MODE(GPS_RESCUE_MODE)) { + gpsRescueInjectRcCommands(); + } +#endif } // Function for loop trigger diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 57aec5a68..68f66240e 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -43,7 +43,6 @@ #include "flight/failsafe.h" #include "flight/imu.h" -#include "flight/gps_rescue.h" #include "flight/pid.h" #include "pg/rx.h" #include "rx/rx.h" @@ -528,10 +527,6 @@ FAST_CODE FAST_CODE_NOINLINE void updateRcCommands(void) rcCommand[YAW] = rcCommandBuff.Z; } } - - if (FLIGHT_MODE(GPS_RESCUE_MODE)) { - rcCommand[THROTTLE] = rescueThrottle; - } } void resetYawAxis(void) diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 8ba62d210..9086e3105 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -71,6 +71,9 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, .minSats = 8 ); +static uint16_t rescueThrottle; +static uint16_t rescueYaw; + int32_t gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 }; uint16_t hoverThrottle = 0; float averageThrottle = 0.0; @@ -398,7 +401,13 @@ void setBearing(int16_t deg) dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed); - rcCommand[YAW] = - (dif * gpsRescueConfig()->yawP / 20); + rescueYaw = - (dif * gpsRescueConfig()->yawP / 20); +} + +void gpsRescueInjectRcCommands(void) +{ + rcCommand[THROTTLE] = rescueThrottle; + rcCommand[YAW] = rescueYaw; } #endif diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index 29128b67f..cfce5bd41 100644 --- a/src/main/flight/gps_rescue.h +++ b/src/main/flight/gps_rescue.h @@ -42,8 +42,6 @@ typedef struct gpsRescue_s { PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig); -uint16_t rescueThrottle; - typedef enum { RESCUE_IDLE, RESCUE_INITIALIZE, @@ -110,3 +108,5 @@ void performSanityChecks(void); void sensorUpdate(void); void rescueAttainPosition(void); + +void gpsRescueInjectRcCommands(void);