diff --git a/make/source.mk b/make/source.mk index f84c9b25f..d1e6afdb7 100644 --- a/make/source.mk +++ b/make/source.mk @@ -123,6 +123,7 @@ COMMON_SRC = \ cms/cms_menu_blackbox.c \ cms/cms_menu_builtin.c \ cms/cms_menu_failsafe.c \ + cms/cms_menu_gps_rescue.c\ cms/cms_menu_imu.c \ cms/cms_menu_ledstrip.c \ cms/cms_menu_misc.c \ @@ -302,6 +303,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ cms/cms_menu_blackbox.c \ cms/cms_menu_builtin.c \ cms/cms_menu_failsafe.c \ + cms/cms_menu_gps_rescue.c\ cms/cms_menu_imu.c \ cms/cms_menu_ledstrip.c \ cms/cms_menu_misc.c \ diff --git a/src/main/cms/cms_menu_failsafe.c b/src/main/cms/cms_menu_failsafe.c index e3a9541af..59cc5b274 100644 --- a/src/main/cms/cms_menu_failsafe.c +++ b/src/main/cms/cms_menu_failsafe.c @@ -31,6 +31,10 @@ #include "cms/cms_types.h" #include "cms/cms_menu_failsafe.h" +#ifdef USE_CMS_GPS_RESCUE_MENU +#include "cms/cms_menu_gps_rescue.h" +#endif + #include "config/feature.h" #include "fc/config.h" @@ -73,6 +77,9 @@ static const OSD_Entry cmsx_menuFailsafeEntries[] = { "GUARD TIME", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &failsafeConfig_failsafe_delay, 0, 200, 1, 100 }, 0 }, { "STAGE 2 DELAY", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &failsafeConfig_failsafe_off_delay, 0, 200, 1, 100 }, 0 }, { "STAGE 2 THROTTLE", OME_UINT16, NULL, &(OSD_UINT16_t) { &failsafeConfig_failsafe_throttle, PWM_PULSE_MIN, PWM_PULSE_MAX, 1 }, 0 }, +#ifdef USE_CMS_GPS_RESCUE_MENU + { "GPS RESCUE", OME_Submenu, cmsMenuChange, &cmsx_menuGpsRescue, 0}, +#endif { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } }; diff --git a/src/main/cms/cms_menu_gps_rescue.c b/src/main/cms/cms_menu_gps_rescue.c new file mode 100644 index 000000000..9a7e82c7b --- /dev/null +++ b/src/main/cms/cms_menu_gps_rescue.c @@ -0,0 +1,183 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#ifdef USE_CMS_GPS_RESCUE_MENU + +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_gps_rescue.h" + +#include "config/feature.h" + +#include "fc/config.h" + +#include "flight/gps_rescue.h" + + +static uint16_t gpsRescueConfig_angle; //degrees +static uint16_t gpsRescueConfig_initialAltitudeM; //meters +static uint16_t gpsRescueConfig_descentDistanceM; //meters +static uint16_t gpsRescueConfig_rescueGroundspeed; // centimeters per second +static uint16_t gpsRescueConfig_throttleMin; +static uint16_t gpsRescueConfig_throttleMax; +static uint16_t gpsRescueConfig_throttleHover; +static uint8_t gpsRescueConfig_minSats; +static uint16_t gpsRescueConfig_minRescueDth; //meters +static uint8_t gpsRescueConfig_allowArmingWithoutFix; +static uint16_t gpsRescueConfig_throttleP, gpsRescueConfig_throttleI, gpsRescueConfig_throttleD; +static uint16_t gpsRescueConfig_velP, gpsRescueConfig_velI, gpsRescueConfig_velD; +static uint16_t gpsRescueConfig_yawP; + + +static long cms_menuGpsRescuePidOnEnter(void) +{ + + gpsRescueConfig_throttleP = gpsRescueConfig()->throttleP; + gpsRescueConfig_throttleI = gpsRescueConfig()->throttleI; + gpsRescueConfig_throttleD = gpsRescueConfig()->throttleD; + + gpsRescueConfig_yawP = gpsRescueConfig()->yawP; + + gpsRescueConfig_velP = gpsRescueConfig()->velP; + gpsRescueConfig_velI = gpsRescueConfig()->velI; + gpsRescueConfig_velD = gpsRescueConfig()->velD; + + return 0; +} + +static long cms_menuGpsRescuePidOnExit(const OSD_Entry *self) +{ + UNUSED(self); + + gpsRescueConfigMutable()->throttleP = gpsRescueConfig_throttleP; + gpsRescueConfigMutable()->throttleI = gpsRescueConfig_throttleI; + gpsRescueConfigMutable()->throttleD = gpsRescueConfig_throttleD; + + gpsRescueConfigMutable()->yawP = gpsRescueConfig_yawP; + + gpsRescueConfigMutable()->velP = gpsRescueConfig_velP; + gpsRescueConfigMutable()->velI = gpsRescueConfig_velI; + gpsRescueConfigMutable()->velD = gpsRescueConfig_velD; + + return 0; +} + +const OSD_Entry cms_menuGpsRescuePidEntries[] = +{ + {"--- GPS RESCUE PID---", OME_Label, NULL, NULL, 0}, + + { "THROTTLE P", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleP, 0, 500, 1 }, 0 }, + { "THROTTLE I", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleI, 0, 500, 1 }, 0 }, + { "THROTTLE D", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleD, 0, 500, 1 }, 0 }, + + { "YAW P", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_yawP, 0, 500, 1 }, 0 }, + + { "VELOCITY P", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_velP, 0, 500, 1 }, 0 }, + { "VELOCITY I", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_velI, 0, 500, 1 }, 0 }, + { "VELOCITY D", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_velD, 0, 500, 1 }, 0 }, + + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu cms_menuGpsRescuePid = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "MENUGPSRPID", + .GUARD_type = OME_MENU, +#endif + .onEnter = cms_menuGpsRescuePidOnEnter, + .onExit = cms_menuGpsRescuePidOnExit, + .entries = cms_menuGpsRescuePidEntries, +}; + +static long cmsx_menuGpsRescueOnEnter(void) +{ + + gpsRescueConfig_angle = gpsRescueConfig()->angle; + gpsRescueConfig_initialAltitudeM = gpsRescueConfig()->initialAltitudeM; + gpsRescueConfig_descentDistanceM = gpsRescueConfig()->descentDistanceM; + gpsRescueConfig_rescueGroundspeed = gpsRescueConfig()->rescueGroundspeed; + gpsRescueConfig_throttleMin = gpsRescueConfig()->throttleMin ; + gpsRescueConfig_throttleMax = gpsRescueConfig()->throttleMax; + gpsRescueConfig_throttleHover = gpsRescueConfig()->throttleHover; + gpsRescueConfig_minSats = gpsRescueConfig()->minSats; + gpsRescueConfig_minRescueDth = gpsRescueConfig()->minRescueDth; + gpsRescueConfig_allowArmingWithoutFix = gpsRescueConfig()->allowArmingWithoutFix; + + return 0; +} + +static long cmsx_menuGpsRescueOnExit(const OSD_Entry *self) +{ + UNUSED(self); + + + gpsRescueConfigMutable()->angle = gpsRescueConfig_angle; + gpsRescueConfigMutable()->initialAltitudeM = gpsRescueConfig_initialAltitudeM; + gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM; + gpsRescueConfigMutable()->rescueGroundspeed = gpsRescueConfig_rescueGroundspeed; + gpsRescueConfigMutable()->throttleMin = gpsRescueConfig_throttleMin; + gpsRescueConfigMutable()->throttleMax = gpsRescueConfig_throttleMax; + gpsRescueConfigMutable()->throttleHover = gpsRescueConfig_throttleHover; + gpsRescueConfigMutable()->minSats = gpsRescueConfig_minSats; + gpsRescueConfigMutable()->minRescueDth = gpsRescueConfig_minRescueDth; + gpsRescueConfigMutable()->allowArmingWithoutFix = gpsRescueConfig_allowArmingWithoutFix; + + return 0; +} + +const OSD_Entry cmsx_menuGpsRescueEntries[] = +{ + {"--- GPS RESCUE ---", OME_Label, NULL, NULL, 0}, + + { "ANGLE", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_angle, 0, 200 ,1 }, 0 }, + { "MIN DIST HOME M", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_minRescueDth, 50, 1000 ,1 }, 0 }, + { "INITAL ALT M", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_initialAltitudeM, 20, 100, 1 }, 0 }, + { "DESCENT DIST M", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descentDistanceM, 30, 500, 1 }, 0 }, + { "GROUND SPEED C/M", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_rescueGroundspeed, 30, 3000, 1 }, 0 }, + { "THROTTLE MIN", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleMin, 1000, 2000, 1 }, 0 }, + { "THROTTLE MAX", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleMax, 1000, 2000, 1 }, 0 }, + { "THROTTLE HOV", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleHover, 1000, 2000, 1 }, 0 }, + { "ARM WITHOUT FIX", OME_Bool, NULL, &gpsRescueConfig_allowArmingWithoutFix, 0 }, + { "MIN SATELITES", OME_UINT8, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_minSats, 5, 50, 1 }, 0 }, + { "GPS RESCUE PID", OME_Submenu, cmsMenuChange, &cms_menuGpsRescuePid, 0}, + + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu cmsx_menuGpsRescue = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "MENUGPSRES", + .GUARD_type = OME_MENU, +#endif + .onEnter = cmsx_menuGpsRescueOnEnter, + .onExit = cmsx_menuGpsRescueOnExit, + .entries = cmsx_menuGpsRescueEntries, +}; + +#endif diff --git a/src/main/cms/cms_menu_gps_rescue.h b/src/main/cms/cms_menu_gps_rescue.h new file mode 100644 index 000000000..02f5e7a21 --- /dev/null +++ b/src/main/cms/cms_menu_gps_rescue.h @@ -0,0 +1,23 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +extern CMS_Menu cmsx_menuGpsRescue; diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index ba2b26ff1..1dd29e6f9 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -280,6 +280,10 @@ #undef USE_ACRO_TRAINER #endif +#if (!defined(USE_GPS_RESCUE) || !defined(USE_CMS_FAILSAFE_MENU)) +#undef USE_CMS_GPS_RESCUE_MENU +#endif + #ifndef USE_BEEPER #undef BEEPER_PIN #undef BEEPER_PWM_HZ diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index 46beae91e..e65a3248f 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -296,6 +296,8 @@ #define USE_ESCSERIAL_SIMONK #define USE_SERIAL_4WAY_SK_BOOTLOADER #define USE_CMS_FAILSAFE_MENU +#define USE_CMS_GPS_RESCUE_MENU +#define USE_SMART_FEEDFORWARD #define USE_TELEMETRY_SENSORS_DISABLED_DETAILS // Re-enable this after 4.0 has been released, and remove the define from STM32F4DISCOVERY //#define USE_VTX_TABLE