ardupilot/libraries/AP_Notify/MMLPlayer.cpp

319 lines
7.4 KiB
C++

#include "MMLPlayer.h"
#include <ctype.h>
#include <math.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_Notify/AP_Notify.h>
#if HAL_CANMANAGER_ENABLED
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <AP_CANManager/AP_CANManager.h>
#endif
extern const AP_HAL::HAL& hal;
void MMLPlayer::update()
{
// Check if note is over
if (_playing && AP_HAL::micros()-_note_start_us > _note_duration_us) {
next_action();
}
}
void MMLPlayer::prepare_to_play_string(const char* string)
{
stop();
_string = string;
_next = 0;
_tempo = 120;
_default_note_length = 4;
_note_mode = MODE_NORMAL;
_octave = 4;
_volume = 255;
_silence_duration = 0;
_repeat = false;
_playing = true;
_note_duration_us = 0;
}
void MMLPlayer::play(const char* string)
{
prepare_to_play_string(string);
next_action();
}
void MMLPlayer::stop()
{
_playing = false;
hal.util->toneAlarm_set_buzzer_tone(0,0,0);
}
void MMLPlayer::start_silence(float duration)
{
_note_start_us = AP_HAL::micros();
_note_duration_us = duration*1e6;
hal.util->toneAlarm_set_buzzer_tone(0, 0, 0);
}
void MMLPlayer::start_note(float duration, float frequency, float volume)
{
_note_start_us = AP_HAL::micros();
_note_duration_us = duration*1e6;
hal.util->toneAlarm_set_buzzer_tone(frequency, volume, _note_duration_us/1000U);
#if HAL_CANMANAGER_ENABLED
// support CAN buzzers too
uint8_t can_num_drivers = AP::can().get_num_drivers();
for (uint8_t i = 0; i < can_num_drivers; i++) {
AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i);
if (uavcan != nullptr &&
(AP::notify().get_buzzer_types() & AP_Notify::Notify_Buzz_UAVCAN)) {
uavcan->set_buzzer_tone(frequency, _note_duration_us*1.0e-6);
}
}
#endif
}
char MMLPlayer::next_char()
{
while (_string[_next] != '\0' && isspace(_string[_next])) {
_next++;
}
return toupper(_string[_next]);
}
uint8_t MMLPlayer::next_number()
{
uint8_t ret = 0;
while (isdigit(next_char())) {
ret = (ret*10) + (next_char() - '0');
_next++;
}
return ret;
}
size_t MMLPlayer::next_dots()
{
size_t ret = 0;
while (next_char() == '.') {
ret++;
_next++;
}
return ret;
}
float MMLPlayer::rest_duration(uint32_t rest_length, uint8_t dots) const
{
float whole_note_period = 240.0f / _tempo;
if (rest_length == 0) {
rest_length = 1;
}
float rest_period = whole_note_period/rest_length;
float dot_extension = rest_period * 0.5f;
while (dots--) {
rest_period += dot_extension;
dot_extension *= 0.5f;
}
return rest_period;
}
void MMLPlayer::next_action()
{
if (_silence_duration > 0) {
start_silence(_silence_duration);
_silence_duration = 0;
return;
}
uint8_t note = 0;
uint8_t note_length;
while (note == 0) {
char c = next_char();
if (c == '\0') {
if (_repeat) {
// don't "play" here, as we may have been called from
// there, and it turns out infinite recursion on
// invalid strings is suboptimal. The next call to
// update() will push things out as appropriate.
prepare_to_play_string(_string);
} else {
stop();
}
return;
}
_next++;
switch (c) {
case 'V': {
_volume = next_number();
break;
}
case 'L': {
_default_note_length = next_number();
if (_default_note_length == 0) {
stop();
return;
}
break;
}
case 'O':
_octave = next_number();
if (_octave > 6) {
_octave = 6;
}
break;
case '<':
if (_octave > 0) {
_octave--;
}
break;
case '>':
if (_octave < 6) {
_octave++;
}
break;
case 'M':
c = next_char();
if (c == '\0') {
stop();
return;
}
_next++;
switch (c) {
case 'N':
_note_mode = MODE_NORMAL;
break;
case 'L':
_note_mode = MODE_LEGATO;
break;
case 'S':
_note_mode = MODE_STACCATO;
break;
case 'F':
_repeat = false;
break;
case 'B':
_repeat = true;
break;
default:
stop();
return;
}
break;
case 'R':
case 'P': {
uint8_t num = next_number();
uint8_t dots = next_dots();
start_silence(rest_duration(num, dots));
return;
}
case 'T':
_tempo = next_number();
if (_tempo < 32) {
stop();
return;
}
break;
case 'N':
note = next_number();
note_length = _default_note_length;
if (note > 84) {
stop();
return;
}
if (note == 0) {
uint8_t num = next_number();
uint8_t dots = next_dots();
start_silence(rest_duration(num, dots));
return;
}
break;
case 'A':
case 'B':
case 'C':
case 'D':
case 'E':
case 'F':
case 'G': {
static const uint8_t note_tab[] = {9,11,0,2,4,5,7};
note = note_tab[c-'A'] + (_octave*12) + 1;
c = next_char();
switch (c) {
case '#':
case '+':
if (note < 84) {
note++;
}
_next++;
break;
case '-':
if (note > 1) {
note--;
}
_next++;
break;
default:
break;
}
note_length = next_number();
if (note_length == 0) {
note_length = _default_note_length;
}
break;
}
default:
stop();
return;
}
}
// Avoid division by zero
if (_tempo == 0 || note_length == 0) {
stop();
return;
}
float note_period = 240.0f / (float)_tempo / (float)note_length;
switch (_note_mode) {
case MODE_NORMAL:
_silence_duration = note_period/8;
break;
case MODE_STACCATO:
_silence_duration = note_period/4;
break;
case MODE_LEGATO:
_silence_duration = 0;
break;
}
note_period -= _silence_duration;
float dot_extension = note_period * 0.5f;
uint8_t dots = next_dots();
while (dots--) {
note_period += dot_extension;
dot_extension *= 0.5f;
}
float note_frequency = 880.0f * expf(logf(2.0f) * ((int)note - 46) / 12.0f);
float note_volume = _volume/255.0f;
note_volume *= AP::notify().get_buzz_volume() * 0.01;
note_volume = constrain_float(note_volume, 0, 1);
note_frequency = constrain_float(note_frequency, 10, 22000);
start_note(note_period, note_frequency, note_volume);
}