src/main/io/beeper.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   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.
   9  *
  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.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include "stdbool.h"
  22 #include "stdint.h"
  23
  24 #include "platform.h"
  25
  26 #include "common/utils.h"
  27
  28 #include "config/feature.h"
  29
  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"
  36
  37 #include "flight/mixer.h"
  38
  39 #include "config/config.h"
  40 #include "fc/core.h"
  41 #include "fc/runtime_config.h"
  42
  43 #include "io/statusindicator.h"
  44 #include "io/vtx_control.h"
  45
  46 #ifdef USE_GPS
  47 #include "io/gps.h"
  48 #endif
  49
  50 #include "pg/beeper.h"
  51
  52 #include "scheduler/scheduler.h"
  53
  54 #include "sensors/battery.h"
  55 #include "sensors/sensors.h"
  56
  57 #include "beeper.h"
  58
  59 #ifdef BEEPER_INVERTED
  60 #define IS_OPEN_DRAIN   false
  61 #define IS_INVERTED     true
  62 #else
  63 #define IS_OPEN_DRAIN   true
  64 #define IS_INVERTED     false
  65 #endif
  66
  67 #ifdef USE_BEEPER
  68 #ifndef BEEPER_PWM_HZ
  69 #define BEEPER_PWM_HZ   0
  70 #endif
  71 #else
  72 #define BEEPER_PIN      NONE
  73 #define BEEPER_PWM_HZ   0
  74 #endif
  75
  76 #define MAX_MULTI_BEEPS 64   //size limit for 'beep_multiBeeps[]'
  77
  78 #define BEEPER_COMMAND_REPEAT 0xFE
  79 #define BEEPER_COMMAND_STOP   0xFF
  80
  81 #ifdef USE_DSHOT
  82 static timeUs_t lastDshotBeaconCommandTimeUs;
  83 #endif
  84
  85 #ifdef USE_BEEPER
  86 /* Beeper Sound Sequences: (Square wave generation)
  87  * Sequence must end with 0xFF or 0xFE. 0xFE repeats the sequence from
  88  * start when 0xFF stops the sound when it's completed.
  89  *
  90  * "Sound" Sequences are made so that 1st, 3rd, 5th.. are the delays how
  91  * long the beeper is on and 2nd, 4th, 6th.. are the delays how long beeper
  92  * is off. Delays are in milliseconds/10 (i.e., 5 => 50ms).
  93  */
  94 // short fast beep
  95 static const uint8_t beep_shortBeep[] = {
  96     10, 10, BEEPER_COMMAND_STOP
  97 };
  98 // arming beep
  99 static const uint8_t beep_armingBeep[] = {
 100     30, 5, 5, 5, BEEPER_COMMAND_STOP
 101 };
 102 // Arming when GPS is fixed
 103 static const uint8_t beep_armingGpsFix[] = {
 104     5, 5, 15, 5, 5, 5, 15, 30, BEEPER_COMMAND_STOP
 105 };
 106 // Arming when GPS is not fixed
 107 static const uint8_t beep_armingGpsNoFix[] = {
 108     30, 5, 30, 5, 30, 5, BEEPER_COMMAND_STOP
 109 };
 110 // armed beep (first pause, then short beep)
 111 static const uint8_t beep_armedBeep[] = {
 112     0, 245, 10, 5, BEEPER_COMMAND_STOP
 113 };
 114 // disarming beeps
 115 static const uint8_t beep_disarmBeep[] = {
 116     15, 5, 15, 5, BEEPER_COMMAND_STOP
 117 };
 118 // beeps while stick held in disarm position (after pause)
 119 static const uint8_t beep_disarmRepeatBeep[] = {
 120     0, 100, 10, BEEPER_COMMAND_STOP
 121 };
 122 // Long beep and pause after that
 123 static const uint8_t beep_lowBatteryBeep[] = {
 124     25, 50, BEEPER_COMMAND_STOP
 125 };
 126 // critical battery beep
 127 static const uint8_t beep_critBatteryBeep[] = {
 128     50, 2, BEEPER_COMMAND_STOP
 129 };
 130
 131 // transmitter-signal-lost tone
 132 static const uint8_t beep_txLostBeep[] = {
 133     50, 50, BEEPER_COMMAND_STOP
 134 };
 135 // SOS morse code:
 136 static const uint8_t beep_sos[] = {
 137     10, 10, 10, 10, 10, 40, 40, 10, 40, 10, 40, 40, 10, 10, 10, 10, 10, 70, BEEPER_COMMAND_STOP
 138 };
 139 // Ready beeps. When gps has fix and copter is ready to fly.
 140 static const uint8_t beep_readyBeep[] = {
 141     4, 5, 4, 5, 8, 5, 15, 5, 8, 5, 4, 5, 4, 5, BEEPER_COMMAND_STOP
 142 };
 143 // 2 fast short beeps
 144 static const uint8_t beep_2shortBeeps[] = {
 145     5, 5, 5, 5, BEEPER_COMMAND_STOP
 146 };
 147 // 2 longer beeps
 148 static const uint8_t beep_2longerBeeps[] = {
 149     20, 15, 35, 5, BEEPER_COMMAND_STOP
 150 };
 151 // 3 beeps
 152 static const uint8_t beep_gyroCalibrated[] = {
 153     20, 10, 20, 10, 20, 10, BEEPER_COMMAND_STOP
 154 };
 155
 156 // Cam connection opened
 157 static const uint8_t beep_camOpenBeep[] = {
 158     5, 15, 10, 15, 20, BEEPER_COMMAND_STOP
 159 };
 160
 161 // Cam connection close
 162 static const uint8_t beep_camCloseBeep[] = {
 163     10, 8, 5, BEEPER_COMMAND_STOP
 164 };
 165
 166 // RC Smoothing filter not initialized - 3 short + 1 long
 167 static const uint8_t beep_rcSmoothingInitFail[] = {
 168     10, 10, 10, 10, 10, 10, 50, 25, BEEPER_COMMAND_STOP
 169 };
 170
 171 // array used for variable # of beeps (reporting GPS sat count, etc)
 172 static uint8_t beep_multiBeeps[MAX_MULTI_BEEPS + 1];
 173
 174 #define BEEPER_CONFIRMATION_BEEP_DURATION 2
 175 #define BEEPER_CONFIRMATION_BEEP_GAP_DURATION 20
 176
 177 #define BEEPER_WARNING_LONG_BEEP_MULTIPLIER 5
 178
 179 #define BEEPER_WARNING_BEEP_1_DURATION 20
 180 #define BEEPER_WARNING_BEEP_2_DURATION 5
 181 #define BEEPER_WARNING_BEEP_GAP_DURATION 10
 182
 183 static bool beeperIsOn = false;
 184
 185 // Place in current sequence
 186 static uint16_t beeperPos = 0;
 187 // Time when beeper routine must act next time
 188 static uint32_t beeperNextToggleTime = 0;
 189 // Time of last arming beep in microseconds (for blackbox)
 190 static uint32_t armingBeepTimeMicros = 0;
 191
 192 static void beeperProcessCommand(timeUs_t currentTimeUs);
 193
 194 typedef struct beeperTableEntry_s {
 195     uint8_t mode;
 196     uint8_t priority; // 0 = Highest
 197     const uint8_t *sequence;
 198     const char *name;
 199 } beeperTableEntry_t;
 200
 201 #define BEEPER_ENTRY(a,b,c,d) a,b,c,d
 202
 203 // IMPORTANT: these are in priority order, 0 = Highest
 204 static const beeperTableEntry_t beeperTable[] = {
 205     { BEEPER_ENTRY(BEEPER_GYRO_CALIBRATED,       0, beep_gyroCalibrated,   "GYRO_CALIBRATED") },
 206     { BEEPER_ENTRY(BEEPER_RX_LOST,               1, beep_txLostBeep,       "RX_LOST") },
 207     { BEEPER_ENTRY(BEEPER_RX_LOST_LANDING,       2, beep_sos,              "RX_LOST_LANDING") },
 208     { BEEPER_ENTRY(BEEPER_DISARMING,             3, beep_disarmBeep,       "DISARMING") },
 209     { BEEPER_ENTRY(BEEPER_ARMING,                4, beep_armingBeep,       "ARMING")  },
 210     { BEEPER_ENTRY(BEEPER_ARMING_GPS_FIX,        5, beep_armingGpsFix,     "ARMING_GPS_FIX") },
 211     { BEEPER_ENTRY(BEEPER_ARMING_GPS_NO_FIX,     6, beep_armingGpsNoFix,   "ARMING_GPS_NO_FIX") },
 212     { BEEPER_ENTRY(BEEPER_BAT_CRIT_LOW,          7, beep_critBatteryBeep,  "BAT_CRIT_LOW") },
 213     { BEEPER_ENTRY(BEEPER_BAT_LOW,               8, beep_lowBatteryBeep,   "BAT_LOW") },
 214     { BEEPER_ENTRY(BEEPER_GPS_STATUS,            9, beep_multiBeeps,       "GPS_STATUS") },
 215     { BEEPER_ENTRY(BEEPER_RX_SET,                10, beep_shortBeep,       "RX_SET") },
 216     { BEEPER_ENTRY(BEEPER_ACC_CALIBRATION,       11, beep_2shortBeeps,     "ACC_CALIBRATION") },
 217     { BEEPER_ENTRY(BEEPER_ACC_CALIBRATION_FAIL,  12, beep_2longerBeeps,    "ACC_CALIBRATION_FAIL") },
 218     { BEEPER_ENTRY(BEEPER_READY_BEEP,            13, beep_readyBeep,       "READY_BEEP") },
 219     { 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.
 220     { BEEPER_ENTRY(BEEPER_DISARM_REPEAT,         15, beep_disarmRepeatBeep, "DISARM_REPEAT") },
 221     { BEEPER_ENTRY(BEEPER_ARMED,                 16, beep_armedBeep,       "ARMED") },
 222     { BEEPER_ENTRY(BEEPER_SYSTEM_INIT,           17, NULL,                 "SYSTEM_INIT") },
 223     { BEEPER_ENTRY(BEEPER_USB,                   18, NULL,                 "ON_USB") },
 224     { BEEPER_ENTRY(BEEPER_BLACKBOX_ERASE,        19, beep_2shortBeeps,     "BLACKBOX_ERASE") },
 225     { BEEPER_ENTRY(BEEPER_CRASH_FLIP_MODE,       20, beep_2longerBeeps,    "CRASH_FLIP") },
 226     { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN,   21, beep_camOpenBeep,     "CAM_CONNECTION_OPEN") },
 227     { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE,  22, beep_camCloseBeep,    "CAM_CONNECTION_CLOSE") },
 228     { BEEPER_ENTRY(BEEPER_RC_SMOOTHING_INIT_FAIL,23, beep_rcSmoothingInitFail, "RC_SMOOTHING_INIT_FAIL") },
 229     { BEEPER_ENTRY(BEEPER_ALL,                   24, NULL,                 "ALL") },
 230 };
 231
 232 static const beeperTableEntry_t *currentBeeperEntry = NULL;
 233
 234 #define BEEPER_TABLE_ENTRY_COUNT (sizeof(beeperTable) / sizeof(beeperTableEntry_t))
 235
 236 /*
 237  * Called to activate/deactivate beeper, using the given "BEEPER_..." value.
 238  * This function returns immediately (does not block).
 239  */
 240 void beeper(beeperMode_e mode)
 241 {
 242     if (
 243         mode == BEEPER_SILENCE || (
 244             (beeperConfig()->beeper_off_flags & BEEPER_GET_FLAG(BEEPER_USB))
 245             && getBatteryState() == BATTERY_NOT_PRESENT
 246         ) || IS_RC_MODE_ACTIVE(BOXBEEPERMUTE)
 247     ) {
 248         beeperSilence();
 249         return;
 250     }
 251
 252     const beeperTableEntry_t *selectedCandidate = NULL;
 253     for (uint32_t i = 0; i < BEEPER_TABLE_ENTRY_COUNT; i++) {
 254         const beeperTableEntry_t *candidate = &beeperTable[i];
 255         if (candidate->mode != mode) {
 256             continue;
 257         }
 258
 259         if (!currentBeeperEntry) {
 260             selectedCandidate = candidate;
 261             break;
 262         }
 263
 264         if (candidate->priority < currentBeeperEntry->priority) {
 265             selectedCandidate = candidate;
 266         }
 267
 268         break;
 269     }
 270
 271     if (!selectedCandidate) {
 272         return;
 273     }
 274
 275     currentBeeperEntry = selectedCandidate;
 276
 277     beeperPos = 0;
 278     beeperNextToggleTime = 0;
 279 }
 280
 281 void beeperSilence(void)
 282 {
 283     BEEP_OFF;
 284     beeperIsOn = false;
 285
 286     warningLedDisable();
 287     warningLedRefresh();
 288
 289     beeperNextToggleTime = 0;
 290     beeperPos = 0;
 291
 292     currentBeeperEntry = NULL;
 293 }
 294
 295 /*
 296  * Emits the given number of 20ms beeps (with 200ms spacing).
 297  * This function returns immediately (does not block).
 298  */
 299 void beeperConfirmationBeeps(uint8_t beepCount)
 300 {
 301     uint32_t i = 0;
 302     uint32_t cLimit = beepCount * 2;
 303     if (cLimit > MAX_MULTI_BEEPS) {
 304         cLimit = MAX_MULTI_BEEPS;
 305     }
 306     do {
 307         beep_multiBeeps[i++] = BEEPER_CONFIRMATION_BEEP_DURATION;
 308         beep_multiBeeps[i++] = BEEPER_CONFIRMATION_BEEP_GAP_DURATION;
 309     } while (i < cLimit);
 310     beep_multiBeeps[i] = BEEPER_COMMAND_STOP;
 311
 312     beeper(BEEPER_MULTI_BEEPS);
 313 }
 314
 315 void beeperWarningBeeps(uint8_t beepCount)
 316 {
 317     uint8_t longBeepCount = beepCount / BEEPER_WARNING_LONG_BEEP_MULTIPLIER;
 318     uint8_t shortBeepCount = beepCount % BEEPER_WARNING_LONG_BEEP_MULTIPLIER;
 319
 320     unsigned i = 0;
 321
 322     unsigned count = 0;
 323     while (i < MAX_MULTI_BEEPS - 1 && count < WARNING_FLASH_COUNT) {
 324         beep_multiBeeps[i++] = WARNING_FLASH_DURATION_MS / 10;
 325         if (++count < WARNING_FLASH_COUNT) {
 326             beep_multiBeeps[i++] = WARNING_FLASH_DURATION_MS / 10;
 327         } else {
 328             beep_multiBeeps[i++] = WARNING_PAUSE_DURATION_MS / 10;
 329         }
 330     }
 331
 332     while (i < MAX_MULTI_BEEPS - 1 && longBeepCount > 0) {
 333         beep_multiBeeps[i++] = WARNING_CODE_DURATION_LONG_MS / 10;
 334         if (--longBeepCount > 0) {
 335             beep_multiBeeps[i++] = WARNING_CODE_DURATION_LONG_MS / 10;
 336         } else {
 337             beep_multiBeeps[i++] = WARNING_PAUSE_DURATION_MS / 10;
 338         }
 339     }
 340
 341     while (i < MAX_MULTI_BEEPS - 1 && shortBeepCount > 0) {
 342         beep_multiBeeps[i++] = WARNING_CODE_DURATION_SHORT_MS / 10;
 343         if (--shortBeepCount > 0) {
 344             beep_multiBeeps[i++] = WARNING_CODE_DURATION_LONG_MS / 10;
 345         }
 346     }
 347
 348     beep_multiBeeps[i] = BEEPER_COMMAND_STOP;
 349
 350     beeper(BEEPER_MULTI_BEEPS);
 351 }
 352
 353 #ifdef USE_GPS
 354 static void beeperGpsStatus(void)
 355 {
 356     if (!(beeperConfig()->beeper_off_flags & BEEPER_GET_FLAG(BEEPER_GPS_STATUS))) {
 357         // if GPS fix then beep out number of satellites
 358         if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
 359             uint8_t i = 0;
 360             do {
 361                 beep_multiBeeps[i++] = 5;
 362                 beep_multiBeeps[i++] = 10;
 363             } while (i < MAX_MULTI_BEEPS && gpsSol.numSat > i / 2);
 364
 365             beep_multiBeeps[i - 1] = 50; // extend last pause
 366             beep_multiBeeps[i] = BEEPER_COMMAND_STOP;
 367
 368             beeper(BEEPER_MULTI_BEEPS);    //initiate sequence
 369         }
 370     }
 371 }
 372 #endif
 373
 374 /*
 375  * Beeper handler function to be called periodically in loop. Updates beeper
 376  * state via time schedule.
 377  */
 378 void beeperUpdate(timeUs_t currentTimeUs)
 379 {
 380     // If beeper option from AUX switch has been selected
 381     if (IS_RC_MODE_ACTIVE(BOXBEEPERON)) {
 382         beeper(BEEPER_RX_SET);
 383 #ifdef USE_GPS
 384     } else if (featureIsEnabled(FEATURE_GPS) && IS_RC_MODE_ACTIVE(BOXBEEPGPSCOUNT)) {
 385         beeperGpsStatus();
 386 #endif
 387     }
 388
 389     // Beeper routine doesn't need to update if there aren't any sounds ongoing
 390     if (currentBeeperEntry == NULL) {
 391         return;
 392     }
 393
 394     if (beeperNextToggleTime > currentTimeUs) {
 395         schedulerIgnoreTaskExecTime();
 396         return;
 397     }
 398
 399     if (!beeperIsOn) {
 400 #ifdef USE_DSHOT
 401         if (!areMotorsRunning()
 402             && ((currentBeeperEntry->mode == BEEPER_RX_SET && !(beeperConfig()->dshotBeaconOffFlags & BEEPER_GET_FLAG(BEEPER_RX_SET)))
 403             || (currentBeeperEntry->mode == BEEPER_RX_LOST && !(beeperConfig()->dshotBeaconOffFlags & BEEPER_GET_FLAG(BEEPER_RX_LOST))))) {
 404
 405             if ((currentTimeUs - getLastDisarmTimeUs() > DSHOT_BEACON_GUARD_DELAY_US) && !isTryingToArm()) {
 406                 lastDshotBeaconCommandTimeUs = currentTimeUs;
 407                 dshotCommandWrite(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone, DSHOT_CMD_TYPE_INLINE);
 408             }
 409         }
 410 #endif
 411
 412         if (currentBeeperEntry->sequence[beeperPos] != 0) {
 413             if (!(beeperConfig()->beeper_off_flags & BEEPER_GET_FLAG(currentBeeperEntry->mode))) {
 414                 BEEP_ON;
 415                 beeperIsOn = true;
 416             }
 417
 418             warningLedEnable();
 419             warningLedRefresh();
 420             // if this was arming beep then mark time (for blackbox)
 421             if (
 422                 beeperPos == 0
 423                 && (currentBeeperEntry->mode == BEEPER_ARMING || currentBeeperEntry->mode == BEEPER_ARMING_GPS_FIX
 424                 || currentBeeperEntry->mode == BEEPER_ARMING_GPS_NO_FIX)) {
 425                 armingBeepTimeMicros = micros();
 426             }
 427         }
 428     } else {
 429         if (currentBeeperEntry->sequence[beeperPos] != 0) {
 430             BEEP_OFF;
 431             beeperIsOn = false;
 432
 433             warningLedDisable();
 434             warningLedRefresh();
 435         }
 436     }
 437
 438     beeperProcessCommand(currentTimeUs);
 439 }
 440
 441 /*
 442  * Calculates array position when next to change beeper state is due.
 443  */
 444 static void beeperProcessCommand(timeUs_t currentTimeUs)
 445 {
 446     if (currentBeeperEntry->sequence[beeperPos] == BEEPER_COMMAND_REPEAT) {
 447         beeperPos = 0;
 448     } else if (currentBeeperEntry->sequence[beeperPos] == BEEPER_COMMAND_STOP) {
 449         beeperSilence();
 450     } else {
 451         // Otherwise advance the sequence and calculate next toggle time
 452         beeperNextToggleTime = currentTimeUs + 1000 * 10 * currentBeeperEntry->sequence[beeperPos];
 453         beeperPos++;
 454     }
 455 }
 456
 457 /*
 458  * Returns the time that the last arming beep occurred (in system-uptime
 459  * microseconds).  This is fetched and logged by blackbox.
 460  */
 461 uint32_t getArmingBeepTimeMicros(void)
 462 {
 463     return armingBeepTimeMicros;
 464 }
 465
 466 /*
 467  * Returns the 'beeperMode_e' value for the given beeper-table index,
 468  * or BEEPER_SILENCE if none.
 469  */
 470 beeperMode_e beeperModeForTableIndex(int idx)
 471 {
 472     return (idx >= 0 && idx < (int)BEEPER_TABLE_ENTRY_COUNT) ? beeperTable[idx].mode : BEEPER_SILENCE;
 473 }
 474
 475 /*
 476  * Returns the binary mask for the 'beeperMode_e' value corresponding to a given
 477  * beeper-table index, or 0 if the beeperMode is BEEPER_SILENCE.
 478  */
 479 uint32_t beeperModeMaskForTableIndex(int idx)
 480 {
 481     beeperMode_e beeperMode = beeperModeForTableIndex(idx);
 482     if (beeperMode == BEEPER_SILENCE)
 483         return 0;
 484     return BEEPER_GET_FLAG(beeperMode);
 485 }
 486
 487 /*
 488  * Returns the name for the given beeper-table index, or NULL if none.
 489  */
 490 const char *beeperNameForTableIndex(int idx)
 491 {
 492     return (idx >= 0 && idx < (int)BEEPER_TABLE_ENTRY_COUNT) ? beeperTable[idx].name : NULL;
 493 }
 494
 495 /*
 496  * Returns the number of entries in the beeper-sounds table.
 497  */
 498 int beeperTableEntryCount(void)
 499 {
 500     return (int)BEEPER_TABLE_ENTRY_COUNT;
 501 }
 502
 503 /*
 504  * Returns true if the beeper is on, false otherwise
 505  */
 506 bool isBeeperOn(void)
 507 {
 508     return beeperIsOn;
 509 }
 510
 511 #else
 512
 513 // Stub out beeper functions if #BEEPER not defined
 514 void beeper(beeperMode_e mode) {UNUSED(mode);}
 515 void beeperSilence(void) {}
 516 void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);}
 517 void beeperWarningBeeps(uint8_t beepCount) {UNUSED(beepCount);}
 518 void beeperUpdate(timeUs_t currentTimeUs) {UNUSED(currentTimeUs);}
 519 uint32_t getArmingBeepTimeMicros(void) {return 0;}
 520 beeperMode_e beeperModeForTableIndex(int idx) {UNUSED(idx); return BEEPER_SILENCE;}
 521 uint32_t beeperModeMaskForTableIndex(int idx) {UNUSED(idx); return 0;}
 522 const char *beeperNameForTableIndex(int idx) {UNUSED(idx); return NULL;}
 523 int beeperTableEntryCount(void) {return 0;}
 524 bool isBeeperOn(void) {return false;}
 525
 526 #endif
 527
 528 #ifdef USE_DSHOT
 529 timeUs_t getLastDshotBeaconCommandTimeUs(void)
 530 {
 531     return lastDshotBeaconCommandTimeUs;
 532 }
 533 #endif