diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 9ee11c8fbc..d884aa72c2 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -941,7 +941,7 @@ const clivalue_t valueTable[] = { #ifdef USE_GPS { "gps_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_PROVIDER }, PG_GPS_CONFIG, offsetof(gpsConfig_t, provider) }, { "gps_sbas_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_SBAS_MODE }, PG_GPS_CONFIG, offsetof(gpsConfig_t, sbasMode) }, - { "gps_sbas_integrity", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, sbas_integrity) }, + { "gps_sbas_integrity", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, sbas_integrity) }, { "gps_auto_config", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, autoConfig) }, { "gps_auto_baud", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, autoBaud) }, { "gps_ublox_use_galileo", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_use_galileo) }, @@ -952,6 +952,7 @@ const clivalue_t valueTable[] = { #ifdef USE_GPS_RESCUE // PG_GPS_RESCUE { "gps_rescue_angle", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, angle) }, + { "gps_rescue_alt_buffer", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueAltitudeBufferM) }, { "gps_rescue_initial_alt", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 20, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, initialAltitudeM) }, { "gps_rescue_descent_dist", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 30, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descentDistanceM) }, { "gps_rescue_landing_alt", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 3, 10 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, targetLandingAltitudeM) }, diff --git a/src/main/cms/cms_menu_gps_rescue.c b/src/main/cms/cms_menu_gps_rescue.c index 654aa349fe..7ab4fc7224 100644 --- a/src/main/cms/cms_menu_gps_rescue.c +++ b/src/main/cms/cms_menu_gps_rescue.c @@ -41,6 +41,7 @@ static uint16_t gpsRescueConfig_angle; //degrees +static uint16_t gpsRescueConfig_rescueAltitudeBufferM; //meters static uint16_t gpsRescueConfig_initialAltitudeM; //meters static uint16_t gpsRescueConfig_descentDistanceM; //meters static uint16_t gpsRescueConfig_rescueGroundspeed; // centimeters per second @@ -128,6 +129,7 @@ static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp) UNUSED(pDisp); gpsRescueConfig_angle = gpsRescueConfig()->angle; + gpsRescueConfig_rescueAltitudeBufferM = gpsRescueConfig()->rescueAltitudeBufferM; gpsRescueConfig_initialAltitudeM = gpsRescueConfig()->initialAltitudeM; gpsRescueConfig_descentDistanceM = gpsRescueConfig()->descentDistanceM; gpsRescueConfig_rescueGroundspeed = gpsRescueConfig()->rescueGroundspeed; @@ -152,6 +154,7 @@ static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entr UNUSED(self); gpsRescueConfigMutable()->angle = gpsRescueConfig_angle; + gpsRescueConfigMutable()->rescueAltitudeBufferM = gpsRescueConfig_rescueAltitudeBufferM; gpsRescueConfigMutable()->initialAltitudeM = gpsRescueConfig_initialAltitudeM; gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM; gpsRescueConfigMutable()->rescueGroundspeed = gpsRescueConfig_rescueGroundspeed; @@ -178,6 +181,7 @@ const OSD_Entry cmsx_menuGpsRescueEntries[] = { "MIN DIST HOME M", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_minRescueDth, 50, 1000 ,1 }, REBOOT_REQUIRED }, { "INITAL ALT M", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_initialAltitudeM, 20, 100, 1 }, REBOOT_REQUIRED }, { "ALTITUDE MODE" , OME_TAB, NULL, &(OSD_TAB_t) { &gpsRescueConfig_altitudeMode, 2, lookupTableRescueAltitudeMode}, REBOOT_REQUIRED }, + { "ALT BUFFER M", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_rescueAltitudeBufferM, 0, 100, 1 }, REBOOT_REQUIRED }, { "DESCENT DIST M", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descentDistanceM, 30, 500, 1 }, REBOOT_REQUIRED }, { "LANDING ALT M", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_targetLandingAltitudeM, 3, 10, 1 }, REBOOT_REQUIRED }, { "LANDING DIST M", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_targetLandingDistanceM, 5, 15, 1 }, REBOOT_REQUIRED }, diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index aa633b0fdd..f3db7e2dbe 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -155,6 +155,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCU PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, .angle = 32, .initialAltitudeM = 50, + .rescueAltitudeBufferM = 15, .descentDistanceM = 200, .rescueGroundspeed = 2000, .throttleP = 150, @@ -606,11 +607,11 @@ void updateGPSRescueState(void) newAltitude = gpsRescueConfig()->initialAltitudeM * 100; break; case CURRENT_ALT: - newAltitude = rescueState.sensor.currentAltitudeCm; + newAltitude = rescueState.sensor.currentAltitudeCm + gpsRescueConfig()->rescueAltitudeBufferM * 100; break; case MAX_ALT: default: - newAltitude = MAX(gpsRescueConfig()->initialAltitudeM * 100, rescueState.sensor.maxAltitudeCm + 1500); + newAltitude = MAX(gpsRescueConfig()->initialAltitudeM * 100, rescueState.sensor.maxAltitudeCm + gpsRescueConfig()->rescueAltitudeBufferM * 100); break; } @@ -746,4 +747,3 @@ bool gpsRescueDisableMag(void) } #endif #endif - diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index 31113287d0..f29eafa482 100644 --- a/src/main/flight/gps_rescue.h +++ b/src/main/flight/gps_rescue.h @@ -24,6 +24,7 @@ typedef struct gpsRescue_s { uint16_t angle; //degrees uint16_t initialAltitudeM; //meters + uint16_t rescueAltitudeBufferM; //meters uint16_t descentDistanceM; //meters uint16_t rescueGroundspeed; //centimeters per second uint16_t throttleP, throttleI, throttleD; diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 9720c9c067..24b714e902 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1384,6 +1384,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) #ifdef USE_GPS_RESCUE case MSP_GPS_RESCUE: sbufWriteU16(dst, gpsRescueConfig()->angle); + sbufWriteU16(dst, gpsRescueConfig()->rescueAltitudeBufferM); sbufWriteU16(dst, gpsRescueConfig()->initialAltitudeM); sbufWriteU16(dst, gpsRescueConfig()->descentDistanceM); sbufWriteU16(dst, gpsRescueConfig()->rescueGroundspeed); @@ -2324,6 +2325,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, case MSP_SET_GPS_RESCUE: gpsRescueConfigMutable()->angle = sbufReadU16(src); gpsRescueConfigMutable()->initialAltitudeM = sbufReadU16(src); + gpsRescueConfigMutable()->rescueAltitudeBufferM = sbufReadU16(src); gpsRescueConfigMutable()->descentDistanceM = sbufReadU16(src); gpsRescueConfigMutable()->rescueGroundspeed = sbufReadU16(src); gpsRescueConfigMutable()->throttleMin = sbufReadU16(src);