From d535fd61806cbaef8316468c7151e5ed8c6ee187 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sat, 26 May 2018 15:28:08 +1200 Subject: [PATCH] Renamed 'gpsRescue' to 'gpsRescueConfig' and moved it into the appropriate location. --- src/main/fc/fc_core.c | 2 +- src/main/flight/gps_rescue.c | 77 +++++++++++++++++++++++------------ src/main/flight/gps_rescue.h | 26 +++++++++++- src/main/interface/cli.c | 2 +- src/main/interface/settings.c | 35 ++++++++-------- src/main/io/gps.c | 25 ------------ src/main/io/gps.h | 27 ------------ 7 files changed, 97 insertions(+), 97 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index c6d4026a7e..07d13bcd48 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -257,7 +257,7 @@ void updateArmingStatus(void) #ifdef USE_GPS_RESCUE if (isModeActivationConditionPresent(BOXGPSRESCUE)) { - if (rescueState.sensor.numSat < gpsRescue()->minSats) { + if (rescueState.sensor.numSat < gpsRescueConfig()->minSats) { setArmingDisabled(ARMING_DISABLED_GPS); } else { unsetArmingDisabled(ARMING_DISABLED_GPS); diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 1f78f34643..0db7c749dd 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -18,32 +18,59 @@ #include #include -#include "common/axis.h" -#include "common/maths.h" - -#include "build/debug.h" - -#include "drivers/time.h" +#include "platform.h" #ifdef USE_GPS_RESCUE +#include "build/debug.h" + +#include "common/axis.h" +#include "common/maths.h" + +#include "drivers/time.h" + #include "io/gps.h" -#include "fc/fc_core.h" -#include "fc/runtime_config.h" #include "fc/config.h" +#include "fc/fc_core.h" #include "fc/rc_controls.h" +#include "fc/runtime_config.h" -#include "flight/position.h" #include "flight/failsafe.h" -#include "flight/gps_rescue.h" #include "flight/imu.h" #include "flight/pid.h" +#include "flight/position.h" + +#include "pg/pg.h" +#include "pg/pg_ids.h" #include "rx/rx.h" #include "sensors/acceleration.h" +#include "gps_rescue.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0); + +PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, + .angle = 32, + .initialAltitude = 50, + .descentDistance = 200, + .rescueGroundspeed = 2000, + .throttleP = 150, + .throttleI = 20, + .throttleD = 50, + .velP = 80, + .velI = 20, + .velD = 15, + .yawP = 40, + .throttleMin = 1200, + .throttleMax = 1600, + .throttleHover = 1280, + .sanityChecks = 0, + .minSats = 8 +); + int32_t gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 }; uint16_t hoverThrottle = 0; float averageThrottle = 0.0; @@ -86,7 +113,7 @@ void updateGPSRescueState(void) break; case RESCUE_INITIALIZE: if (hoverThrottle == 0) { //no actual throttle data yet, let's use the default. - hoverThrottle = gpsRescue()->throttleHover; + hoverThrottle = gpsRescueConfig()->throttleHover; } rescueState.phase = RESCUE_ATTAIN_ALT; @@ -98,23 +125,23 @@ void updateGPSRescueState(void) } rescueState.intent.targetGroundspeed = 500; - rescueState.intent.targetAltitude = MAX(gpsRescue()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500); + 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 < gpsRescue()->descentDistance) { + 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 = gpsRescue()->rescueGroundspeed; - rescueState.intent.targetAltitude = MAX(gpsRescue()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500); + 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 = gpsRescue()->angle; + 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 @@ -123,8 +150,8 @@ void updateGPSRescueState(void) } // 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 = gpsRescue()->initialAltitude * 100 * rescueState.sensor.distanceToHome / gpsRescue()->descentDistance; - int32_t newSpeed = gpsRescue()->rescueGroundspeed * rescueState.sensor.distanceToHome / gpsRescue()->descentDistance; + 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); @@ -204,8 +231,8 @@ void performSanityChecks() // Do not abort until each of these items is fully tested if (rescueState.failure != RESCUE_HEALTHY) { - if (gpsRescue()->sanityChecks == RESCUE_SANITY_ON - || (gpsRescue()->sanityChecks == RESCUE_SANITY_FS_ONLY && rescueState.isFailsafe == true)) { + if (gpsRescueConfig()->sanityChecks == RESCUE_SANITY_ON + || (gpsRescueConfig()->sanityChecks == RESCUE_SANITY_FS_ONLY && rescueState.isFailsafe == true)) { rescueState.phase = RESCUE_ABORT; } } @@ -241,7 +268,7 @@ void performSanityChecks() // Minimum sat detection static int8_t msI = 0; - msI = constrain(msI + (rescueState.sensor.numSat < gpsRescue()->minSats) ? 1 : -1, -5, 5); + msI = constrain(msI + (rescueState.sensor.numSat < gpsRescueConfig()->minSats) ? 1 : -1, -5, 5); if (msI == 5) { rescueState.failure = RESCUE_FLYAWAY; @@ -317,7 +344,7 @@ void rescueAttainPosition() previousSpeedError = speedError; - int16_t angleAdjustment = gpsRescue()->velP * speedError + (gpsRescue()->velI * speedIntegral) / 100 + gpsRescue()->velD * speedDerivative; + 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); @@ -341,10 +368,10 @@ void rescueAttainPosition() previousAltitudeError = altitudeError; - int16_t altitudeAdjustment = (gpsRescue()->throttleP * altitudeError + (gpsRescue()->throttleI * altitudeIntegral) / 10 * + gpsRescue()->throttleD * altitudeDerivative) / ct / 20; + 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, gpsRescue()->throttleMin, gpsRescue()->throttleMax); + rescueThrottle = constrain(1000 + altitudeAdjustment + hoverAdjustment, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax); DEBUG_SET(DEBUG_RTH, 0, rescueThrottle); DEBUG_SET(DEBUG_RTH, 1, gpsRescueAngle[AI_PITCH]); @@ -364,7 +391,7 @@ void setBearing(int16_t deg) dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed); - rcCommand[YAW] = - (dif * gpsRescue()->yawP / 20); + rcCommand[YAW] = - (dif * gpsRescueConfig()->yawP / 20); } #endif diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index dc9a6c7749..29128b67fe 100644 --- a/src/main/flight/gps_rescue.h +++ b/src/main/flight/gps_rescue.h @@ -16,7 +16,31 @@ */ #include "common/axis.h" -#include "io/gps.h" + +#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 initialAltitude; //meters + uint16_t descentDistance; //meters + uint16_t rescueGroundspeed; // centimeters per second + uint16_t throttleP, throttleI, throttleD; + uint16_t yawP; + uint16_t throttleMin; + uint16_t throttleMax; + uint16_t throttleHover; + uint16_t velP, velI, velD; + uint8_t minSats; + gpsRescueSanity_e sanityChecks; +} gpsRescueConfig_t; + +PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig); uint16_t rescueThrottle; diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index fc50aa3167..b08bc72a07 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -92,11 +92,11 @@ extern uint8_t __config_end; #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "flight/position.h" #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/pid.h" +#include "flight/position.h" #include "flight/servos.h" #include "interface/cli.h" diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 3d38add3b0..9efedc4bb9 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -45,11 +45,12 @@ #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" -#include "flight/position.h" #include "flight/failsafe.h" +#include "flight/gps_rescue.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/pid.h" +#include "flight/position.h" #include "flight/servos.h" #include "interface/settings.h" @@ -695,23 +696,23 @@ const clivalue_t valueTable[] = { #ifdef USE_GPS_RESCUE // PG_GPS_RESCUE - { "gps_rescue_angle", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, angle) }, - { "gps_rescue_initial_alt", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 20, 100 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, initialAltitude) }, - { "gps_rescue_descent_dist", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, descentDistance) }, - { "gps_rescue_ground_speed", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 3000 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, rescueGroundspeed) }, - { "gps_rescue_throttle_P", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, throttleP) }, - { "gps_rescue_throttle_I", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, throttleI) }, - { "gps_rescue_throttle_D", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, throttleD) }, - { "gps_rescue_velocity_P", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, velP) }, - { "gps_rescue_velocity_I", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, velI) }, - { "gps_rescue_velocity_D", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, velD) }, - { "gps_rescue_yaw_P", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, yawP) }, + { "gps_rescue_angle", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, angle) }, + { "gps_rescue_initial_alt", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 20, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, initialAltitude) }, + { "gps_rescue_descent_dist", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descentDistance) }, + { "gps_rescue_ground_speed", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 3000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueGroundspeed) }, + { "gps_rescue_throttle_P", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleP) }, + { "gps_rescue_throttle_I", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleI) }, + { "gps_rescue_throttle_D", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleD) }, + { "gps_rescue_velocity_P", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velP) }, + { "gps_rescue_velocity_I", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velI) }, + { "gps_rescue_velocity_D", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velD) }, + { "gps_rescue_yaw_P", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, yawP) }, - { "gps_rescue_throttle_min", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, throttleMin) }, - { "gps_rescue_throttle_max", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, throttleMax) }, - { "gps_rescue_throttle_hover", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, throttleHover) }, - { "gps_rescue_sanity_checks", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE }, PG_GPS_RESCUE, offsetof(gpsRescue_t, sanityChecks) }, - { "gps_rescue_min_sats", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 50 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, minSats) }, + { "gps_rescue_throttle_min", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMin) }, + { "gps_rescue_throttle_max", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMax) }, + { "gps_rescue_throttle_hover", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleHover) }, + { "gps_rescue_sanity_checks", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, sanityChecks) }, + { "gps_rescue_min_sats", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 50 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minSats) }, #endif #endif diff --git a/src/main/io/gps.c b/src/main/io/gps.c index f5732a661a..a566b3f7bc 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -220,10 +220,6 @@ gpsData_t gpsData; PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); -#ifdef USE_GPS_RESCUE -PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescue_t, gpsRescue, PG_GPS_RESCUE, 0); -#endif - PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, .provider = GPS_NMEA, .sbasMode = SBAS_AUTO, @@ -231,27 +227,6 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, .autoBaud = GPS_AUTOBAUD_OFF ); -#ifdef USE_GPS_RESCUE -PG_RESET_TEMPLATE(gpsRescue_t, gpsRescue, - .angle = 32, - .initialAltitude = 50, - .descentDistance = 200, - .rescueGroundspeed = 2000, - .throttleP = 150, - .throttleI = 20, - .throttleD = 50, - .velP = 80, - .velI = 20, - .velD = 15, - .yawP = 40, - .throttleMin = 1200, - .throttleMax = 1600, - .throttleHover = 1280, - .sanityChecks = 0, - .minSats = 8 -); -#endif - static void shiftPacketLog(void) { uint32_t i; diff --git a/src/main/io/gps.h b/src/main/io/gps.h index cc48c20ecd..8b3cea4151 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -65,12 +65,6 @@ typedef enum { GPS_AUTOBAUD_ON } gpsAutoBaud_e; -typedef enum { - RESCUE_SANITY_OFF = 0, - RESCUE_SANITY_ON, - RESCUE_SANITY_FS_ONLY -} gpsRescueSanity_e; - #define GPS_BAUDRATE_MAX GPS_BAUDRATE_9600 typedef struct gpsConfig_s { @@ -82,27 +76,6 @@ typedef struct gpsConfig_s { PG_DECLARE(gpsConfig_t, gpsConfig); -#ifdef USE_GPS_RESCUE - -typedef struct gpsRescue_s { - uint16_t angle; //degrees - uint16_t initialAltitude; //meters - uint16_t descentDistance; //meters - uint16_t rescueGroundspeed; // centimeters per second - uint16_t throttleP, throttleI, throttleD; - uint16_t yawP; - uint16_t throttleMin; - uint16_t throttleMax; - uint16_t throttleHover; - uint16_t velP, velI, velD; - uint8_t minSats; - gpsRescueSanity_e sanityChecks; -} gpsRescue_t; - -PG_DECLARE(gpsRescue_t, gpsRescue); - -#endif - typedef struct gpsCoordinateDDDMMmmmm_s { int16_t dddmm; int16_t mmmm;