diff --git a/docs/Buzzer.md b/docs/Buzzer.md index 376235d57..63d1300dc 100644 --- a/docs/Buzzer.md +++ b/docs/Buzzer.md @@ -1,11 +1,18 @@ # Buzzer -Cleanflight supports a buzzer which is used for the following purposes, and more: +Cleanflight supports a buzzer which is used for the following purposes: - * Low Battery alarm (when battery monitoring enabled) - * Notification of calibration complete status. - * AUX operated beeping - useful for locating your aircraft after a crash. - * Failsafe status. + * Low and critical battery alarms (when battery monitoring enabled) + * Arm/disarm tones (and warning beeps while armed) + * Notification of calibration complete status + * TX-AUX operated beeping - useful for locating your aircraft after a crash + * Failsafe status + * Flight mode change + * Rate profile change (via TX-AUX switch) + +If the arm/disarm is via the control stick, holding the stick in the disarm position will sound a repeating tone. This can be used as a lost-model locator. + +There is a special arming tone used if a GPS fix has been attained, and there's a "ready" tone sounded after a GPS fix has been attained (only happens once). The tone sounded via the TX-AUX-switch will count out the number of satellites (if GPS fix). Buzzer is enabled by default on platforms that have buzzer connections. diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 078178e99..2b072cd5c 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -38,7 +38,6 @@ #include "drivers/pwm_rx.h" #include "drivers/accgyro.h" #include "drivers/light_led.h" -#include "drivers/sound_beeper.h" #include "sensors/sensors.h" #include "sensors/boardalignment.h" @@ -981,22 +980,15 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) } } -// Beep the buzzer and write the current time to the log as a synchronization point -static void blackboxPlaySyncBeep() +// Write the time of the last arming beep to the log as a synchronization point +static void blackboxLogArmingBeep() { flightLogEvent_syncBeep_t eventData; - eventData.time = micros(); - - /* - * The regular beep routines aren't going to work for us, because they queue up the beep to be executed later. - * Our beep is timing sensitive, so start beeping now without setting the beeperIsOn flag. - */ - BEEP_ON; - - // Have the regular beeper code turn off the beep for us eventually, since that's not timing-sensitive - queueConfirmationBeep(1); + // Get time of last arming beep (in system-uptime microseconds) + eventData.time = getArmingBeepTimeMicros(); + // Write the time to the log blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *) &eventData); } @@ -1064,7 +1056,7 @@ void handleBlackbox(void) case BLACKBOX_STATE_PRERUN: blackboxSetState(BLACKBOX_STATE_RUNNING); - blackboxPlaySyncBeep(); + blackboxLogArmingBeep(); break; case BLACKBOX_STATE_RUNNING: // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0 diff --git a/src/main/config/runtime_config.c b/src/main/config/runtime_config.c index 3ca243d22..3c9d8aa2c 100644 --- a/src/main/config/runtime_config.c +++ b/src/main/config/runtime_config.c @@ -19,6 +19,7 @@ #include #include "config/runtime_config.h" +#include "io/beeper.h" uint8_t armingFlags = 0; uint8_t stateFlags = 0; @@ -26,6 +27,34 @@ uint16_t flightModeFlags = 0; static uint32_t enabledSensors = 0; +/** + * Enables the given flight mode. A beep is sounded if the flight mode + * has changed. Returns the new 'flightModeFlags' value. + */ +uint16_t enableFlightMode(flightModeFlags_e mask) +{ + uint16_t oldVal = flightModeFlags; + + flightModeFlags |= (mask); + if (flightModeFlags != oldVal) + queueConfirmationBeep(1); + return flightModeFlags; +} + +/** + * Disables the given flight mode. A beep is sounded if the flight mode + * has changed. Returns the new 'flightModeFlags' value. + */ +uint16_t disableFlightMode(flightModeFlags_e mask) +{ + uint16_t oldVal = flightModeFlags; + + flightModeFlags &= ~(mask); + if (flightModeFlags != oldVal) + queueConfirmationBeep(1); + return flightModeFlags; +} + bool sensors(uint32_t mask) { return enabledSensors & mask; diff --git a/src/main/config/runtime_config.h b/src/main/config/runtime_config.h index fc92386c2..090a57ca7 100644 --- a/src/main/config/runtime_config.h +++ b/src/main/config/runtime_config.h @@ -45,8 +45,8 @@ typedef enum { extern uint16_t flightModeFlags; -#define DISABLE_FLIGHT_MODE(mask) (flightModeFlags &= ~(mask)) -#define ENABLE_FLIGHT_MODE(mask) (flightModeFlags |= (mask)) +#define DISABLE_FLIGHT_MODE(mask) disableFlightMode(mask) +#define ENABLE_FLIGHT_MODE(mask) enableFlightMode(mask) #define FLIGHT_MODE(mask) (flightModeFlags & (mask)) typedef enum { @@ -63,6 +63,8 @@ typedef enum { extern uint8_t stateFlags; +uint16_t enableFlightMode(flightModeFlags_e mask); +uint16_t disableFlightMode(flightModeFlags_e mask); bool sensors(uint32_t mask); void sensorsSet(uint32_t mask); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 6ffd27dd6..125e74e49 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -18,9 +18,12 @@ #include #include +#include "debug.h" + #include "common/axis.h" #include "rx/rx.h" +#include "io/beeper.h" #include "io/escservo.h" #include "io/rc_controls.h" #include "config/runtime_config.h" @@ -74,11 +77,11 @@ failsafePhase_e failsafePhase() return failsafeState.phase; } -#define MAX_COUNTER_VALUE_WHEN_RX_IS_RECEIVED_AFTER_RX_CYCLE 1 +#define FAILSAFE_COUNTER_THRESHOLD 20 bool failsafeIsReceivingRxData(void) { - return failsafeState.counter <= MAX_COUNTER_VALUE_WHEN_RX_IS_RECEIVED_AFTER_RX_CYCLE; + return failsafeState.counter <= FAILSAFE_COUNTER_THRESHOLD; } bool failsafeIsMonitoring(void) @@ -129,8 +132,8 @@ static void failsafeApplyControlInput(void) void failsafeOnValidDataReceived(void) { - if (failsafeState.counter > 20) - failsafeState.counter -= 20; + if (failsafeState.counter > FAILSAFE_COUNTER_THRESHOLD) + failsafeState.counter -= FAILSAFE_COUNTER_THRESHOLD; else failsafeState.counter = 0; } @@ -139,10 +142,13 @@ void failsafeUpdateState(void) { bool receivingRxData = failsafeIsReceivingRxData(); bool armed = ARMING_FLAG(ARMED); + beeperMode_e beeperMode = BEEPER_STOPPED; if (receivingRxData) { failsafeState.phase = FAILSAFE_IDLE; failsafeState.active = false; + } else { + beeperMode = BEEPER_RX_LOST; } @@ -161,6 +167,7 @@ void failsafeUpdateState(void) break; case FAILSAFE_RX_LOSS_DETECTED: + if (failsafeShouldForceLanding(armed)) { // Stabilize, and set Throttle to specified level failsafeActivate(); @@ -172,6 +179,7 @@ void failsafeUpdateState(void) case FAILSAFE_LANDING: if (armed) { failsafeApplyControlInput(); + beeperMode = BEEPER_RX_LOST_LANDING; } if (failsafeShouldHaveCausedLandingByNow() || !armed) { @@ -188,6 +196,7 @@ void failsafeUpdateState(void) if (!armed) { break; } + // This will prevent the automatic rearm if failsafe shuts it down and prevents // to restart accidently by just reconnect to the tx - you will have to switch off first to rearm ENABLE_ARMING_FLAG(PREVENT_ARMING); @@ -201,6 +210,9 @@ void failsafeUpdateState(void) } } while (reprocessState); + if (beeperMode != BEEPER_STOPPED) { + beeper(beeperMode); + } } /** diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index ff2974997..64fdc0591 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -36,6 +36,7 @@ #include "sensors/sensors.h" +#include "io/beeper.h" #include "io/serial.h" #include "io/gps.h" #include "io/rc_controls.h" @@ -659,6 +660,7 @@ void updateGpsStateForHomeAndHoldMode(void) void updateGpsWaypointsAndMode(void) { bool resetNavNow = false; + static bool gpsReadyBeepDone = false; if (STATE(GPS_FIX) && GPS_numSat >= 5) { @@ -711,6 +713,10 @@ void updateGpsWaypointsAndMode(void) } } } + if (!gpsReadyBeepDone) { //if 'ready' beep not yet done + beeper(BEEPER_READY_BEEP); //do ready beep now + gpsReadyBeepDone = true; //only beep once + } } else { if (FLIGHT_MODE(GPS_HOLD_MODE | GPS_HOME_MODE)) { diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index a5c06fa7d..dc92034b6 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -17,6 +17,7 @@ #include "stdbool.h" #include "stdint.h" +#include "stdlib.h" #include "platform.h" @@ -30,170 +31,301 @@ #include "rx/rx.h" #include "io/rc_controls.h" +#ifdef GPS +#include "io/gps.h" +#endif + #include "config/runtime_config.h" #include "config/config.h" #include "io/beeper.h" -#define LONG_PAUSE_DURATION_MILLIS 200 -#define DOUBLE_PAUSE_DURATION_MILLIS (LONG_PAUSE_DURATION_MILLIS * 2) -#define SHORT_PAUSE_DURATION_MILLIS (LONG_PAUSE_DURATION_MILLIS / 4) -#define MEDIUM_PAUSE_DURATION_MILLIS (LONG_PAUSE_DURATION_MILLIS / 2) -#define SHORT_CONFIRMATION_BEEP_DURATION_MILLIS (SHORT_PAUSE_DURATION_MILLIS / 2) +#define MAX_MULTI_BEEPS 20 //size limit for 'beep_multiBeeps[]' -static uint8_t beeperIsOn = 0, beepDone = 0; -static uint32_t beeperLastToggleTime; -static void beep(uint16_t pulseMillis); -static void beep_code(char first, char second, char third, char pause); +/* Beeper Sound Sequences: (Square wave generation) + * Sequence must end with 0xFF or 0xFE. 0xFE repeats the sequence from + * start when 0xFF stops the sound when it's completed. + * If repeat is used then BEEPER_STOP call must be used for stopping the sound. + * + * "Sound" Sequences are made so that 1st, 3rd, 5th.. are the delays how + * long the beeper is on and 2nd, 4th, 6th.. are the delays how long beeper + * is off. Delays are in milliseconds/10 (i.e., 5 => 50ms). + */ +// short fast beep +static const uint8_t beep_shortBeep[] = { + 10, 10, 0xFF +}; +// arming beep +static const uint8_t beep_armingBeep[] = { + 30, 5, 5, 5, 0xFF +}; +// armed beep (first pause, then short beep) +static const uint8_t beep_armedBeep[] = { + 0, 245, 10, 5, 0xFF +}; +// disarming beeps +static const uint8_t beep_disarmBeep[] = { + 15, 5, 15, 5, 0xFF +}; +// beeps while stick held in disarm position (after pause) +static const uint8_t beep_disarmRepeatBeep[] = { + 0, 35, 40, 5, 0xFF +}; +// Long beep and pause after that +static const uint8_t beep_lowBatteryBeep[] = { + 25, 50, 0xFF +}; +// critical battery beep +static const uint8_t beep_critBatteryBeep[] = { + 50, 2, 0xFF +}; +// single confirmation beep +static const uint8_t beep_confirmBeep[] = { + 2, 20, 0xFF +}; +// transmitter-signal-lost tone +static const uint8_t beep_txLostBeep[] = { + 50, 50, 0xFF +}; +// SOS morse code: +static const uint8_t beep_sos[] = { + 10, 10, 10, 10, 10, 40, 40, 10, 40, 10, 40, 40, 10, 10, 10, 10, 10, 70, 0xFF +}; +// Arming when GPS is fixed +static const uint8_t beep_armedGpsFix[] = { + 5, 5, 15, 5, 5, 5, 15, 30, 0xFF +}; +// 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, 0xFF +}; +// 2 fast short beeps +static const uint8_t beep_2shortBeeps[] = { + 5, 5, 5, 5, 0xFF +}; +// 3 fast short beeps +static const uint8_t beep_3shortBeeps[] = { + 5, 5, 5, 5, 5, 5, 0xFF +}; +// array used for variable # of beeps (reporting GPS sat count, etc) +static uint8_t beep_multiBeeps[MAX_MULTI_BEEPS+2]; -static uint8_t toggleBeep = 0; +// Current Beeper mode +static uint8_t beeperMode = BEEPER_STOPPED; +// Beeper off = 0 Beeper on = 1 +static uint8_t beeperIsOn = 0; +// Pointer to current sequence +static const uint8_t *beeperPtr = NULL; +// Place in current sequence +static uint16_t beeperPos = 0; +// Time when beeper routine must act next time +static uint32_t beeperNextToggleTime = 0; +// Time of last arming beep in microseconds (for blackbox) +static uint32_t armingBeepTimeMicros = 0; -typedef enum { - FAILSAFE_WARNING_NONE = 0, - FAILSAFE_WARNING_LANDING, - FAILSAFE_WARNING_FIND_ME -} failsafeBeeperWarnings_e; +static void beeperCalculations(void); -void beepcodeInit(void) +/* + * Called to activate/deactive beeper, using the given "BEEPER_..." value. + * This function returns immediately (does not block). + */ +void beeper(beeperMode_e mode) { -} - -void beepcodeUpdateState(batteryState_e batteryState) -{ - static uint8_t beeperOnBox; #ifdef GPS - static uint8_t warn_noGPSfix = 0; -#endif - static failsafeBeeperWarnings_e warn_failsafe = FAILSAFE_WARNING_NONE; - - //===================== BeeperOn via rcOptions ===================== - if (IS_RC_MODE_ACTIVE(BOXBEEPERON)) { // unconditional beeper on via AUXn switch - beeperOnBox = 1; - } else { - beeperOnBox = 0; - } - //===================== Beeps for failsafe ===================== - if (feature(FEATURE_FAILSAFE)) { - switch (failsafePhase()) { - case FAILSAFE_LANDING: - warn_failsafe = FAILSAFE_WARNING_LANDING; - break; - case FAILSAFE_LANDED: - warn_failsafe = FAILSAFE_WARNING_FIND_ME; - break; - default: - warn_failsafe = FAILSAFE_WARNING_NONE; - } - - if (rxIsReceivingSignal()) { - warn_failsafe = FAILSAFE_WARNING_NONE; - } - } - -#ifdef GPS - //===================== GPS fix notification handling ===================== - if (sensors(SENSOR_GPS)) { - if ((IS_RC_MODE_ACTIVE(BOXGPSHOME) || IS_RC_MODE_ACTIVE(BOXGPSHOLD)) && !STATE(GPS_FIX)) { // if no fix and gps funtion is activated: do warning beeps - warn_noGPSfix = 1; - } else { - warn_noGPSfix = 0; - } - } + uint8_t i; #endif - //===================== Priority driven Handling ===================== - // beepcode(length1,length2,length3,pause) - // D: Double, L: Long, M: Middle, S: Short, N: None - if (warn_failsafe == 2) - beep_code('L','N','N','D'); // failsafe "find me" signal - else if (warn_failsafe == 1) - beep_code('S','M','L','M'); // failsafe landing active -#ifdef GPS - else if (warn_noGPSfix == 1) - beep_code('S','S','N','M'); -#endif - else if (beeperOnBox == 1) - beep_code('S','S','S','M'); // beeperon - else if (batteryState == BATTERY_CRITICAL) - beep_code('S','S','M','D'); - else if (batteryState == BATTERY_WARNING) - beep_code('S','M','M','D'); - else if (FLIGHT_MODE(AUTOTUNE_MODE)) - beep_code('S','M','S','M'); - else if (toggleBeep > 0) - beep(SHORT_CONFIRMATION_BEEP_DURATION_MILLIS); // fast confirmation beep - else { - beeperIsOn = 0; - BEEP_OFF; - } -} + // Just return if same or higher priority sound is active. + if (beeperMode <= mode) + return; -// duration is specified in multiples of SHORT_CONFIRMATION_BEEP_DURATION_MILLIS -void queueConfirmationBeep(uint8_t duration) { - toggleBeep = duration; -} - -void beep_code(char first, char second, char third, char pause) -{ - char patternChar[4]; - uint16_t Duration; - static uint8_t icnt = 0; - - patternChar[0] = first; - patternChar[1] = second; - patternChar[2] = third; - patternChar[3] = pause; - switch (patternChar[icnt]) { - case 'N': - Duration = 0; - break; - case 'S': - Duration = LONG_PAUSE_DURATION_MILLIS / 4; - break; - case 'M': - Duration = LONG_PAUSE_DURATION_MILLIS / 2; - break; - case 'D': - Duration = LONG_PAUSE_DURATION_MILLIS * 2; - break; - case 'L': - default: - Duration = LONG_PAUSE_DURATION_MILLIS; - break; - } - - if (icnt < 3 && Duration != 0) - beep(Duration); - if (icnt >= 3 && (beeperLastToggleTime < millis() - Duration)) { - icnt = 0; - toggleBeep = 0; - } - if (beepDone == 1 || Duration == 0) { - if (icnt < 3) - icnt++; - beepDone = 0; - beeperIsOn = 0; - BEEP_OFF; - } -} - -static void beep(uint16_t pulseMillis) -{ - if (beeperIsOn) { - if (millis() >= beeperLastToggleTime + pulseMillis) { - beeperIsOn = 0; + switch (mode) { + case BEEPER_STOP: + beeperMode = BEEPER_STOPPED; + beeperNextToggleTime = millis(); BEEP_OFF; - beeperLastToggleTime = millis(); - if (toggleBeep >0) - toggleBeep--; - beepDone = 1; - } + beeperIsOn = 0; + beeperPtr = NULL; + break; + case BEEPER_READY_BEEP: + beeperPtr = beep_readyBeep; + beeperMode = mode; + break; + case BEEPER_ARMING: + beeperPtr = beep_armingBeep; + beeperMode = mode; + beeperNextToggleTime = 0; + break; + case BEEPER_DISARMING: + beeperPtr = beep_disarmBeep; + beeperMode = mode; + beeperNextToggleTime = 0; + break; + case BEEPER_DISARM_REPEAT: + beeperPtr = beep_disarmRepeatBeep; + beeperMode = mode; + beeperNextToggleTime = 0; + break; + case BEEPER_ACC_CALIBRATION: + beeperPtr = beep_2shortBeeps; + beeperMode = mode; + break; + case BEEPER_ACC_CALIBRATION_FAIL: + beeperPtr = beep_3shortBeeps; + beeperMode = mode; + break; + case BEEPER_RX_LOST_LANDING: + beeperPtr = beep_sos; + beeperMode = mode; + beeperNextToggleTime = 0; + break; + case BEEPER_RX_LOST: + beeperPtr = beep_txLostBeep; + beeperMode = mode; + beeperNextToggleTime = 0; + break; + case BEEPER_BAT_LOW: + beeperPtr = beep_lowBatteryBeep; + beeperMode = mode; + beeperNextToggleTime = 0; + break; + case BEEPER_BAT_CRIT_LOW: + beeperPtr = beep_critBatteryBeep; + beeperMode = mode; + beeperNextToggleTime = 0; + break; + case BEEPER_ARMED: + beeperPtr = beep_armedBeep; + beeperMode = mode; + break; + case BEEPER_ARMING_GPS_FIX: + beeperPtr = beep_armedGpsFix; + beeperMode = mode; + beeperNextToggleTime = 0; + break; + case BEEPER_CONFIRM_BEEP: + beeperPtr = beep_confirmBeep; + beeperMode = mode; + beeperNextToggleTime = 0; + break; + case BEEPER_MULTI_BEEPS: + beeperPtr = beep_multiBeeps; + beeperMode = mode; + beeperNextToggleTime = 0; + break; + case BEEPER_RX_SET: +#ifdef GPS // if GPS fix then beep out number of satellites + if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5) { + i = 0; + do { + beep_multiBeeps[i++] = 5; + beep_multiBeeps[i++] = 10; + } while (i < MAX_MULTI_BEEPS && GPS_numSat > i / 2); + beep_multiBeeps[i-1] = 50; //extend last pause + beep_multiBeeps[i] = 0xFF; + beeperPtr = beep_multiBeeps; + beeperMode = mode; + break; + } +#endif + beeperPtr = beep_shortBeep; + beeperMode = mode; + beeperNextToggleTime = 0; + break; + + default: + return; + } + beeperPos = 0; +} + +/* + * Emits the given number of 20ms beeps (with 200ms spacing). + * This function returns immediately (does not block). + */ +void queueConfirmationBeep(uint8_t beepCount) +{ + int i; + int cLimit; + + if(beepCount <= 1) //if single beep then + beeper(BEEPER_CONFIRM_BEEP); //use dedicated array + else { + i = 0; + cLimit = beepCount * 2; + if(cLimit > MAX_MULTI_BEEPS) + cLimit = MAX_MULTI_BEEPS; //stay within array size + do { + beep_multiBeeps[i++] = 2; //20ms beep + beep_multiBeeps[i++] = 20; //200ms pause + } while (i < cLimit); + beep_multiBeeps[i] = 0xFF; //sequence end + beeper(BEEPER_MULTI_BEEPS); //initiate sequence + } +} + +/* + * Beeper handler function to be called periodically in loop. Updates beeper + * state via time schedule. + */ +void beeperUpdate(void) +{ + // If beeper option from AUX switch has been selected + if (IS_RC_MODE_ACTIVE(BOXBEEPERON)) { + if (beeperMode > BEEPER_RX_SET) + beeper(BEEPER_RX_SET); + } + + // Beeper routine doesn't need to update if there aren't any sounds ongoing + if (beeperMode == BEEPER_STOPPED || beeperPtr == NULL) { return; } - if (millis() >= (beeperLastToggleTime + LONG_PAUSE_DURATION_MILLIS)) { // Beeper is off and long pause time is up -> turn it on + if (!beeperIsOn && beeperNextToggleTime <= millis()) { beeperIsOn = 1; - BEEP_ON; - beeperLastToggleTime = millis(); // save the time the buzer turned on + if (beeperPtr[beeperPos] != 0) { + BEEP_ON; + //if this was arming beep then mark time (for blackbox) + if (beeperPos == 0 && (beeperMode == BEEPER_ARMING || + beeperMode == BEEPER_ARMING_GPS_FIX)) { + armingBeepTimeMicros = micros(); + } + } + beeperCalculations(); + } else if (beeperIsOn && beeperNextToggleTime <= millis()) { + beeperIsOn = 0; + if (beeperPtr[beeperPos] != 0) { + BEEP_OFF; + } + beeperCalculations(); } } + +/* + * Calculates array position when next to change beeper state is due. + */ +void beeperCalculations(void) +{ + if (beeperPtr[beeperPos] == 0xFE) { + // If sequence is 0xFE then repeat from start + beeperPos = 0; + } else if (beeperPtr[beeperPos] == 0xFF) { + // If sequence is 0xFF then stop + beeperMode = BEEPER_STOPPED; + BEEP_OFF; + beeperIsOn = 0; + } else { + // Otherwise advance the sequence and calculate next toggle time + beeperNextToggleTime = millis() + 10 * beeperPtr[beeperPos]; + beeperPos++; + } +} + +/* + * Returns the time that the last arming beep occurred (in system-uptime + * microseconds). This is fetched and logged by blackbox. + */ +uint32_t getArmingBeepTimeMicros(void) +{ + return armingBeepTimeMicros; +} diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index 09ee348ad..8a35ae863 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -17,5 +17,45 @@ #pragma once -void beepcodeUpdateState(batteryState_e batteryState); -void queueConfirmationBeep(uint8_t duration); +/* Beeper different modes: (lower number is higher priority) + * BEEPER_STOP - Stops beeping + * BEEPER_RX_LOST_LANDING - Beeps SOS when armed and TX is turned off or signal lost (autolanding/autodisarm) + * BEEPER_RX_LOST - Beeps when TX is turned off or signal lost (repeat until TX is okay) + * BEEPER_DISARMING - Beep when disarming the board + * BEEPER_ARMING - Beep when arming the board + * BEEPER_ARMING_GPS_FIX - Beep a tone when arming the board and GPS has fix + * BEEPER_BAT_CRIT_LOW - Faster warning beeps when battery is critically low (repeats) + * BEEPER_BAT_LOW - Warning beeps when battery is getting low (repeats) + * BEEPER_RX_SET - Beeps when aux channel is set for beep or beep sequence how many satellites has found if GPS enabled. + * BEEPER_DISARM_REPEAT - Beeps sounded while stick held in disarm position + * BEEPER_ACC_CALIBRATION - ACC inflight calibration completed confirmation + * BEEPER_ACC_CALIBRATION_FAIL - ACC inflight calibration failed + * BEEPER_READY_BEEP - Ring a tone when board is ready to flight (GPS ready). + * BEEPER_CONFIRM_BEEP - Single short confirmation beep. + * BEEPER_MULTI_BEEPS - Internal value used by 'queueConfirmationBeep()'. + * BEEPER_ARMED - Warning beeps when board is armed. (repeats until board is disarmed or throttle is increased) + */ +typedef enum { + BEEPER_STOP = 0, // Highest priority command which is used only for stopping the beeper + BEEPER_RX_LOST_LANDING, + BEEPER_RX_LOST, + BEEPER_DISARMING, + BEEPER_ARMING, + BEEPER_ARMING_GPS_FIX, + BEEPER_BAT_CRIT_LOW, + BEEPER_BAT_LOW, + BEEPER_RX_SET, + BEEPER_DISARM_REPEAT, + BEEPER_ACC_CALIBRATION, + BEEPER_ACC_CALIBRATION_FAIL, + BEEPER_READY_BEEP, + BEEPER_CONFIRM_BEEP, + BEEPER_MULTI_BEEPS, + BEEPER_ARMED, + BEEPER_STOPPED // State which is used when beeper is in idle mode +} beeperMode_e; + +void beeper(beeperMode_e mode); +void beeperUpdate(void); +void queueConfirmationBeep(uint8_t beepCount); +uint32_t getArmingBeepTimeMicros(void); diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 0d680a7e7..37e952f57 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -140,18 +140,29 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat return; } - if (ARMING_FLAG(ARMED)) { - // actions during armed - if (isUsingSticksToArm) { // Disarm on throttle down + yaw - if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) - mwDisarm(); - - // Disarm on roll (only when retarded_arm is enabled) - if (retarded_arm && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO)) + if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) { + if (ARMING_FLAG(ARMED)) //board was armed mwDisarm(); + else { //board was not armed + beeper(BEEPER_DISARM_REPEAT); //sound tone while stick held + rcDelayCommand = 0; //reset so disarm tone will repeat + } } + // Disarm on roll (only when retarded_arm is enabled) + if (retarded_arm && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO)) { + if (ARMING_FLAG(ARMED)) //board was armed + mwDisarm(); + else { //board was not armed + beeper(BEEPER_DISARM_REPEAT); //sound tone while stick held + rcDelayCommand = 0; //reset so disarm tone will repeat + } + } + } + + if (ARMING_FLAG(ARMED)) { + // actions during armed return; } diff --git a/src/main/main.c b/src/main/main.c index 25d3aa518..fb7d4d227 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -106,7 +106,6 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init); void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMixers); void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration); void rxInit(rxConfig_t *rxConfig); -void beepcodeInit(void); void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig); void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile); bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int8_t magHardwareToUse, int16_t magDeclinationFromConfig); @@ -346,8 +345,6 @@ void init(void) failsafeInit(&masterConfig.rxConfig); - beepcodeInit(); - rxInit(&masterConfig.rxConfig); #ifdef GPS diff --git a/src/main/mw.c b/src/main/mw.c index 2f4bf2a35..a2ebcc1f1 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -242,6 +242,11 @@ void annexCode(void) if (feature(FEATURE_VBAT)) { updateBatteryVoltage(); batteryState = calculateBatteryState(); + //handle beepers for battery levels + if (batteryState == BATTERY_CRITICAL) + beeper(BEEPER_BAT_CRIT_LOW); //critically low battery + else if (batteryState == BATTERY_WARNING) + beeper(BEEPER_BAT_LOW); //low battery } if (feature(FEATURE_CURRENT_METER)) { @@ -251,7 +256,7 @@ void annexCode(void) } } - beepcodeUpdateState(batteryState); + beeperUpdate(); //call periodic beeper handler if (ARMING_FLAG(ARMED)) { LED0_ON; @@ -317,6 +322,8 @@ void mwDisarm(void) finishBlackbox(); } #endif + + beeper(BEEPER_DISARMING); // emit disarm tone } } @@ -355,6 +362,16 @@ void mwArm(void) #endif disarmAt = millis() + masterConfig.auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero + //beep to indicate arming +#ifdef GPS + if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5) + beeper(BEEPER_ARMING_GPS_FIX); + else + beeper(BEEPER_ARMING); +#else + beeper(BEEPER_ARMING); +#endif + return; } } @@ -380,9 +397,9 @@ void handleInflightCalibrationStickPosition(void) } else { AccInflightCalibrationArmed = !AccInflightCalibrationArmed; if (AccInflightCalibrationArmed) { - queueConfirmationBeep(4); + beeper(BEEPER_ACC_CALIBRATION); } else { - queueConfirmationBeep(6); + beeper(BEEPER_ACC_CALIBRATION_FAIL); } } } @@ -497,6 +514,8 @@ void executePeriodicTasks(void) void processRx(void) { + static bool armedBeeperOn = false; + calculateRxChannelsAndUpdateFailsafe(currentTime); // in 3D mode, we need to be able to disarm by switch at any time @@ -522,18 +541,48 @@ void processRx(void) pidResetErrorAngle(); pidResetErrorGyro(); } - // When armed and motors aren't spinning, disarm board after delay so users without buzzer won't lose fingers. + + // When armed and motors aren't spinning, do beeps and then disarm + // board after delay so users without buzzer won't lose fingers. // mixTable constrains motor commands, so checking throttleStatus is enough if (ARMING_FLAG(ARMED) - && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) - && masterConfig.auto_disarm_delay != 0 - && isUsingSticksForArming()) - { - if (throttleStatus == THROTTLE_LOW) { - if ((int32_t)(disarmAt - millis()) < 0) // delay is over - mwDisarm(); + && feature(FEATURE_MOTOR_STOP) + && !STATE(FIXED_WING) + ) { + if (isUsingSticksForArming()) { + if (throttleStatus == THROTTLE_LOW) { + if (masterConfig.auto_disarm_delay != 0 + && (int32_t)(disarmAt - millis()) < 0 + ) { + // auto-disarm configured and delay is over + mwDisarm(); + armedBeeperOn = false; + } else { + // still armed; do warning beeps while armed + beeper(BEEPER_ARMED); + armedBeeperOn = true; + } + } else { + // throttle is not low + if (masterConfig.auto_disarm_delay != 0) { + // extend disarm time + disarmAt = millis() + masterConfig.auto_disarm_delay * 1000; + } + + if (armedBeeperOn) { + beeper(BEEPER_STOP); + armedBeeperOn = false; + } + } } else { - disarmAt = millis() + masterConfig.auto_disarm_delay * 1000; // extend delay + // arming is via AUX switch; beep while throttle low + if (throttleStatus == THROTTLE_LOW) { + beeper(BEEPER_ARMED); + armedBeeperOn = true; + } else if (armedBeeperOn) { + beeper(BEEPER_STOP); + armedBeeperOn = false; + } } } diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 480930af4..af0005c55 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -225,6 +225,9 @@ void updateRx(uint32_t currentTime) if (rxSignalReceived) { if (((int32_t)(currentTime - needRxSignalBefore) >= 0)) { rxSignalReceived = false; +#ifdef DEBUG_RX_SIGNAL_LOSS + debug[0]++; +#endif } } diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index c265b7595..152cfdd60 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -139,7 +139,7 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri if (InflightcalibratingA == 1) { AccInflightCalibrationActive = false; AccInflightCalibrationMeasurementDone = true; - queueConfirmationBeep(5); // beeper to indicating the end of calibration + beeper(BEEPER_ACC_CALIBRATION); // indicate end of calibration // recover saved values to maintain current flight behaviour until new values are transferred accelerationTrims->raw[X] = accZero_saved[X]; accelerationTrims->raw[Y] = accZero_saved[Y]; diff --git a/src/test/unit/altitude_hold_unittest.cc b/src/test/unit/altitude_hold_unittest.cc index 4b8102309..02bf80b9f 100644 --- a/src/test/unit/altitude_hold_unittest.cc +++ b/src/test/unit/altitude_hold_unittest.cc @@ -157,6 +157,16 @@ uint8_t armingFlags; int32_t sonarAlt; +uint16_t enableFlightMode(flightModeFlags_e mask) +{ + return flightModeFlags |= (mask); +} + +uint16_t disableFlightMode(flightModeFlags_e mask) +{ + return flightModeFlags &= ~(mask); +} + void gyroUpdate(void) {}; bool sensors(uint32_t mask) { diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index 4a6d7e224..66981e605 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/src/test/unit/flight_failsafe_unittest.cc @@ -30,6 +30,8 @@ extern "C" { #include "config/runtime_config.h" + #include "io/beeper.h" + #include "rx/rx.h" #include "flight/failsafe.h" @@ -174,22 +176,27 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndStartsLanding) // given ENABLE_ARMING_FLAG(ARMED); - // when - failsafeOnRxCycleStarted(); - // no call to failsafeOnValidDataReceived(); - - failsafeUpdateState(); - - // then - EXPECT_EQ(false, failsafeIsActive()); - EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); - // - // currently one cycle must occur (above) so that the next cycle (below) can detect the lack of an update. + // currently 20 cycles must occur before the lack of an update triggers RX loss detection. // + // FIXME see comments about RX_SERIAL/RX_MSP above, the test should likely deal with time rather than counters. + int failsafeCounterThreshold = 20; // when - for (int i = 0; i < FAILSAFE_UPDATE_HZ - 1; i++) { + for (int i = 0; i < failsafeCounterThreshold; i++) { + + failsafeOnRxCycleStarted(); + // no call to failsafeOnValidDataReceived(); + + failsafeUpdateState(); + + // then + EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); + EXPECT_EQ(false, failsafeIsActive()); + } + + // when + for (int i = 0; i < FAILSAFE_UPDATE_HZ - failsafeCounterThreshold; i++) { failsafeOnRxCycleStarted(); // no call to failsafeOnValidDataReceived(); @@ -199,7 +206,6 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndStartsLanding) // then EXPECT_EQ(FAILSAFE_RX_LOSS_DETECTED, failsafePhase()); EXPECT_EQ(false, failsafeIsActive()); - } // @@ -303,6 +309,7 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected extern "C" { int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; uint8_t armingFlags; +int16_t debug[DEBUG16_VALUE_COUNT]; void delay(uint32_t) {} @@ -314,4 +321,8 @@ void mwDisarm(void) { callCounts[COUNTER_MW_DISARM]++; } +void beeper(beeperMode_e mode) { + UNUSED(mode); +} + } diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 5d5f42078..c575a74f5 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -95,6 +95,15 @@ int16_t accADC[XYZ_AXIS_COUNT]; int16_t gyroADC[XYZ_AXIS_COUNT]; +uint16_t enableFlightMode(flightModeFlags_e mask) +{ + return flightModeFlags |= (mask); +} + +uint16_t disableFlightMode(flightModeFlags_e mask) +{ + return flightModeFlags &= ~(mask); +} void gyroUpdate(void) {}; bool sensors(uint32_t mask) diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 209110ec0..8005f6c68 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -191,6 +191,10 @@ void queueConfirmationBeep(uint8_t) { callCounts[COUNTER_QUEUE_CONFIRMATION_BEEP]++; } +void beeper(uint8_t mode) { + UNUSED(mode); +} + void changeControlRateProfile(uint8_t) { callCounts[COUNTER_CHANGE_CONTROL_RATE_PROFILE]++; }