Line data    Source code

       1             : #include "MMLPlayer.h"
       2             : 
       3             : #include <ctype.h>
       4             : #include <math.h>
       5             : #include <AP_HAL/AP_HAL.h>
       6             : #include <AP_Math/AP_Math.h>
       7             : #include <AP_Notify/AP_Notify.h>
       8             : 
       9             : #if HAL_CANMANAGER_ENABLED
      10             : #include <AP_DroneCAN/AP_DroneCAN.h>
      11             : #include <AP_CANManager/AP_CANManager.h>
      12             : #endif
      13             : 
      14             : extern const AP_HAL::HAL& hal;
      15             : 
      16           0 : void MMLPlayer::update()
      17             : {
      18             :     // Check if note is over
      19           0 :     if (_playing && AP_HAL::micros()-_note_start_us > _note_duration_us) {
      20           0 :         next_action();
      21             :     }
      22           0 : }
      23             : 
      24           0 : void MMLPlayer::prepare_to_play_string(const char* string)
      25             : {
      26           0 :     stop();
      27             : 
      28           0 :     _string = string;
      29           0 :     _next = 0;
      30           0 :     _tempo = 120;
      31           0 :     _default_note_length = 4;
      32           0 :     _note_mode = MODE_NORMAL;
      33           0 :     _octave = 4;
      34           0 :     _volume = 255;
      35           0 :     _silence_duration = 0;
      36           0 :     _repeat = false;
      37             : 
      38           0 :     _playing = true;
      39           0 :     _note_duration_us = 0;
      40           0 : }
      41             : 
      42           0 : void MMLPlayer::play(const char* string)
      43             : {
      44           0 :     prepare_to_play_string(string);
      45           0 :     next_action();
      46           0 : }
      47             : 
      48           0 : void MMLPlayer::stop()
      49             : {
      50           0 :     _playing = false;
      51           0 :     hal.util->toneAlarm_set_buzzer_tone(0,0,0);
      52           0 : }
      53             : 
      54           0 : void MMLPlayer::start_silence(float duration)
      55             : {
      56           0 :     _note_start_us = AP_HAL::micros();
      57           0 :     _note_duration_us = duration*1e6;
      58           0 :     hal.util->toneAlarm_set_buzzer_tone(0, 0, 0);
      59           0 : }
      60             : 
      61           0 : void MMLPlayer::start_note(float duration, float frequency, float volume)
      62             : {
      63           0 :     _note_start_us = AP_HAL::micros();
      64           0 :     _note_duration_us = duration*1e6;
      65           0 :     hal.util->toneAlarm_set_buzzer_tone(frequency, volume, _note_duration_us/1000U);
      66             : 
      67             : #if HAL_ENABLE_DRONECAN_DRIVERS
      68             :     // support CAN buzzers too
      69           0 :     uint8_t can_num_drivers = AP::can().get_num_drivers();
      70             :     uavcan_equipment_indication_BeepCommand msg;
      71             : 
      72           0 :     for (uint8_t i = 0; i < can_num_drivers; i++) {
      73           0 :         AP_DroneCAN *uavcan = AP_DroneCAN::get_dronecan(i);
      74           0 :         if (uavcan != nullptr &&
      75           0 :             (AP::notify().get_buzzer_types() & uint8_t(AP_Notify::BuzzerType::UAVCAN))) {
      76           0 :             msg.frequency = frequency;
      77           0 :             msg.duration = _note_duration_us*1.0e-6;
      78           0 :             uavcan->buzzer.broadcast(msg);
      79             :         }
      80             :     }
      81             : #endif
      82           0 : }
      83             : 
      84           0 : char MMLPlayer::next_char()
      85             : {
      86           0 :     while (_string[_next] != '\0' && isspace(_string[_next])) {
      87           0 :         _next++;
      88             :     }
      89             : 
      90           0 :     return toupper(_string[_next]);
      91             : }
      92             : 
      93           0 : uint8_t MMLPlayer::next_number()
      94             : {
      95           0 :     uint8_t ret = 0;
      96           0 :     while (isdigit(next_char())) {
      97           0 :         ret = (ret*10) + (next_char() - '0');
      98           0 :         _next++;
      99             :     }
     100           0 :     return ret;
     101             : }
     102             : 
     103           0 : size_t MMLPlayer::next_dots()
     104             : {
     105           0 :     size_t ret = 0;
     106           0 :     while (next_char() == '.') {
     107           0 :         ret++;
     108           0 :         _next++;
     109             :     }
     110           0 :     return ret;
     111             : }
     112             : 
     113           0 : float MMLPlayer::rest_duration(uint32_t rest_length, uint8_t dots) const
     114             : {
     115           0 :     float whole_note_period = 240.0f / _tempo;
     116           0 :     if (rest_length == 0) {
     117           0 :         rest_length = 1;
     118             :     }
     119             : 
     120           0 :     float rest_period = whole_note_period/rest_length;
     121           0 :     float dot_extension = rest_period * 0.5f;
     122             : 
     123           0 :     while (dots--) {
     124           0 :         rest_period += dot_extension;
     125           0 :         dot_extension *= 0.5f;
     126             :     }
     127             : 
     128           0 :     return rest_period;
     129             : }
     130             : 
     131           0 : void MMLPlayer::next_action()
     132             : {
     133           0 :     if (_silence_duration > 0) {
     134           0 :         start_silence(_silence_duration);
     135           0 :         _silence_duration = 0;
     136           0 :         return;
     137             :     }
     138             : 
     139           0 :     uint8_t note = 0;
     140             :     uint8_t note_length;
     141             : 
     142           0 :     while (note == 0) {
     143           0 :         char c = next_char();
     144           0 :         if (c == '\0') {
     145           0 :             if (_repeat) {
     146             :                 // don't "play" here, as we may have been called from
     147             :                 // there, and it turns out infinite recursion on
     148             :                 // invalid strings is suboptimal.  The next call to
     149             :                 // update() will push things out as appropriate.
     150           0 :                 prepare_to_play_string(_string);
     151             :             } else {
     152           0 :                 stop();
     153             :             }
     154           0 :             return;
     155             :         }
     156             : 
     157           0 :         _next++;
     158             : 
     159           0 :         switch (c) {
     160           0 :         case 'V': {
     161           0 :             _volume = next_number();
     162           0 :             break;
     163             :         }
     164           0 :         case 'L': {
     165           0 :             _default_note_length = next_number();
     166           0 :             if (_default_note_length == 0) {
     167           0 :                 stop();
     168           0 :                 return;
     169             :             }
     170           0 :             break;
     171             :         }
     172           0 :         case 'O':
     173           0 :             _octave = next_number();
     174           0 :             if (_octave > 6) {
     175           0 :                 _octave = 6;
     176             :             }
     177           0 :             break;
     178           0 :         case '<':
     179           0 :             if (_octave > 0) {
     180           0 :                 _octave--;
     181             :             }
     182           0 :             break;
     183           0 :         case '>':
     184           0 :             if (_octave < 6) {
     185           0 :                 _octave++;
     186             :             }
     187           0 :             break;
     188           0 :         case 'M':
     189           0 :             c = next_char();
     190           0 :             if (c == '\0') {
     191           0 :                 stop();
     192           0 :                 return;
     193             :             }
     194           0 :             _next++;
     195           0 :             switch (c) {
     196           0 :             case 'N':
     197           0 :                 _note_mode = MODE_NORMAL;
     198           0 :                 break;
     199           0 :             case 'L':
     200           0 :                 _note_mode = MODE_LEGATO;
     201           0 :                 break;
     202           0 :             case 'S':
     203           0 :                 _note_mode = MODE_STACCATO;
     204           0 :                 break;
     205           0 :             case 'F':
     206           0 :                 _repeat = false;
     207           0 :                 break;
     208           0 :             case 'B':
     209           0 :                 _repeat = true;
     210           0 :                 break;
     211           0 :             default:
     212           0 :                 stop();
     213           0 :                 return;
     214             :             }
     215           0 :             break;
     216           0 :         case 'R':
     217             :         case 'P': {
     218           0 :             uint8_t num = next_number();
     219           0 :             uint8_t dots = next_dots();
     220           0 :             start_silence(rest_duration(num, dots));
     221           0 :             return;
     222             :         }
     223           0 :         case 'T':
     224           0 :             _tempo = next_number();
     225           0 :             if (_tempo < 32) {
     226           0 :                 stop();
     227           0 :                 return;
     228             :             }
     229           0 :             break;
     230           0 :         case 'N':
     231           0 :             note = next_number();
     232           0 :             note_length = _default_note_length;
     233           0 :             if (note > 84) {
     234           0 :                 stop();
     235           0 :                 return;
     236             :             }
     237           0 :             if (note == 0) {
     238           0 :                 uint8_t num = next_number();
     239           0 :                 uint8_t dots = next_dots();
     240           0 :                 start_silence(rest_duration(num, dots));
     241           0 :                 return;
     242             :             }
     243           0 :             break;
     244           0 :         case 'A':
     245             :         case 'B':
     246             :         case 'C':
     247             :         case 'D':
     248             :         case 'E':
     249             :         case 'F':
     250             :         case 'G': {
     251             :             static const uint8_t note_tab[] = {9,11,0,2,4,5,7};
     252           0 :             note = note_tab[c-'A'] + (_octave*12) + 1;
     253             : 
     254           0 :             c = next_char();
     255             : 
     256           0 :             switch (c) {
     257           0 :             case '#':
     258             :             case '+':
     259           0 :                 if (note < 84) {
     260           0 :                     note++;
     261             :                 }
     262           0 :                 _next++;
     263           0 :                 break;
     264           0 :             case '-':
     265           0 :                 if (note > 1) {
     266           0 :                     note--;
     267             :                 }
     268           0 :                 _next++;
     269           0 :                 break;
     270           0 :             default:
     271           0 :                 break;
     272             :             }
     273           0 :             note_length = next_number();
     274           0 :             if (note_length == 0) {
     275           0 :                 note_length = _default_note_length;
     276             :             }
     277           0 :             break;
     278             :         }
     279           0 :         default:
     280           0 :             stop();
     281           0 :             return;
     282             :         }
     283             :     }
     284             : 
     285             :     // Avoid division by zero
     286           0 :     if (_tempo == 0 || note_length == 0) {
     287           0 :         stop();
     288           0 :         return;
     289             :     }
     290             : 
     291           0 :     float note_period = 240.0f / (float)_tempo / (float)note_length;
     292             : 
     293           0 :     switch (_note_mode) {
     294           0 :     case MODE_NORMAL:
     295           0 :         _silence_duration = note_period/8;
     296           0 :         break;
     297           0 :     case MODE_STACCATO:
     298           0 :         _silence_duration = note_period/4;
     299           0 :         break;
     300           0 :     case MODE_LEGATO:
     301           0 :         _silence_duration = 0;
     302           0 :         break;
     303             :     }
     304           0 :     note_period -= _silence_duration;
     305             : 
     306           0 :     float dot_extension = note_period * 0.5f;
     307           0 :     uint8_t dots = next_dots();
     308           0 :     while (dots--) {
     309           0 :         note_period += dot_extension;
     310           0 :         dot_extension *= 0.5f;
     311             :     }
     312             : 
     313           0 :     float note_frequency = 880.0f * expf(logf(2.0f) * ((int)note - 46) / 12.0f);
     314           0 :     float note_volume = _volume/255.0f;
     315           0 :     note_volume *= AP::notify().get_buzz_volume() * 0.01;
     316           0 :     note_volume = constrain_float(note_volume, 0, 1);
     317             : 
     318           0 :     note_frequency = constrain_float(note_frequency, 10, 22000);
     319             : 
     320           0 :     start_note(note_period, note_frequency, note_volume);
     321             : }