Refactor sat count checks and GPS trust code
[betaflight.git] / src / main / io / beeper.c
bloba68f1cf006499a3ff4471d62ab3e8a115a04a025
1 /*
2 * This file is part of Cleanflight and Betaflight.
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
8 * any later version.
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
21 #include <stdbool.h>
22 #include <stdint.h>
24 #include "platform.h"
26 #include "common/utils.h"
28 #include "config/feature.h"
30 #include "drivers/dshot_command.h"
31 #include "drivers/io.h"
32 #include "drivers/pwm_output.h"
33 #include "drivers/sound_beeper.h"
34 #include "drivers/system.h"
35 #include "drivers/time.h"
37 #include "flight/mixer.h"
39 #include "config/config.h"
40 #include "fc/core.h"
41 #include "fc/runtime_config.h"
43 #include "io/statusindicator.h"
44 #include "io/vtx_control.h"
46 #ifdef USE_GPS
47 #include "io/gps.h"
48 #endif
50 #ifdef USE_OSD
51 #include "osd/osd.h"
52 #endif
54 #include "pg/beeper.h"
56 #include "scheduler/scheduler.h"
58 #include "sensors/battery.h"
59 #include "sensors/sensors.h"
61 #include "beeper.h"
63 #ifdef BEEPER_INVERTED
64 #define IS_OPEN_DRAIN false
65 #define IS_INVERTED true
66 #else
67 #define IS_OPEN_DRAIN true
68 #define IS_INVERTED false
69 #endif
71 #ifdef USE_BEEPER
72 #ifndef BEEPER_PWM_HZ
73 #define BEEPER_PWM_HZ 0
74 #endif
75 #else
76 #define BEEPER_PIN NONE
77 #define BEEPER_PWM_HZ 0
78 #endif
80 #define MAX_MULTI_BEEPS 64 //size limit for 'beep_multiBeeps[]'
82 #define BEEPER_COMMAND_REPEAT 0xFE
83 #define BEEPER_COMMAND_STOP 0xFF
85 #ifdef USE_DSHOT
86 static timeUs_t lastDshotBeaconCommandTimeUs;
87 #endif
89 #ifdef USE_BEEPER
90 /* Beeper Sound Sequences: (Square wave generation)
91 * Sequence must end with 0xFF or 0xFE. 0xFE repeats the sequence from
92 * start when 0xFF stops the sound when it's completed.
94 * "Sound" Sequences are made so that 1st, 3rd, 5th.. are the delays how
95 * long the beeper is on and 2nd, 4th, 6th.. are the delays how long beeper
96 * is off. Delays are in milliseconds/10 (i.e., 5 => 50ms).
98 // short fast beep
99 static const uint8_t beep_shortBeep[] = {
100 10, 10, BEEPER_COMMAND_STOP
102 // arming beep
103 static const uint8_t beep_armingBeep[] = {
104 30, 5, 5, 5, BEEPER_COMMAND_STOP
106 // Arming when GPS is fixed
107 static const uint8_t beep_armingGpsFix[] = {
108 5, 5, 15, 5, 5, 5, 15, 30, BEEPER_COMMAND_STOP
110 // Arming when GPS is not fixed
111 static const uint8_t beep_armingGpsNoFix[] = {
112 30, 5, 30, 5, 30, 5, BEEPER_COMMAND_STOP
114 // armed beep (first pause, then short beep)
115 static const uint8_t beep_armedBeep[] = {
116 0, 245, 10, 5, BEEPER_COMMAND_STOP
118 // disarming beeps
119 static const uint8_t beep_disarmBeep[] = {
120 15, 5, 15, 5, BEEPER_COMMAND_STOP
122 // beeps while stick held in disarm position (after pause)
123 static const uint8_t beep_disarmRepeatBeep[] = {
124 0, 100, 10, BEEPER_COMMAND_STOP
126 // Long beep and pause after that
127 static const uint8_t beep_lowBatteryBeep[] = {
128 25, 50, BEEPER_COMMAND_STOP
130 // critical battery beep
131 static const uint8_t beep_critBatteryBeep[] = {
132 50, 2, BEEPER_COMMAND_STOP
135 // transmitter-signal-lost tone
136 static const uint8_t beep_txLostBeep[] = {
137 50, 50, BEEPER_COMMAND_STOP
139 // SOS morse code:
140 static const uint8_t beep_sos[] = {
141 10, 10, 10, 10, 10, 40, 40, 10, 40, 10, 40, 40, 10, 10, 10, 10, 10, 70, BEEPER_COMMAND_STOP
143 // Ready beeps. When gps has fix and copter is ready to fly.
144 static const uint8_t beep_readyBeep[] = {
145 4, 5, 4, 5, 8, 5, 15, 5, 8, 5, 4, 5, 4, 5, BEEPER_COMMAND_STOP
147 // 2 fast short beeps
148 static const uint8_t beep_2shortBeeps[] = {
149 5, 5, 5, 5, BEEPER_COMMAND_STOP
151 // 2 longer beeps
152 static const uint8_t beep_2longerBeeps[] = {
153 20, 15, 35, 5, BEEPER_COMMAND_STOP
155 // 3 beeps
156 static const uint8_t beep_gyroCalibrated[] = {
157 20, 10, 20, 10, 20, 10, BEEPER_COMMAND_STOP
160 // Cam connection opened
161 static const uint8_t beep_camOpenBeep[] = {
162 5, 15, 10, 15, 20, BEEPER_COMMAND_STOP
165 // Cam connection close
166 static const uint8_t beep_camCloseBeep[] = {
167 10, 8, 5, BEEPER_COMMAND_STOP
170 // RC Smoothing filter not initialized - 3 short + 1 long
171 static const uint8_t beep_rcSmoothingInitFail[] = {
172 10, 10, 10, 10, 10, 10, 50, 25, BEEPER_COMMAND_STOP
175 // array used for variable # of beeps (reporting GPS sat count, etc)
176 static uint8_t beep_multiBeeps[MAX_MULTI_BEEPS + 1];
178 #define BEEPER_CONFIRMATION_BEEP_DURATION 2
179 #define BEEPER_CONFIRMATION_BEEP_GAP_DURATION 20
181 #define BEEPER_WARNING_LONG_BEEP_MULTIPLIER 5
183 #define BEEPER_WARNING_BEEP_1_DURATION 20
184 #define BEEPER_WARNING_BEEP_2_DURATION 5
185 #define BEEPER_WARNING_BEEP_GAP_DURATION 10
187 static bool beeperIsOn = false;
189 // Place in current sequence
190 static uint16_t beeperPos = 0;
191 // Time when beeper routine must act next time
192 static uint32_t beeperNextToggleTime = 0;
193 // Time of last arming beep in microseconds (for blackbox)
194 static uint32_t armingBeepTimeMicros = 0;
196 static void beeperProcessCommand(timeUs_t currentTimeUs);
198 typedef struct beeperTableEntry_s {
199 uint8_t mode;
200 uint8_t priority; // 0 = Highest
201 const uint8_t *sequence;
202 const char *name;
203 } beeperTableEntry_t;
205 #define BEEPER_ENTRY(a,b,c,d) a,b,c,d
207 // IMPORTANT: these are in priority order, 0 = Highest
208 static const beeperTableEntry_t beeperTable[] = {
209 { BEEPER_ENTRY(BEEPER_GYRO_CALIBRATED, 0, beep_gyroCalibrated, "GYRO_CALIBRATED") },
210 { BEEPER_ENTRY(BEEPER_RX_LOST, 1, beep_txLostBeep, "RX_LOST") },
211 { BEEPER_ENTRY(BEEPER_RX_LOST_LANDING, 2, beep_sos, "RX_LOST_LANDING") },
212 { BEEPER_ENTRY(BEEPER_DISARMING, 3, beep_disarmBeep, "DISARMING") },
213 { BEEPER_ENTRY(BEEPER_ARMING, 4, beep_armingBeep, "ARMING") },
214 { BEEPER_ENTRY(BEEPER_ARMING_GPS_FIX, 5, beep_armingGpsFix, "ARMING_GPS_FIX") },
215 { BEEPER_ENTRY(BEEPER_ARMING_GPS_NO_FIX, 6, beep_armingGpsNoFix, "ARMING_GPS_NO_FIX") },
216 { BEEPER_ENTRY(BEEPER_BAT_CRIT_LOW, 7, beep_critBatteryBeep, "BAT_CRIT_LOW") },
217 { BEEPER_ENTRY(BEEPER_BAT_LOW, 8, beep_lowBatteryBeep, "BAT_LOW") },
218 { BEEPER_ENTRY(BEEPER_GPS_STATUS, 9, beep_multiBeeps, "GPS_STATUS") },
219 { BEEPER_ENTRY(BEEPER_RX_SET, 10, beep_shortBeep, "RX_SET") },
220 { BEEPER_ENTRY(BEEPER_ACC_CALIBRATION, 11, beep_2shortBeeps, "ACC_CALIBRATION") },
221 { BEEPER_ENTRY(BEEPER_ACC_CALIBRATION_FAIL, 12, beep_2longerBeeps, "ACC_CALIBRATION_FAIL") },
222 { BEEPER_ENTRY(BEEPER_READY_BEEP, 13, beep_readyBeep, "READY_BEEP") },
223 { 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.
224 { BEEPER_ENTRY(BEEPER_DISARM_REPEAT, 15, beep_disarmRepeatBeep, "DISARM_REPEAT") },
225 { BEEPER_ENTRY(BEEPER_ARMED, 16, beep_armedBeep, "ARMED") },
226 { BEEPER_ENTRY(BEEPER_SYSTEM_INIT, 17, NULL, "SYSTEM_INIT") },
227 { BEEPER_ENTRY(BEEPER_USB, 18, NULL, "ON_USB") },
228 { BEEPER_ENTRY(BEEPER_BLACKBOX_ERASE, 19, beep_2shortBeeps, "BLACKBOX_ERASE") },
229 { BEEPER_ENTRY(BEEPER_CRASH_FLIP_MODE, 20, beep_2longerBeeps, "CRASH_FLIP") },
230 { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN, 21, beep_camOpenBeep, "CAM_CONNECTION_OPEN") },
231 { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE, 22, beep_camCloseBeep, "CAM_CONNECTION_CLOSE") },
232 { BEEPER_ENTRY(BEEPER_RC_SMOOTHING_INIT_FAIL,23, beep_rcSmoothingInitFail, "RC_SMOOTHING_INIT_FAIL") },
233 { BEEPER_ENTRY(BEEPER_ALL, 24, NULL, "ALL") },
236 static const beeperTableEntry_t *currentBeeperEntry = NULL;
238 #define BEEPER_TABLE_ENTRY_COUNT (sizeof(beeperTable) / sizeof(beeperTableEntry_t))
241 * Called to activate/deactivate beeper, using the given "BEEPER_..." value.
242 * This function returns immediately (does not block).
244 void beeper(beeperMode_e mode)
246 if (
247 mode == BEEPER_SILENCE || (
248 (beeperConfig()->beeper_off_flags & BEEPER_GET_FLAG(BEEPER_USB))
249 && getBatteryState() == BATTERY_NOT_PRESENT
250 ) || IS_RC_MODE_ACTIVE(BOXBEEPERMUTE)
252 beeperSilence();
253 return;
256 const beeperTableEntry_t *selectedCandidate = NULL;
257 for (uint32_t i = 0; i < BEEPER_TABLE_ENTRY_COUNT; i++) {
258 const beeperTableEntry_t *candidate = &beeperTable[i];
259 if (candidate->mode != mode) {
260 continue;
263 if (!currentBeeperEntry) {
264 selectedCandidate = candidate;
265 break;
268 if (candidate->priority < currentBeeperEntry->priority) {
269 selectedCandidate = candidate;
272 break;
275 if (!selectedCandidate) {
276 return;
279 currentBeeperEntry = selectedCandidate;
281 beeperPos = 0;
282 beeperNextToggleTime = 0;
285 void beeperSilence(void)
287 BEEP_OFF;
288 beeperIsOn = false;
290 warningLedDisable();
291 warningLedRefresh();
293 beeperNextToggleTime = 0;
294 beeperPos = 0;
296 currentBeeperEntry = NULL;
300 * Emits the given number of 20ms beeps (with 200ms spacing).
301 * This function returns immediately (does not block).
303 void beeperConfirmationBeeps(uint8_t beepCount)
305 uint32_t i = 0;
306 uint32_t cLimit = beepCount * 2;
307 if (cLimit > MAX_MULTI_BEEPS) {
308 cLimit = MAX_MULTI_BEEPS;
310 do {
311 beep_multiBeeps[i++] = BEEPER_CONFIRMATION_BEEP_DURATION;
312 beep_multiBeeps[i++] = BEEPER_CONFIRMATION_BEEP_GAP_DURATION;
313 } while (i < cLimit);
314 beep_multiBeeps[i] = BEEPER_COMMAND_STOP;
316 beeper(BEEPER_MULTI_BEEPS);
319 void beeperWarningBeeps(uint8_t beepCount)
321 uint8_t longBeepCount = beepCount / BEEPER_WARNING_LONG_BEEP_MULTIPLIER;
322 uint8_t shortBeepCount = beepCount % BEEPER_WARNING_LONG_BEEP_MULTIPLIER;
324 unsigned i = 0;
326 unsigned count = 0;
327 while (i < MAX_MULTI_BEEPS - 1 && count < WARNING_FLASH_COUNT) {
328 beep_multiBeeps[i++] = WARNING_FLASH_DURATION_MS / 10;
329 if (++count < WARNING_FLASH_COUNT) {
330 beep_multiBeeps[i++] = WARNING_FLASH_DURATION_MS / 10;
331 } else {
332 beep_multiBeeps[i++] = WARNING_PAUSE_DURATION_MS / 10;
336 while (i < MAX_MULTI_BEEPS - 1 && longBeepCount > 0) {
337 beep_multiBeeps[i++] = WARNING_CODE_DURATION_LONG_MS / 10;
338 if (--longBeepCount > 0) {
339 beep_multiBeeps[i++] = WARNING_CODE_DURATION_LONG_MS / 10;
340 } else {
341 beep_multiBeeps[i++] = WARNING_PAUSE_DURATION_MS / 10;
345 while (i < MAX_MULTI_BEEPS - 1 && shortBeepCount > 0) {
346 beep_multiBeeps[i++] = WARNING_CODE_DURATION_SHORT_MS / 10;
347 if (--shortBeepCount > 0) {
348 beep_multiBeeps[i++] = WARNING_CODE_DURATION_LONG_MS / 10;
352 beep_multiBeeps[i] = BEEPER_COMMAND_STOP;
354 beeper(BEEPER_MULTI_BEEPS);
357 #ifdef USE_GPS
358 static void beeperGpsStatus(void)
360 if (!(beeperConfig()->beeper_off_flags & BEEPER_GET_FLAG(BEEPER_GPS_STATUS))) {
361 // if GPS 3D fix and at least the minimum number available, then beep out number of satellites
362 if (STATE(GPS_FIX) && gpsSol.numSat > gpsConfig()->gpsMinimumSats) {
363 uint8_t i = 0;
364 do {
365 beep_multiBeeps[i++] = 5;
366 beep_multiBeeps[i++] = 10;
367 } while (i < MAX_MULTI_BEEPS && gpsSol.numSat > i / 2);
369 beep_multiBeeps[i - 1] = 50; // extend last pause
370 beep_multiBeeps[i] = BEEPER_COMMAND_STOP;
372 beeper(BEEPER_MULTI_BEEPS); //initiate sequence
376 #endif
379 * Beeper handler function to be called periodically in loop. Updates beeper
380 * state via time schedule.
382 void beeperUpdate(timeUs_t currentTimeUs)
384 // If beeper option from AUX switch has been selected
385 if (IS_RC_MODE_ACTIVE(BOXBEEPERON)) {
386 beeper(BEEPER_RX_SET);
387 #ifdef USE_GPS
388 } else if (featureIsEnabled(FEATURE_GPS) && IS_RC_MODE_ACTIVE(BOXBEEPGPSCOUNT)) {
389 beeperGpsStatus();
390 #endif
393 // Beeper routine doesn't need to update if there aren't any sounds ongoing
394 if (currentBeeperEntry == NULL) {
395 return;
398 if (beeperNextToggleTime > currentTimeUs) {
399 schedulerIgnoreTaskExecTime();
400 return;
403 if (!beeperIsOn) {
404 #ifdef USE_DSHOT
405 if (!areMotorsRunning()
406 && ((currentBeeperEntry->mode == BEEPER_RX_SET && !(beeperConfig()->dshotBeaconOffFlags & BEEPER_GET_FLAG(BEEPER_RX_SET)))
407 || (currentBeeperEntry->mode == BEEPER_RX_LOST && !(beeperConfig()->dshotBeaconOffFlags & BEEPER_GET_FLAG(BEEPER_RX_LOST))))) {
409 if ((currentTimeUs - getLastDisarmTimeUs() > DSHOT_BEACON_GUARD_DELAY_US) && !isTryingToArm()) {
410 lastDshotBeaconCommandTimeUs = currentTimeUs;
411 dshotCommandWrite(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone, DSHOT_CMD_TYPE_INLINE);
414 #endif
416 if (currentBeeperEntry->sequence[beeperPos] != 0) {
417 if (!(beeperConfig()->beeper_off_flags & BEEPER_GET_FLAG(currentBeeperEntry->mode))) {
418 BEEP_ON;
419 beeperIsOn = true;
422 warningLedEnable();
423 warningLedRefresh();
424 // if this was arming beep then mark time (for blackbox)
425 if (
426 beeperPos == 0
427 && (currentBeeperEntry->mode == BEEPER_ARMING || currentBeeperEntry->mode == BEEPER_ARMING_GPS_FIX
428 || currentBeeperEntry->mode == BEEPER_ARMING_GPS_NO_FIX)) {
429 armingBeepTimeMicros = micros();
432 } else {
433 if (currentBeeperEntry->sequence[beeperPos] != 0) {
434 BEEP_OFF;
435 beeperIsOn = false;
437 warningLedDisable();
438 warningLedRefresh();
442 #if defined(USE_OSD)
443 static bool beeperWasOn = false;
444 if (beeperIsOn && !beeperWasOn) {
445 osdSetVisualBeeperState(true);
447 beeperWasOn = beeperIsOn;
448 #endif
450 beeperProcessCommand(currentTimeUs);
454 * Calculates array position when next to change beeper state is due.
456 static void beeperProcessCommand(timeUs_t currentTimeUs)
458 if (currentBeeperEntry->sequence[beeperPos] == BEEPER_COMMAND_REPEAT) {
459 beeperPos = 0;
460 } else if (currentBeeperEntry->sequence[beeperPos] == BEEPER_COMMAND_STOP) {
461 beeperSilence();
462 } else {
463 // Otherwise advance the sequence and calculate next toggle time
464 beeperNextToggleTime = currentTimeUs + 1000 * 10 * currentBeeperEntry->sequence[beeperPos];
465 beeperPos++;
470 * Returns the time that the last arming beep occurred (in system-uptime
471 * microseconds). This is fetched and logged by blackbox.
473 uint32_t getArmingBeepTimeMicros(void)
475 return armingBeepTimeMicros;
479 * Returns the 'beeperMode_e' value for the given beeper-table index,
480 * or BEEPER_SILENCE if none.
482 beeperMode_e beeperModeForTableIndex(int idx)
484 return (idx >= 0 && idx < (int)BEEPER_TABLE_ENTRY_COUNT) ? beeperTable[idx].mode : BEEPER_SILENCE;
488 * Returns the binary mask for the 'beeperMode_e' value corresponding to a given
489 * beeper-table index, or 0 if the beeperMode is BEEPER_SILENCE.
491 uint32_t beeperModeMaskForTableIndex(int idx)
493 beeperMode_e beeperMode = beeperModeForTableIndex(idx);
494 if (beeperMode == BEEPER_SILENCE)
495 return 0;
496 return BEEPER_GET_FLAG(beeperMode);
500 * Returns the name for the given beeper-table index, or NULL if none.
502 const char *beeperNameForTableIndex(int idx)
504 return (idx >= 0 && idx < (int)BEEPER_TABLE_ENTRY_COUNT) ? beeperTable[idx].name : NULL;
508 * Returns the number of entries in the beeper-sounds table.
510 int beeperTableEntryCount(void)
512 return (int)BEEPER_TABLE_ENTRY_COUNT;
516 * Returns true if the beeper is on, false otherwise
518 bool isBeeperOn(void)
520 return beeperIsOn;
523 #else
525 // Stub out beeper functions if #BEEPER not defined
526 void beeper(beeperMode_e mode) {UNUSED(mode);}
527 void beeperSilence(void) {}
528 void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);}
529 void beeperWarningBeeps(uint8_t beepCount) {UNUSED(beepCount);}
530 void beeperUpdate(timeUs_t currentTimeUs) {UNUSED(currentTimeUs);}
531 uint32_t getArmingBeepTimeMicros(void) {return 0;}
532 beeperMode_e beeperModeForTableIndex(int idx) {UNUSED(idx); return BEEPER_SILENCE;}
533 uint32_t beeperModeMaskForTableIndex(int idx) {UNUSED(idx); return 0;}
534 const char *beeperNameForTableIndex(int idx) {UNUSED(idx); return NULL;}
535 int beeperTableEntryCount(void) {return 0;}
536 bool isBeeperOn(void) {return false;}
538 #endif
540 #ifdef USE_DSHOT
541 timeUs_t getLastDshotBeaconCommandTimeUs(void)
543 return lastDshotBeaconCommandTimeUs;
545 #endif