Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Notify: run astyle on ToneAlarm and MMLPlayer

Browse Source
This commit is contained in:
Jonathan Challinger 2018-07-24 19:00:24 -07:00 committed by Andrew Tridgell
parent b31ddedfef
commit 720676b28c
3 changed files with 187 additions and 174 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

260
libraries/AP_Notify/MMLPlayer.cpp
View File

@ -6,14 +6,16 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
void MMLPlayer::update() { void MMLPlayer::update()
{
// Check if note is over // Check if note is over
if (_playing && AP_HAL::micros()-_note_start_us > _note_duration_us) { if (_playing && AP_HAL::micros()-_note_start_us > _note_duration_us) {
next_action(); next_action();
} }
} }
void MMLPlayer::play(const char* string) { void MMLPlayer::play(const char* string)
{
stop(); stop();
_string = string; _string = string;
@ -30,24 +32,28 @@ void MMLPlayer::play(const char* string) {
next_action(); next_action();
} }
void MMLPlayer::stop() { void MMLPlayer::stop()
{
_playing = false; _playing = false;
hal.util->toneAlarm_set_buzzer_tone(0,0); hal.util->toneAlarm_set_buzzer_tone(0,0);
} }
void MMLPlayer::start_silence(float duration) { void MMLPlayer::start_silence(float duration)
{
_note_start_us = AP_HAL::micros(); _note_start_us = AP_HAL::micros();
_note_duration_us = duration*1e6; _note_duration_us = duration*1e6;
hal.util->toneAlarm_set_buzzer_tone(0, 0); hal.util->toneAlarm_set_buzzer_tone(0, 0);
} }
void MMLPlayer::start_note(float duration, float frequency, float volume) { void MMLPlayer::start_note(float duration, float frequency, float volume)
{
_note_start_us = AP_HAL::micros(); _note_start_us = AP_HAL::micros();
_note_duration_us = duration*1e6; _note_duration_us = duration*1e6;
hal.util->toneAlarm_set_buzzer_tone(frequency, volume); hal.util->toneAlarm_set_buzzer_tone(frequency, volume);
} }
char MMLPlayer::next_char() { char MMLPlayer::next_char()
{
while (_string[_next] != '\0' && isspace(_string[_next])) { while (_string[_next] != '\0' && isspace(_string[_next])) {
_next++; _next++;
} }
@ -55,7 +61,8 @@ char MMLPlayer::next_char() {
return toupper(_string[_next]); return toupper(_string[_next]);
} }
uint8_t MMLPlayer::next_number() { uint8_t MMLPlayer::next_number()
{
uint8_t ret = 0; uint8_t ret = 0;
while (isdigit(next_char())) { while (isdigit(next_char())) {
ret = (ret*10) + (next_char() - '0'); ret = (ret*10) + (next_char() - '0');
@ -64,7 +71,8 @@ uint8_t MMLPlayer::next_number() {
return ret; return ret;
} }
size_t MMLPlayer::next_dots() { size_t MMLPlayer::next_dots()
{
size_t ret = 0; size_t ret = 0;
while (next_char() == '.') { while (next_char() == '.') {
ret++; ret++;
@ -73,7 +81,8 @@ size_t MMLPlayer::next_dots() {
return ret; return ret;
} }
float MMLPlayer::rest_duration(uint32_t rest_length, uint8_t dots) { float MMLPlayer::rest_duration(uint32_t rest_length, uint8_t dots)
{
float whole_note_period = 240.0f / _tempo; float whole_note_period = 240.0f / _tempo;
if (rest_length == 0) { if (rest_length == 0) {
rest_length = 1; rest_length = 1;
@ -90,7 +99,8 @@ float MMLPlayer::rest_duration(uint32_t rest_length, uint8_t dots) {
return rest_period; return rest_period;
} }
void MMLPlayer::next_action() { void MMLPlayer::next_action()
{
if (_silence_duration > 0) { if (_silence_duration > 0) {
start_silence(_silence_duration); start_silence(_silence_duration);
_silence_duration = 0; _silence_duration = 0;
@ -113,129 +123,129 @@ void MMLPlayer::next_action() {
_next++; _next++;
switch(c) { switch (c) {
case 'V': { case 'V': {
_volume = next_number(); _volume = next_number();
break; break;
}
case 'L': {
_default_note_length = next_number();
if (_default_note_length == 0) {
stop();
return;
} }
case 'L': { break;
_default_note_length = next_number(); }
if (_default_note_length == 0) { case 'O':
stop(); _octave = next_number();
return; if (_octave > 6) {
} _octave = 6;
break;
} }
case 'O': break;
_octave = next_number(); case '<':
if (_octave > 6) { if (_octave > 0) {
_octave = 6; _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; break;
case '<': case 'L':
if (_octave > 0) { _note_mode = MODE_LEGATO;
_octave--;
}
break; break;
case '>': case 'S':
if (_octave < 6) { _note_mode = MODE_STACCATO;
_octave++;
}
break; break;
case 'M': case 'F':
c = next_char(); _repeat = false;
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; break;
case 'R': case 'B':
case 'P': { _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 num = next_number();
uint8_t dots = next_dots(); uint8_t dots = next_dots();
start_silence(rest_duration(num, dots)); start_silence(rest_duration(num, dots));
return; return;
} }
case 'T': break;
_tempo = next_number(); case 'A':
if (_tempo < 32) { case 'B':
stop(); case 'C':
return; case 'D':
} case 'E':
break; case 'F':
case 'N': case 'G': {
note = next_number(); uint8_t note_tab[] = {9,11,0,2,4,5,7};
note_length = _default_note_length; note = note_tab[c-'A'] + (_octave*12) + 1;
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': {
uint8_t note_tab[] = {9,11,0,2,4,5,7};
note = note_tab[c-'A'] + (_octave*12) + 1;
c = next_char(); c = next_char();
switch (c) { switch (c) {
case '#': case '#':
case '+': case '+':
if (note < 84) { if (note < 84) {
note++; note++;
}
_next++;
break;
case '-':
if (note > 1) {
note--;
}
_next++;
break;
default:
break;
} }
note_length = next_number(); _next++;
if (note_length == 0) { break;
note_length = _default_note_length; case '-':
if (note > 1) {
note--;
} }
_next++;
break;
default:
break; break;
} }
default: note_length = next_number();
stop(); if (note_length == 0) {
return; note_length = _default_note_length;
}
break;
}
default:
stop();
return;
} }
} }
@ -248,15 +258,15 @@ void MMLPlayer::next_action() {
float note_period = 240.0f / (float)_tempo / (float)note_length; float note_period = 240.0f / (float)_tempo / (float)note_length;
switch (_note_mode) { switch (_note_mode) {
case MODE_NORMAL: case MODE_NORMAL:
_silence_duration = note_period/8; _silence_duration = note_period/8;
break; break;
case MODE_STACCATO: case MODE_STACCATO:
_silence_duration = note_period/4; _silence_duration = note_period/4;
break; break;
case MODE_LEGATO: case MODE_LEGATO:
_silence_duration = 0; _silence_duration = 0;
break; break;
} }
note_period -= _silence_duration; note_period -= _silence_duration;

84
libraries/AP_Notify/ToneAlarm.cpp
View File

@ -32,65 +32,65 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
const ToneAlarm::Tone ToneAlarm::_tones[] { const ToneAlarm::Tone ToneAlarm::_tones[] {
#define AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK 0 #define AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK 0
{ "MFT200L4<<<B#A#2", false }, { "MFT200L4<<<B#A#2", false },
#define AP_NOTIFY_TONE_LOUD_NEG_FEEDBACK 1 #define AP_NOTIFY_TONE_LOUD_NEG_FEEDBACK 1
{ "MFT100L4>B#A#2P8B#A#2", false }, { "MFT100L4>B#A#2P8B#A#2", false },
#define AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK 2 #define AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK 2
{ "MFT200L4<B#", false }, { "MFT200L4<B#", false },
#define AP_NOTIFY_TONE_LOUD_NEU_FEEDBACK 3 #define AP_NOTIFY_TONE_LOUD_NEU_FEEDBACK 3
{ "MFT100L4>B#", false }, { "MFT100L4>B#", false },
#define AP_NOTIFY_TONE_QUIET_POS_FEEDBACK 4 #define AP_NOTIFY_TONE_QUIET_POS_FEEDBACK 4
{ "MFT200L4<A#B#", false }, { "MFT200L4<A#B#", false },
#define AP_NOTIFY_TONE_LOUD_POS_FEEDBACK 5 #define AP_NOTIFY_TONE_LOUD_POS_FEEDBACK 5
{ "MFT100L4>A#B#", false }, { "MFT100L4>A#B#", false },
#define AP_NOTIFY_TONE_LOUD_READY_OR_FINISHED 6 #define AP_NOTIFY_TONE_LOUD_READY_OR_FINISHED 6
{ "MFT100L4>G#6A#6B#4", false }, { "MFT100L4>G#6A#6B#4", false },
#define AP_NOTIFY_TONE_QUIET_READY_OR_FINISHED 7 #define AP_NOTIFY_TONE_QUIET_READY_OR_FINISHED 7
{ "MFT200L4<G#6A#6B#4", false }, { "MFT200L4<G#6A#6B#4", false },
#define AP_NOTIFY_TONE_LOUD_ATTENTION_NEEDED 8 #define AP_NOTIFY_TONE_LOUD_ATTENTION_NEEDED 8
{ "MFT100L4>A#A#A#A#", false }, { "MFT100L4>A#A#A#A#", false },
#define AP_NOTIFY_TONE_QUIET_ARMING_WARNING 9 #define AP_NOTIFY_TONE_QUIET_ARMING_WARNING 9
{ "MNT75L1O2G", false }, { "MNT75L1O2G", false },
#define AP_NOTIFY_TONE_LOUD_WP_COMPLETE 10 #define AP_NOTIFY_TONE_LOUD_WP_COMPLETE 10
{ "MFT200L8G>C3", false }, { "MFT200L8G>C3", false },
#define AP_NOTIFY_TONE_LOUD_LAND_WARNING_CTS 11 #define AP_NOTIFY_TONE_LOUD_LAND_WARNING_CTS 11
{ "MBT200L2A-G-A-G-A-G-", true }, { "MBT200L2A-G-A-G-A-G-", true },
#define AP_NOTIFY_TONE_LOUD_VEHICLE_LOST_CTS 12 #define AP_NOTIFY_TONE_LOUD_VEHICLE_LOST_CTS 12
{ "MBT200>A#1", true }, { "MBT200>A#1", true },
#define AP_NOTIFY_TONE_LOUD_BATTERY_ALERT_CTS 13 #define AP_NOTIFY_TONE_LOUD_BATTERY_ALERT_CTS 13
{ "MBNT255>A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8", true }, { "MBNT255>A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8", true },
#define AP_NOTIFY_TONE_QUIET_COMPASS_CALIBRATING_CTS 14 #define AP_NOTIFY_TONE_QUIET_COMPASS_CALIBRATING_CTS 14
{ "MBNT255<C16P2", true }, { "MBNT255<C16P2", true },
#define AP_NOTIFY_TONE_WAITING_FOR_THROW 15 #define AP_NOTIFY_TONE_WAITING_FOR_THROW 15
{ "MBNT90L4O2A#O3DFN0N0N0", true}, { "MBNT90L4O2A#O3DFN0N0N0", true},
#define AP_NOTIFY_TONE_LOUD_1 16 #define AP_NOTIFY_TONE_LOUD_1 16
{ "MFT100L8>B", false}, { "MFT100L8>B", false},
#define AP_NOTIFY_TONE_LOUD_2 17 #define AP_NOTIFY_TONE_LOUD_2 17
{ "MFT100L8>BB", false}, { "MFT100L8>BB", false},
#define AP_NOTIFY_TONE_LOUD_3 18 #define AP_NOTIFY_TONE_LOUD_3 18
{ "MFT100L8>BBB", false}, { "MFT100L8>BBB", false},
#define AP_NOTIFY_TONE_LOUD_4 19 #define AP_NOTIFY_TONE_LOUD_4 19
{ "MFT100L8>BBBB", false}, { "MFT100L8>BBBB", false},
#define AP_NOTIFY_TONE_LOUD_5 20 #define AP_NOTIFY_TONE_LOUD_5 20
{ "MFT100L8>BBBBB", false}, { "MFT100L8>BBBBB", false},
#define AP_NOTIFY_TONE_LOUD_6 21 #define AP_NOTIFY_TONE_LOUD_6 21
{ "MFT100L8>BBBBBB", false}, { "MFT100L8>BBBBBB", false},
#define AP_NOTIFY_TONE_LOUD_7 22 #define AP_NOTIFY_TONE_LOUD_7 22
{ "MFT100L8>BBBBBBB", false}, { "MFT100L8>BBBBBBB", false},
#define AP_NOTIFY_TONE_TUNING_START 23 #define AP_NOTIFY_TONE_TUNING_START 23
{ "MFT100L20>C#D#", false}, { "MFT100L20>C#D#", false},
#define AP_NOTIFY_TONE_TUNING_SAVE 24 #define AP_NOTIFY_TONE_TUNING_SAVE 24
{ "MFT100L10DBDB>", false}, { "MFT100L10DBDB>", false},
#define AP_NOTIFY_TONE_TUNING_ERROR 25 #define AP_NOTIFY_TONE_TUNING_ERROR 25
{ "MFT100L10>BBBBBBBB", false}, { "MFT100L10>BBBBBBBB", false},
#define AP_NOTIFY_TONE_LEAK_DETECTED 26 #define AP_NOTIFY_TONE_LEAK_DETECTED 26
{ "MBT255L8>A+AA-", true}, { "MBT255L8>A+AA-", true},
#define AP_NOTIFY_TONE_QUIET_SHUTDOWN 27 #define AP_NOTIFY_TONE_QUIET_SHUTDOWN 27
{ "MFMST200L32O3ceP32cdP32ceP32c<c>c<cccP8L32>c>c<P32<c<c", false }, { "MFMST200L32O3ceP32cdP32ceP32c<c>c<cccP8L32>c>c<P32<c<c", false },
#define AP_NOTIFY_TONE_QUIET_NOT_READY_OR_NOT_FINISHED 28 #define AP_NOTIFY_TONE_QUIET_NOT_READY_OR_NOT_FINISHED 28
{ "MFT200L4<B#4A#6G#6", false }, { "MFT200L4<B#4A#6G#6", false },
#define AP_NOTIFY_TONE_STARTUP 29 #define AP_NOTIFY_TONE_STARTUP 29
{ "MFT240L8O4aO5dcO4aO5dcO4aO5dcL16dcdcdcdc", false }, { "MFT240L8O4aO5dcO4aO5dcO4aO5dcL16dcdcdcdc", false },
}; };
@ -119,7 +119,7 @@ void ToneAlarm::play_tone(const uint8_t tone_index)
uint32_t tnow_ms = AP_HAL::millis(); uint32_t tnow_ms = AP_HAL::millis();
const Tone &tone_requested = _tones[tone_index]; const Tone &tone_requested = _tones[tone_index];
if(tone_requested.continuous) { if (tone_requested.continuous) {
_cont_tone_playing = tone_index; _cont_tone_playing = tone_index;
} }
@ -129,7 +129,8 @@ void ToneAlarm::play_tone(const uint8_t tone_index)
play_string(tone_requested.str); play_string(tone_requested.str);
} }
void ToneAlarm::_timer_task() { void ToneAlarm::_timer_task()
{
if (_sem) { if (_sem) {
_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER); _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER);
_mml_player.update(); _mml_player.update();
@ -137,7 +138,8 @@ void ToneAlarm::_timer_task() {
} }
} }
void ToneAlarm::play_string(const char *str) { void ToneAlarm::play_string(const char *str)
{
if (_sem) { if (_sem) {
_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER); _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER);
_mml_player.stop(); _mml_player.stop();
@ -148,15 +150,17 @@ void ToneAlarm::play_string(const char *str) {
} }
} }
void ToneAlarm::stop_cont_tone() { void ToneAlarm::stop_cont_tone()
if(_cont_tone_playing == _tone_playing) { {
if (_cont_tone_playing == _tone_playing) {
play_string(""); play_string("");
_tone_playing = -1; _tone_playing = -1;
} }
_cont_tone_playing = -1; _cont_tone_playing = -1;
} }
void ToneAlarm::check_cont_tone() { void ToneAlarm::check_cont_tone()
{
uint32_t tnow_ms = AP_HAL::millis(); uint32_t tnow_ms = AP_HAL::millis();
// if we are supposed to be playing a continuous tone, // if we are supposed to be playing a continuous tone,
// and it was interrupted, and the interrupting tone has timed out, // and it was interrupted, and the interrupting tone has timed out,
@ -186,11 +190,11 @@ void ToneAlarm::update()
} }
if (AP_Notify::flags.compass_cal_running != flags.compass_cal_running) { if (AP_Notify::flags.compass_cal_running != flags.compass_cal_running) {
if(AP_Notify::flags.compass_cal_running) { if (AP_Notify::flags.compass_cal_running) {
play_tone(AP_NOTIFY_TONE_QUIET_COMPASS_CALIBRATING_CTS); play_tone(AP_NOTIFY_TONE_QUIET_COMPASS_CALIBRATING_CTS);
play_tone(AP_NOTIFY_TONE_QUIET_POS_FEEDBACK); play_tone(AP_NOTIFY_TONE_QUIET_POS_FEEDBACK);
} else { } else {
if(_cont_tone_playing == AP_NOTIFY_TONE_QUIET_COMPASS_CALIBRATING_CTS) { if (_cont_tone_playing == AP_NOTIFY_TONE_QUIET_COMPASS_CALIBRATING_CTS) {
stop_cont_tone(); stop_cont_tone();
} }
} }
@ -256,7 +260,7 @@ void ToneAlarm::update()
} }
// failsafe initiated mode change // failsafe initiated mode change
if(AP_Notify::events.failsafe_mode_change) { if (AP_Notify::events.failsafe_mode_change) {
play_tone(AP_NOTIFY_TONE_LOUD_ATTENTION_NEEDED); play_tone(AP_NOTIFY_TONE_LOUD_ATTENTION_NEEDED);
} }
@ -302,7 +306,7 @@ void ToneAlarm::update()
if (flags.armed) { if (flags.armed) {
// arming tune // arming tune
play_tone(AP_NOTIFY_TONE_QUIET_ARMING_WARNING); play_tone(AP_NOTIFY_TONE_QUIET_ARMING_WARNING);
}else{ } else {
// disarming tune // disarming tune
play_tone(AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK); play_tone(AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK);
if (!flags.leak_detected) { if (!flags.leak_detected) {

17
libraries/AP_Notify/ToneAlarm.h
View File

@ -24,8 +24,7 @@
#define AP_NOTIFY_PX4_MAX_TONE_LENGTH_MS 2000 #define AP_NOTIFY_PX4_MAX_TONE_LENGTH_MS 2000
#define AP_NOTIFY_TONEALARM_TONE_BUF_SIZE 512 #define AP_NOTIFY_TONEALARM_TONE_BUF_SIZE 512
class ToneAlarm: public NotifyDevice class ToneAlarm: public NotifyDevice {
{
public: public:
/// init - initialised the tone alarm /// init - initialised the tone alarm
bool init(void); bool init(void);
@ -74,11 +73,11 @@ private:
struct Tone { struct Tone {
const char *str; const char *str;
const uint8_t continuous : 1; const uint8_t continuous : 1;
};
const static Tone _tones[];
AP_HAL::Semaphore* _sem;
MMLPlayer _mml_player;
char _tone_buf[AP_NOTIFY_TONEALARM_TONE_BUF_SIZE];
}; };
const static Tone _tones[];
AP_HAL::Semaphore* _sem;
MMLPlayer _mml_player;
char _tone_buf[AP_NOTIFY_TONEALARM_TONE_BUF_SIZE];
};