From ec71965dd83c8133fe9e1d9e17df48b9a5896e3f Mon Sep 17 00:00:00 2001 From: Tony Cabello Date: Tue, 15 Jan 2019 06:47:23 +0100 Subject: [PATCH] GPS Rescue disabled warning --- src/main/fc/core.c | 8 +++-- src/main/flight/gps_rescue.c | 5 ++++ src/main/flight/gps_rescue.h | 1 + src/main/interface/settings.c | 1 + src/main/io/beeper.c | 56 +++++++++++++++++++---------------- src/main/io/beeper.h | 4 ++- src/main/io/osd.c | 12 ++++++++ src/main/io/osd.h | 3 ++ 8 files changed, 62 insertions(+), 28 deletions(-) diff --git a/src/main/fc/core.c b/src/main/fc/core.c index a6740b503..999f03287 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -460,8 +460,12 @@ void tryArm(void) GPS_reset_home_position(); //beep to indicate arming - if (featureIsEnabled(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) { - beeper(BEEPER_ARMING_GPS_FIX); + if (featureIsEnabled(FEATURE_GPS)) { + if (STATE(GPS_FIX) && gpsSol.numSat >= 5) { + beeper(BEEPER_ARMING_GPS_FIX); + } else { + beeper(BEEPER_ARMING_GPS_NO_FIX); + } } else { beeper(BEEPER_ARMING); } diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 890b398a6..b0a37ef1a 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -618,6 +618,11 @@ bool gpsRescueIsAvailable(void) return rescueState.isAvailable; } +bool gpsRescueIsDisabled(void) +{ + return (!STATE(GPS_FIX_HOME)); +} + bool gpsRescueDisableMag(void) { return ((!gpsRescueConfig()->useMag || magForceDisable) && (rescueState.phase >= RESCUE_INITIALIZE) && (rescueState.phase <= RESCUE_LANDING)); diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index be0d4db13..bdd7c9dfd 100644 --- a/src/main/flight/gps_rescue.h +++ b/src/main/flight/gps_rescue.h @@ -48,4 +48,5 @@ float gpsRescueGetYawRate(void); float gpsRescueGetThrottle(void); bool gpsRescueIsConfigured(void); bool gpsRescueIsAvailable(void); +bool gpsRescueIsDisabled(void); bool gpsRescueDisableMag(void); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index faff32989..847a7e7e2 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -1043,6 +1043,7 @@ const clivalue_t valueTable[] = { { "osd_warn_crash_flip", VAR_UINT16 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_CRASH_FLIP, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)}, { "osd_warn_esc_fail", VAR_UINT16 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_ESC_FAIL, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)}, { "osd_warn_no_gps_rescue", VAR_UINT16 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_GPS_RESCUE_UNAVAILABLE, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)}, + { "osd_warn_gps_rescue_disabled", VAR_UINT16 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_GPS_RESCUE_DISABLED, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)}, #ifdef USE_ADC_INTERNAL { "osd_warn_core_temp", VAR_UINT16 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_CORE_TEMPERATURE, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)}, #endif diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 3007c093a..1e30e66d6 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -100,6 +100,14 @@ static const uint8_t beep_shortBeep[] = { static const uint8_t beep_armingBeep[] = { 30, 5, 5, 5, BEEPER_COMMAND_STOP }; +// Arming when GPS is fixed +static const uint8_t beep_armingGpsFix[] = { + 5, 5, 15, 5, 5, 5, 15, 30, BEEPER_COMMAND_STOP +}; +// Arming when GPS is not fixed +static const uint8_t beep_armingGpsNoFix[] = { + 30, 5, 30, 5, 30, 5, BEEPER_COMMAND_STOP +}; // armed beep (first pause, then short beep) static const uint8_t beep_armedBeep[] = { 0, 245, 10, 5, BEEPER_COMMAND_STOP @@ -129,10 +137,6 @@ static const uint8_t beep_txLostBeep[] = { static const uint8_t beep_sos[] = { 10, 10, 10, 10, 10, 40, 40, 10, 40, 10, 40, 40, 10, 10, 10, 10, 10, 70, BEEPER_COMMAND_STOP }; -// Arming when GPS is fixed -static const uint8_t beep_armedGpsFix[] = { - 5, 5, 15, 5, 5, 5, 15, 30, BEEPER_COMMAND_STOP -}; // Ready beeps. When gps has fix and copter is ready to fly. static const uint8_t beep_readyBeep[] = { 4, 5, 4, 5, 8, 5, 15, 5, 8, 5, 4, 5, 4, 5, BEEPER_COMMAND_STOP @@ -203,31 +207,33 @@ typedef struct beeperTableEntry_s { #define BEEPER_ENTRY(a,b,c,d) a,b,c #endif +// IMPORTANT: these are in priority order, 0 = Highest static const beeperTableEntry_t beeperTable[] = { { BEEPER_ENTRY(BEEPER_GYRO_CALIBRATED, 0, beep_gyroCalibrated, "GYRO_CALIBRATED") }, { BEEPER_ENTRY(BEEPER_RX_LOST, 1, beep_txLostBeep, "RX_LOST") }, { BEEPER_ENTRY(BEEPER_RX_LOST_LANDING, 2, beep_sos, "RX_LOST_LANDING") }, { BEEPER_ENTRY(BEEPER_DISARMING, 3, beep_disarmBeep, "DISARMING") }, { BEEPER_ENTRY(BEEPER_ARMING, 4, beep_armingBeep, "ARMING") }, - { BEEPER_ENTRY(BEEPER_ARMING_GPS_FIX, 5, beep_armedGpsFix, "ARMING_GPS_FIX") }, - { BEEPER_ENTRY(BEEPER_BAT_CRIT_LOW, 6, beep_critBatteryBeep, "BAT_CRIT_LOW") }, - { BEEPER_ENTRY(BEEPER_BAT_LOW, 7, beep_lowBatteryBeep, "BAT_LOW") }, - { BEEPER_ENTRY(BEEPER_GPS_STATUS, 8, beep_multiBeeps, "GPS_STATUS") }, - { BEEPER_ENTRY(BEEPER_RX_SET, 9, beep_shortBeep, "RX_SET") }, - { BEEPER_ENTRY(BEEPER_ACC_CALIBRATION, 10, beep_2shortBeeps, "ACC_CALIBRATION") }, - { BEEPER_ENTRY(BEEPER_ACC_CALIBRATION_FAIL, 11, beep_2longerBeeps, "ACC_CALIBRATION_FAIL") }, - { BEEPER_ENTRY(BEEPER_READY_BEEP, 12, beep_readyBeep, "READY_BEEP") }, - { BEEPER_ENTRY(BEEPER_MULTI_BEEPS, 13, beep_multiBeeps, "MULTI_BEEPS") }, // FIXME having this listed makes no sense since the beep array will not be initialised. - { BEEPER_ENTRY(BEEPER_DISARM_REPEAT, 14, beep_disarmRepeatBeep, "DISARM_REPEAT") }, - { BEEPER_ENTRY(BEEPER_ARMED, 15, beep_armedBeep, "ARMED") }, - { BEEPER_ENTRY(BEEPER_SYSTEM_INIT, 16, NULL, "SYSTEM_INIT") }, - { BEEPER_ENTRY(BEEPER_USB, 17, NULL, "ON_USB") }, - { BEEPER_ENTRY(BEEPER_BLACKBOX_ERASE, 18, beep_2shortBeeps, "BLACKBOX_ERASE") }, - { BEEPER_ENTRY(BEEPER_CRASH_FLIP_MODE, 19, beep_2longerBeeps, "CRASH_FLIP") }, - { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN, 20, beep_camOpenBeep, "CAM_CONNECTION_OPEN") }, - { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE, 21, beep_camCloseBeep, "CAM_CONNECTION_CLOSE") }, - { BEEPER_ENTRY(BEEPER_RC_SMOOTHING_INIT_FAIL,22, beep_rcSmoothingInitFail, "RC_SMOOTHING_INIT_FAIL") }, - { BEEPER_ENTRY(BEEPER_ALL, 23, NULL, "ALL") }, + { BEEPER_ENTRY(BEEPER_ARMING_GPS_FIX, 5, beep_armingGpsFix, "ARMING_GPS_FIX") }, + { BEEPER_ENTRY(BEEPER_ARMING_GPS_NO_FIX, 6, beep_armingGpsNoFix, "ARMING_GPS_NO_FIX") }, + { BEEPER_ENTRY(BEEPER_BAT_CRIT_LOW, 7, beep_critBatteryBeep, "BAT_CRIT_LOW") }, + { BEEPER_ENTRY(BEEPER_BAT_LOW, 8, beep_lowBatteryBeep, "BAT_LOW") }, + { BEEPER_ENTRY(BEEPER_GPS_STATUS, 9, beep_multiBeeps, "GPS_STATUS") }, + { BEEPER_ENTRY(BEEPER_RX_SET, 10, beep_shortBeep, "RX_SET") }, + { BEEPER_ENTRY(BEEPER_ACC_CALIBRATION, 11, beep_2shortBeeps, "ACC_CALIBRATION") }, + { BEEPER_ENTRY(BEEPER_ACC_CALIBRATION_FAIL, 12, beep_2longerBeeps, "ACC_CALIBRATION_FAIL") }, + { BEEPER_ENTRY(BEEPER_READY_BEEP, 13, beep_readyBeep, "READY_BEEP") }, + { BEEPER_ENTRY(BEEPER_MULTI_BEEPS, 14, beep_multiBeeps, "MULTI_BEEPS") }, // FIXME having this listed makes no sense since the beep array will not be initialised. + { BEEPER_ENTRY(BEEPER_DISARM_REPEAT, 15, beep_disarmRepeatBeep, "DISARM_REPEAT") }, + { BEEPER_ENTRY(BEEPER_ARMED, 16, beep_armedBeep, "ARMED") }, + { BEEPER_ENTRY(BEEPER_SYSTEM_INIT, 17, NULL, "SYSTEM_INIT") }, + { BEEPER_ENTRY(BEEPER_USB, 18, NULL, "ON_USB") }, + { BEEPER_ENTRY(BEEPER_BLACKBOX_ERASE, 19, beep_2shortBeeps, "BLACKBOX_ERASE") }, + { BEEPER_ENTRY(BEEPER_CRASH_FLIP_MODE, 20, beep_2longerBeeps, "CRASH_FLIP") }, + { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN, 21, beep_camOpenBeep, "CAM_CONNECTION_OPEN") }, + { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE, 22, beep_camCloseBeep, "CAM_CONNECTION_CLOSE") }, + { BEEPER_ENTRY(BEEPER_RC_SMOOTHING_INIT_FAIL,23, beep_rcSmoothingInitFail, "RC_SMOOTHING_INIT_FAIL") }, + { BEEPER_ENTRY(BEEPER_ALL, 24, NULL, "ALL") }, }; static const beeperTableEntry_t *currentBeeperEntry = NULL; @@ -420,8 +426,8 @@ void beeperUpdate(timeUs_t currentTimeUs) // if this was arming beep then mark time (for blackbox) if ( beeperPos == 0 - && (currentBeeperEntry->mode == BEEPER_ARMING || currentBeeperEntry->mode == BEEPER_ARMING_GPS_FIX) - ) { + && (currentBeeperEntry->mode == BEEPER_ARMING || currentBeeperEntry->mode == BEEPER_ARMING_GPS_FIX + || currentBeeperEntry->mode == BEEPER_ARMING_GPS_NO_FIX)) { armingBeepTimeMicros = micros(); } } diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index c6bca7055..2b177964e 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -30,7 +30,7 @@ #endif typedef enum { - // IMPORTANT: these are in priority order, 0 = Highest + // IMPORTANT: the order of the elements should be preserved for backwards compatibility with the configurator. BEEPER_SILENCE = 0, // Silence, see beeperSilence() BEEPER_GYRO_CALIBRATED, @@ -56,6 +56,7 @@ typedef enum { BEEPER_CAM_CONNECTION_OPEN, // When the 5 key simulation stated BEEPER_CAM_CONNECTION_CLOSE, // When the 5 key simulation stop BEEPER_RC_SMOOTHING_INIT_FAIL, // Warning beep pattern when armed and rc smoothing has not initialized filters + BEEPER_ARMING_GPS_NO_FIX, // Beep a special tone when arming the board and GPS has no fix BEEPER_ALL, // Turn ON or OFF all beeper conditions // BEEPER_ALL must remain at the bottom of this enum } beeperMode_e; @@ -85,6 +86,7 @@ typedef enum { | BEEPER_GET_FLAG(BEEPER_CAM_CONNECTION_OPEN) \ | BEEPER_GET_FLAG(BEEPER_CAM_CONNECTION_CLOSE) \ | BEEPER_GET_FLAG(BEEPER_RC_SMOOTHING_INIT_FAIL) \ + | BEEPER_GET_FLAG(BEEPER_ARMING_GPS_NO_FIX) \ ) #define DSHOT_BEACON_ALLOWED_MODES ( \ diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 2621656a1..75a8bea27 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1015,11 +1015,23 @@ static bool osdDrawSingleElement(uint8_t item) if (osdWarnGetState(OSD_WARNING_GPS_RESCUE_UNAVAILABLE) && ARMING_FLAG(ARMED) && gpsRescueIsConfigured() && + !gpsRescueIsDisabled() && !gpsRescueIsAvailable()) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "NO GPS RESC"); SET_BLINK(OSD_WARNINGS); break; } + + if (osdWarnGetState(OSD_WARNING_GPS_RESCUE_DISABLED) && + ARMING_FLAG(ARMED) && + gpsRescueIsConfigured() && + gpsRescueIsDisabled() && + !cmpTimeUs(stats.armed_time, OSD_GPS_RESCUE_DISABLED_WARNING_DURATION_US)) { + osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "RESC OFF!!!"); + SET_BLINK(OSD_WARNINGS); + break; + } + #endif // Show warning if in HEADFREE flight mode diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 02309935a..5dc0fec6c 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -201,6 +201,7 @@ typedef enum { OSD_WARNING_FAIL_SAFE, OSD_WARNING_LAUNCH_CONTROL, OSD_WARNING_GPS_RESCUE_UNAVAILABLE, + OSD_WARNING_GPS_RESCUE_DISABLED, OSD_WARNING_COUNT // MUST BE LAST } osdWarningsFlags_e; @@ -211,6 +212,8 @@ STATIC_ASSERT(OSD_WARNING_COUNT <= 16, osdwarnings_overflow); #define ESC_TEMP_ALARM_OFF INT8_MIN #define ESC_CURRENT_ALARM_OFF -1 +#define OSD_GPS_RESCUE_DISABLED_WARNING_DURATION_US 3000000 // 3 seconds + typedef struct osdConfig_s { uint16_t item_pos[OSD_ITEM_COUNT];