diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 4306d101f5..e24216cda0 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -121,6 +121,10 @@ static const uint8_t beep_runtimeCalibrationDone[] = { static const uint8_t beep_launchModeBeep[] = { 5, 5, 5, 100, BEEPER_COMMAND_STOP }; +// Two short beeps, then one shorter beep. Beeps to show the throttle needs to be raised. Will repeat every second while the throttle is low. +static const uint8_t beep_launchModeLowThrottleBeep[] = { + 5, 5, 5, 5, 3, 100, BEEPER_COMMAND_STOP +}; // short beeps static const uint8_t beep_hardwareFailure[] = { 10, 10, BEEPER_COMMAND_STOP @@ -164,31 +168,32 @@ typedef struct beeperTableEntry_s { #define BEEPER_ENTRY(a,b,c,d) a,b,c,d /*static*/ const beeperTableEntry_t beeperTable[] = { - { BEEPER_ENTRY(BEEPER_RUNTIME_CALIBRATION_DONE, 0, beep_runtimeCalibrationDone, "RUNTIME_CALIBRATION") }, - { BEEPER_ENTRY(BEEPER_HARDWARE_FAILURE , 1, beep_hardwareFailure, "HW_FAILURE") }, - { BEEPER_ENTRY(BEEPER_RX_LOST, 2, beep_txLostBeep, "RX_LOST") }, - { BEEPER_ENTRY(BEEPER_RX_LOST_LANDING, 3, beep_sos, "RX_LOST_LANDING") }, - { BEEPER_ENTRY(BEEPER_DISARMING, 4, beep_disarmBeep, "DISARMING") }, - { BEEPER_ENTRY(BEEPER_ARMING, 5, beep_armingBeep, "ARMING") }, - { BEEPER_ENTRY(BEEPER_ARMING_GPS_FIX, 6, beep_armedGpsFix, "ARMING_GPS_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_ACTION_SUCCESS, 11, beep_2shortBeeps, "ACTION_SUCCESS") }, - { BEEPER_ENTRY(BEEPER_ACTION_FAIL, 12, beep_2longerBeeps, "ACTION_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_LAUNCH_MODE_ENABLED, 19, beep_launchModeBeep, "LAUNCH_MODE") }, - { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN, 20, beep_camOpenBeep, "CAM_CONNECTION_OPEN") }, - { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE, 21, beep_camCloseBeep, "CAM_CONNECTION_CLOSED") }, + { BEEPER_ENTRY(BEEPER_RUNTIME_CALIBRATION_DONE, 0, beep_runtimeCalibrationDone, "RUNTIME_CALIBRATION") }, + { BEEPER_ENTRY(BEEPER_HARDWARE_FAILURE , 1, beep_hardwareFailure, "HW_FAILURE") }, + { BEEPER_ENTRY(BEEPER_RX_LOST, 2, beep_txLostBeep, "RX_LOST") }, + { BEEPER_ENTRY(BEEPER_RX_LOST_LANDING, 3, beep_sos, "RX_LOST_LANDING") }, + { BEEPER_ENTRY(BEEPER_DISARMING, 4, beep_disarmBeep, "DISARMING") }, + { BEEPER_ENTRY(BEEPER_ARMING, 5, beep_armingBeep, "ARMING") }, + { BEEPER_ENTRY(BEEPER_ARMING_GPS_FIX, 6, beep_armedGpsFix, "ARMING_GPS_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_ACTION_SUCCESS, 11, beep_2shortBeeps, "ACTION_SUCCESS") }, + { BEEPER_ENTRY(BEEPER_ACTION_FAIL, 12, beep_2longerBeeps, "ACTION_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_LAUNCH_MODE_ENABLED, 19, beep_launchModeBeep, "LAUNCH_MODE") }, + { BEEPER_ENTRY(BEEPER_LAUNCH_MODE_LOW_THROTTLE, 20, beep_launchModeLowThrottleBeep, "LAUNCH_MODE_LOW_THROTTLE") }, + { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN, 21, beep_camOpenBeep, "CAM_CONNECTION_OPEN") }, + { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE, 22, beep_camCloseBeep, "CAM_CONNECTION_CLOSED") }, - { BEEPER_ENTRY(BEEPER_ALL, 22, NULL, "ALL") }, - { BEEPER_ENTRY(BEEPER_PREFERENCE, 23, NULL, "PREFERED") }, + { BEEPER_ENTRY(BEEPER_ALL, 23, NULL, "ALL") }, + { BEEPER_ENTRY(BEEPER_PREFERENCE, 24, NULL, "PREFERED") }, }; static const beeperTableEntry_t *currentBeeperEntry = NULL; diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index 5dcff77f0a..0fcc8249dd 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -21,33 +21,34 @@ typedef enum { // IMPORTANT: these are in priority order, 0 = Highest - BEEPER_SILENCE = 0, // Silence, see beeperSilence() + BEEPER_SILENCE = 0, // Silence, see beeperSilence() BEEPER_RUNTIME_CALIBRATION_DONE, - BEEPER_HARDWARE_FAILURE, // HW failure - BEEPER_RX_LOST, // Beeps when TX is turned off or signal lost (repeat until TX is okay) - BEEPER_RX_LOST_LANDING, // Beeps SOS when armed and TX is turned off or signal lost (autolanding/autodisarm) - BEEPER_DISARMING, // Beep when disarming the board - BEEPER_ARMING, // Beep when arming the board - BEEPER_ARMING_GPS_FIX, // Beep a special tone when arming the board and GPS has fix - BEEPER_BAT_CRIT_LOW, // Longer warning beeps when battery is critically low (repeats) - BEEPER_BAT_LOW, // Warning beeps when battery is getting low (repeats) - BEEPER_GPS_STATUS, // FIXME **** Disable beeper when connected to USB **** - BEEPER_RX_SET, // Beeps when aux channel is set for beep or beep sequence how many satellites has found if GPS enabled - BEEPER_ACTION_SUCCESS, // Action success (various actions) - BEEPER_ACTION_FAIL, // Action fail (varions actions) - BEEPER_READY_BEEP, // Ring a tone when GPS is locked and ready - BEEPER_MULTI_BEEPS, // Internal value used by 'beeperConfirmationBeeps()'. - BEEPER_DISARM_REPEAT, // Beeps sounded while stick held in disarm position - BEEPER_ARMED, // Warning beeps when board is armed (repeats until board is disarmed or throttle is increased) - BEEPER_SYSTEM_INIT, // Initialisation beeps when board is powered on - BEEPER_USB, // Some boards have beeper powered USB connected - BEEPER_LAUNCH_MODE_ENABLED, // Fixed-wing launch mode enabled - BEEPER_CAM_CONNECTION_OPEN, // When the 5 key simulation stated - BEEPER_CAM_CONNECTION_CLOSE, // When the 5 key simulation stop + BEEPER_HARDWARE_FAILURE, // HW failure + BEEPER_RX_LOST, // Beeps when TX is turned off or signal lost (repeat until TX is okay) + BEEPER_RX_LOST_LANDING, // Beeps SOS when armed and TX is turned off or signal lost (autolanding/autodisarm) + BEEPER_DISARMING, // Beep when disarming the board + BEEPER_ARMING, // Beep when arming the board + BEEPER_ARMING_GPS_FIX, // Beep a special tone when arming the board and GPS has fix + BEEPER_BAT_CRIT_LOW, // Longer warning beeps when battery is critically low (repeats) + BEEPER_BAT_LOW, // Warning beeps when battery is getting low (repeats) + BEEPER_GPS_STATUS, // FIXME **** Disable beeper when connected to USB **** + BEEPER_RX_SET, // Beeps when aux channel is set for beep or beep sequence how many satellites has found if GPS enabled + BEEPER_ACTION_SUCCESS, // Action success (various actions) + BEEPER_ACTION_FAIL, // Action fail (varions actions) + BEEPER_READY_BEEP, // Ring a tone when GPS is locked and ready + BEEPER_MULTI_BEEPS, // Internal value used by 'beeperConfirmationBeeps()'. + BEEPER_DISARM_REPEAT, // Beeps sounded while stick held in disarm position + BEEPER_ARMED, // Warning beeps when board is armed (repeats until board is disarmed or throttle is increased) + BEEPER_SYSTEM_INIT, // Initialisation beeps when board is powered on + BEEPER_USB, // Some boards have beeper powered USB connected + BEEPER_LAUNCH_MODE_ENABLED, // Fixed-wing launch mode enabled + BEEPER_LAUNCH_MODE_LOW_THROTTLE, // Fixed-wing launch mode enabled, but throttle is low + BEEPER_CAM_CONNECTION_OPEN, // When the 5 key simulation stated + BEEPER_CAM_CONNECTION_CLOSE, // When the 5 key simulation stop - BEEPER_ALL, // Turn ON or OFF all beeper conditions - BEEPER_PREFERENCE, // Save preferred beeper configuration + BEEPER_ALL, // Turn ON or OFF all beeper conditions + BEEPER_PREFERENCE, // Save preferred beeper configuration // BEEPER_ALL and BEEPER_PREFERENCE must remain at the bottom of this enum } beeperMode_e; diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 843d27f650..309bef0cf0 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -226,7 +226,11 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs) // Control beeper if (!launchState.launchFinished) { - beeper(BEEPER_LAUNCH_MODE_ENABLED); + if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) { + beeper(BEEPER_LAUNCH_MODE_ENABLED); + } else { + beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE); + } } // Lock out controls