Skip to content

Commit

Permalink
added all gprescue/failsafe option in osd
Browse files Browse the repository at this point in the history
  • Loading branch information
@loutwice
loutwice committed Sep 5, 2019
1 parent 8561f46 commit 2e31714
Show file tree
Hide file tree
Showing 9 changed files with 58 additions and 41 deletions.
2 changes: 2 additions & 0 deletions make/source.mk
View file
Expand Up @@ -117,6 +117,8 @@ COMMON_SRC = \
cms/cms.c \
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 \
Expand Down
2 changes: 1 addition & 1 deletion src/main/cms/cms_menu_builtin.c
View file
Expand Up @@ -121,7 +121,7 @@ static OSD_Entry menuFeaturesEntries[] =
{"LED STRIP", OME_Submenu, cmsMenuChange, &cmsx_menuLedstrip, 0},
#endif // LED_STRIP
#ifdef USE_CMS_FAILSAFE_MENU
{"FAILSAFE", OME_Submenu, cmsMenuChange, &cmsx_menuFailsafe, 0},
{"FAILSAFE", OME_Submenu, cmsMenuChange, &cmsx_menuFailsafe, 0},
#endif
{"POWER", OME_Submenu, cmsMenuChange, &cmsx_menuPower, 0},
{ "SAVE&EXIT", OME_OSD_Exit, cmsMenuExit, (void *)CMS_EXIT_SAVE, 0},
Expand Down
13 changes: 7 additions & 6 deletions src/main/cms/cms_menu_failsafe.c
View file
Expand Up @@ -69,17 +69,18 @@ static long cmsx_Failsafe_onExit(const OSD_Entry *self)
return 0;
}

static const OSD_Entry cmsx_menuFailsafeEntries[] =
static OSD_Entry cmsx_menuFailsafeEntries[] =
{
{ "-- FAILSAFE --", OME_Label, NULL, NULL, 0},

{ "PROCEDURE", OME_TAB, NULL, &(OSD_TAB_t) { &failsafeConfig_failsafe_procedure, FAILSAFE_PROCEDURE_COUNT - 1, failsafeProcedureNames }, REBOOT_REQUIRED },
{ "GUARD TIME", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &failsafeConfig_failsafe_delay, 0, 200, 1, 100 }, REBOOT_REQUIRED },
{ "STAGE 2 DELAY", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &failsafeConfig_failsafe_off_delay, 0, 200, 1, 100 }, REBOOT_REQUIRED },
{ "STAGE 2 THROTTLE", OME_UINT16, NULL, &(OSD_UINT16_t) { &failsafeConfig_failsafe_throttle, PWM_PULSE_MIN, PWM_PULSE_MAX, 1 }, REBOOT_REQUIRED },
{ "-- CHANGES REQUIRE REBOOT --", OME_Label, NULL, NULL, 0 },
{ "PROCEDURE", OME_TAB, NULL, &(OSD_TAB_t) { &failsafeConfig_failsafe_procedure, FAILSAFE_PROCEDURE_COUNT - 1, failsafeProcedureNames }, 0 },
{ "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
{ "SAVE&EXIT", OME_OSD_Exit, cmsMenuExit, (void *)CMS_EXIT_SAVEREBOOT, 0},
{ "BACK", OME_Back, NULL, NULL, 0 },
{ NULL, OME_END, NULL, NULL, 0 }
};
Expand Down
58 changes: 28 additions & 30 deletions src/main/cms/cms_menu_gps_rescue.c
View file
Expand Up @@ -27,7 +27,7 @@

#ifdef USE_CMS_GPS_RESCUE_MENU

#include "cli/settings.h"
#include "interface/settings.h"

#include "cms/cms.h"
#include "cms/cms_types.h"
Expand All @@ -49,12 +49,10 @@ 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 uin16_t gpsConfig_distanceLimit;
static uint16_t gpsConfig_distanceLimit;


static long cms_menuGpsRescuePidOnEnter(void)
Expand Down Expand Up @@ -91,20 +89,20 @@ static long cms_menuGpsRescuePidOnExit(const OSD_Entry *self)
return 0;
}

const OSD_Entry cms_menuGpsRescuePidEntries[] =
static OSD_Entry cms_menuGpsRescuePidEntries[] =
{
{"--- GPS RESCUE PID---", OME_Label, NULL, NULL, 0},
{ "-- CHANGES REQUIRE REBOOT --", 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 },

{ "THROTTLE P", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleP, 0, 500, 1 }, REBOOT_REQUIRED },
{ "THROTTLE I", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleI, 0, 500, 1 }, REBOOT_REQUIRED },
{ "THROTTLE D", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleD, 0, 500, 1 }, REBOOT_REQUIRED },

{ "YAW P", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_yawP, 0, 500, 1 }, REBOOT_REQUIRED },

{ "VELOCITY P", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_velP, 0, 500, 1 }, REBOOT_REQUIRED },
{ "VELOCITY I", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_velI, 0, 500, 1 }, REBOOT_REQUIRED },
{ "VELOCITY D", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_velD, 0, 500, 1 }, REBOOT_REQUIRED },
{ "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 },
{ "SAVE&EXIT", OME_OSD_Exit, cmsMenuExit, (void *)CMS_EXIT_SAVEREBOOT, 0},
{"BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
};
Expand All @@ -130,7 +128,7 @@ static long cmsx_menuGpsRescueOnEnter(void)
gpsRescueConfig_throttleMax = gpsRescueConfig()->throttleMax;
gpsRescueConfig_throttleHover = gpsRescueConfig()->throttleHover;
gpsRescueConfig_minSats = gpsRescueConfig()->minSats;
gpsConfig_distanceLimit = gpsConfig()->distanceLimit;
gpsConfig_distanceLimit = gpsConfig()->distanceLimit;


return 0;
Expand All @@ -142,35 +140,35 @@ static long cmsx_menuGpsRescueOnExit(const OSD_Entry *self)


gpsRescueConfigMutable()->angle = gpsRescueConfig_angle;
gpsRescueConfigMutable()->initialAltitudeM = gpsRescueConfig_initialAltitudeM;
gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM;
gpsRescueConfigMutable()->initialAltitude = gpsRescueConfig_initialAltitudeM;
gpsRescueConfigMutable()->descentDistance = gpsRescueConfig_descentDistanceM;
gpsRescueConfigMutable()->rescueGroundspeed = gpsRescueConfig_rescueGroundspeed;
gpsRescueConfigMutable()->throttleMin = gpsRescueConfig_throttleMin;
gpsRescueConfigMutable()->throttleMax = gpsRescueConfig_throttleMax;
gpsRescueConfigMutable()->throttleHover = gpsRescueConfig_throttleHover;
gpsRescueConfigMutable()->minSats = gpsRescueConfig_minSats;
gpsConfig()->distanceLimit = gpsConfig_distanceLimit;
gpsConfigMutable()->distanceLimit = gpsConfig_distanceLimit;


return 0;
}

const OSD_Entry cmsx_menuGpsRescueEntries[] =
static OSD_Entry cmsx_menuGpsRescueEntries[] =
{
{"--- GPS RESCUE ---", OME_Label, NULL, NULL, 0},

{ "ANGLE", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_angle, 0, 200 ,1 }, REBOOT_REQUIRED },
{ "INITAL ALT M", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_initialAltitudeM, 20, 100, 1 }, REBOOT_REQUIRED },
{ "DESCENT DIST M", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descentDistanceM, 30, 500, 1 }, REBOOT_REQUIRED },
{ "GROUND SPEED CM/S", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_rescueGroundspeed, 30, 3000, 1 }, REBOOT_REQUIRED },
{ "THROTTLE MIN", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleMin, 1000, 2000, 1 }, REBOOT_REQUIRED },
{ "THROTTLE MAX", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleMax, 1000, 2000, 1 }, REBOOT_REQUIRED },
{ "THROTTLE HOV", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleHover, 1000, 2000, 1 }, REBOOT_REQUIRED },
{ "MIN SATELITES", OME_UINT8, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_minSats, 5, 50, 1 }, REBOOT_REQUIRED },
{ "DISTANCE LIMIT", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsConfig_distanceLimit, 0, 500, 10 }, REBOOT_REQUIRED },
{ "-- CHANGES REQUIRE REBOOT --", OME_Label, NULL, NULL, 0 },
{ "ANGLE", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_angle, 0, 200 ,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 CM/S", 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 },
{ "MIN SATELITES", OME_UINT8, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_minSats, 5, 50, 1 }, 0 },
{ "DISTANCE LIMIT", OME_UINT16, NULL, &(OSD_UINT16_t){ &gpsConfig_distanceLimit, 0, 500, 10 }, 0 },
{ "GPS RESCUE PID", OME_Submenu, cmsMenuChange, &cms_menuGpsRescuePid, 0},


{ "SAVE&EXIT", OME_OSD_Exit, cmsMenuExit, (void *)CMS_EXIT_SAVEREBOOT, 0},
{"BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
};
Expand Down
1 change: 1 addition & 0 deletions src/main/cms/cms_types.h
View file
Expand Up @@ -71,6 +71,7 @@ typedef struct
#define DYNAMIC 0x04 // Value should be updated dynamically
#define OPTSTRING 0x08 // (Temporary) Flag for OME_Submenu, indicating func should be called to get a string to display.


#define IS_PRINTVALUE(p) ((p)->flags & PRINT_VALUE)
#define SET_PRINTVALUE(p) { (p)->flags |= PRINT_VALUE; }
#define CLR_PRINTVALUE(p) { (p)->flags &= ~PRINT_VALUE; }
Expand Down
10 changes: 9 additions & 1 deletion src/main/flight/failsafe.c
View file
Expand Up @@ -72,6 +72,14 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
.failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT // default full failsafe procedure is 0: auto-landing
);

const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = {
"AUTO-LAND",
"DROP",
#ifdef USE_GPS_RESCUE
"GPS-RESCUE",
#endif
};

/*
* Should called when the failsafe config needs to be changed - e.g. a different profile has been selected.
*/
Expand Down Expand Up @@ -126,7 +134,7 @@ static void failsafeActivate(void)
failsafeState.active = true;

failsafeState.phase = FAILSAFE_LANDING;

ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;

Expand Down
9 changes: 7 additions & 2 deletions src/main/flight/failsafe.h
View file
Expand Up @@ -61,8 +61,13 @@ typedef enum {
typedef enum {
FAILSAFE_PROCEDURE_AUTO_LANDING = 0,
FAILSAFE_PROCEDURE_DROP_IT,
FAILSAFE_PROCEDURE_GPS_RESCUE
} failsafeProcedure_e;
#ifdef USE_GPS_RESCUE
FAILSAFE_PROCEDURE_GPS_RESCUE,
#endif
FAILSAFE_PROCEDURE_COUNT // must be last
} failsafeProcedure_e;

extern const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT];

typedef enum {
FAILSAFE_SWITCH_MODE_STAGE1 = 0,
Expand Down
2 changes: 2 additions & 0 deletions src/main/interface/msp.c
View file
Expand Up @@ -1048,6 +1048,7 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, gpsConfig()->sbasMode);
sbufWriteU8(dst, gpsConfig()->autoConfig);
sbufWriteU8(dst, gpsConfig()->autoBaud);
sbufWriteU16(dst, gpsConfig()->distanceLimit);
break;

case MSP_RAW_GPS:
Expand Down Expand Up @@ -1739,6 +1740,7 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
gpsConfigMutable()->sbasMode = sbufReadU8(src);
gpsConfigMutable()->autoConfig = sbufReadU8(src);
gpsConfigMutable()->autoBaud = sbufReadU8(src);
gpsConfigMutable()->distanceLimit = sbufReadU16(src);
break;

#ifdef USE_GPS_RESCUE
Expand Down
2 changes: 1 addition & 1 deletion src/main/io/gps.h
View file
Expand Up @@ -73,7 +73,7 @@ typedef struct gpsConfig_s {
gpsAutoConfig_e autoConfig;
gpsAutoBaud_e autoBaud;
uint8_t gps_ublox_use_galileo;
int16_t distanceLimit;
uint16_t distanceLimit;
} gpsConfig_t;

PG_DECLARE(gpsConfig_t, gpsConfig);
Expand Down

0 comments on commit 2e31714

Please sign in to comment.