From b31ddedfef0f62a0fc91a8f9ed23cb2c178e6428 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 24 Jul 2018 18:30:05 -0700 Subject: [PATCH] AP_Notify: merge ToneAlarm files, use new AP_HAL tonealarm interface --- libraries/AP_Notify/AP_Notify.cpp | 10 +- libraries/AP_Notify/MMLPlayer.cpp | 274 ++++++++++++++++++ libraries/AP_Notify/MMLPlayer.h | 50 ++++ .../{ToneAlarm_PX4.cpp => ToneAlarm.cpp} | 227 ++++++++------- .../{ToneAlarm_PX4.h => ToneAlarm.h} | 41 +-- libraries/AP_Notify/ToneAlarm_ChibiOS.cpp | 128 -------- libraries/AP_Notify/ToneAlarm_ChibiOS.h | 44 --- libraries/AP_Notify/ToneAlarm_Linux.cpp | 105 ------- libraries/AP_Notify/ToneAlarm_Linux.h | 43 --- 9 files changed, 473 insertions(+), 449 deletions(-) create mode 100644 libraries/AP_Notify/MMLPlayer.cpp create mode 100644 libraries/AP_Notify/MMLPlayer.h rename libraries/AP_Notify/{ToneAlarm_PX4.cpp => ToneAlarm.cpp} (57%) rename libraries/AP_Notify/{ToneAlarm_PX4.h => ToneAlarm.h} (67%) delete mode 100644 libraries/AP_Notify/ToneAlarm_ChibiOS.cpp delete mode 100644 libraries/AP_Notify/ToneAlarm_ChibiOS.h delete mode 100644 libraries/AP_Notify/ToneAlarm_Linux.cpp delete mode 100644 libraries/AP_Notify/ToneAlarm_Linux.h diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index 0d5ecbf731..ddd2b47d57 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -23,9 +23,7 @@ #include "PCA9685LED_I2C.h" #include "OreoLED_PX4.h" #include "RCOutputRGBLed.h" -#include "ToneAlarm_Linux.h" -#include "ToneAlarm_ChibiOS.h" -#include "ToneAlarm_PX4.h" +#include "ToneAlarm.h" #include "ToshibaLED_I2C.h" #include "VRBoard_LED.h" #include "DiscreteRGBLed.h" @@ -259,7 +257,7 @@ void AP_Notify::add_backends(void) // Add noise making devices #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || \ CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - ADD_BACKEND(new ToneAlarm_PX4()); + ADD_BACKEND(new ToneAlarm()); // ChibiOS noise makers #elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS @@ -267,7 +265,7 @@ void AP_Notify::add_backends(void) ADD_BACKEND(new Buzzer()); #endif #ifdef HAL_PWM_ALARM - ADD_BACKEND(new ToneAlarm_ChibiOS()); + ADD_BACKEND(new ToneAlarm()); #endif // Linux noise makers @@ -286,7 +284,7 @@ void AP_Notify::add_backends(void) ADD_BACKEND(new Buzzer()); #else // other linux - ADD_BACKEND(new ToneAlarm_Linux()); + ADD_BACKEND(new ToneAlarm()); #endif // F4Light noise makers diff --git a/libraries/AP_Notify/MMLPlayer.cpp b/libraries/AP_Notify/MMLPlayer.cpp new file mode 100644 index 0000000000..a2b205481e --- /dev/null +++ b/libraries/AP_Notify/MMLPlayer.cpp @@ -0,0 +1,274 @@ +#include "MMLPlayer.h" + +#include +#include +#include + +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::play(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; + next_action(); +} + +void MMLPlayer::stop() { + _playing = false; + hal.util->toneAlarm_set_buzzer_tone(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); +} + +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); +} + +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) { + 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/2; + + 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) { + play(_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': { + 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/2; + 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; + + start_note(note_period, note_frequency, note_volume); +} diff --git a/libraries/AP_Notify/MMLPlayer.h b/libraries/AP_Notify/MMLPlayer.h new file mode 100644 index 0000000000..9ab2ff60d3 --- /dev/null +++ b/libraries/AP_Notify/MMLPlayer.h @@ -0,0 +1,50 @@ +#pragma once + +#include +#include + +class MMLPlayer { +public: + void update(); + void play(const char* string); + void stop(); + +private: + bool _playing; + + uint32_t _note_duration_us; + uint32_t _note_start_us; + const char* _string; + uint8_t _tempo; + uint8_t _default_note_length; + uint8_t _volume; + size_t _next; + uint8_t _octave; + float _silence_duration; + bool _repeat; + enum node_mode_t { + MODE_NORMAL, + MODE_LEGATO, + MODE_STACCATO + } _note_mode; + + void start_silence(float duration); + void start_note(float duration, float frequency, float volume); + char next_char(); + uint8_t next_number(); + size_t next_dots(); + float rest_duration(uint32_t rest_length, uint8_t dots); + + // Called when the MML player should start the next action + void next_action(); +}; + + + + + + + + + + diff --git a/libraries/AP_Notify/ToneAlarm_PX4.cpp b/libraries/AP_Notify/ToneAlarm.cpp similarity index 57% rename from libraries/AP_Notify/ToneAlarm_PX4.cpp rename to libraries/AP_Notify/ToneAlarm.cpp index 7061a0faf7..f0327bf1a2 100644 --- a/libraries/AP_Notify/ToneAlarm_PX4.cpp +++ b/libraries/AP_Notify/ToneAlarm.cpp @@ -1,120 +1,120 @@ /* - ToneAlarm PX4 driver -*/ + * ToneAlarm ChibiOS driver + */ /* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . */ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include "ToneAlarm_PX4.h" +#ifdef HAL_PWM_ALARM +#include "ToneAlarm.h" #include "AP_Notify.h" -#include -#include -#include -#include +// #include +// #include +// #include #include -#include #include -#include extern const AP_HAL::HAL& hal; -const ToneAlarm_PX4::Tone ToneAlarm_PX4::_tones[] { - #define AP_NOTIFY_PX4_TONE_QUIET_NEG_FEEDBACK 0 +const ToneAlarm::Tone ToneAlarm::_tones[] { + #define AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK 0 { "MFT200L4<<B#A#2P8B#A#2", false }, - #define AP_NOTIFY_PX4_TONE_QUIET_NEU_FEEDBACK 2 + #define AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK 2 { "MFT200L4B#", false }, - #define AP_NOTIFY_PX4_TONE_QUIET_POS_FEEDBACK 4 + #define AP_NOTIFY_TONE_QUIET_POS_FEEDBACK 4 { "MFT200L4A#B#", false }, - #define AP_NOTIFY_PX4_TONE_LOUD_READY_OR_FINISHED 6 + #define AP_NOTIFY_TONE_LOUD_READY_OR_FINISHED 6 { "MFT100L4>G#6A#6B#4", false }, - #define AP_NOTIFY_PX4_TONE_QUIET_READY_OR_FINISHED 7 + #define AP_NOTIFY_TONE_QUIET_READY_OR_FINISHED 7 { "MFT200L4A#A#A#A#", false }, - #define AP_NOTIFY_PX4_TONE_QUIET_ARMING_WARNING 9 + #define AP_NOTIFY_TONE_QUIET_ARMING_WARNING 9 { "MNT75L1O2G", false }, - #define AP_NOTIFY_PX4_TONE_LOUD_WP_COMPLETE 10 + #define AP_NOTIFY_TONE_LOUD_WP_COMPLETE 10 { "MFT200L8G>C3", false }, - #define AP_NOTIFY_PX4_TONE_LOUD_LAND_WARNING_CTS 11 + #define AP_NOTIFY_TONE_LOUD_LAND_WARNING_CTS 11 { "MBT200L2A-G-A-G-A-G-", true }, - #define AP_NOTIFY_PX4_TONE_LOUD_VEHICLE_LOST_CTS 12 + #define AP_NOTIFY_TONE_LOUD_VEHICLE_LOST_CTS 12 { "MBT200>A#1", true }, - #define AP_NOTIFY_PX4_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 }, - #define AP_NOTIFY_PX4_TONE_QUIET_COMPASS_CALIBRATING_CTS 14 + #define AP_NOTIFY_TONE_QUIET_COMPASS_CALIBRATING_CTS 14 { "MBNT255B", false}, - #define AP_NOTIFY_PX4_TONE_LOUD_2 17 + #define AP_NOTIFY_TONE_LOUD_2 17 { "MFT100L8>BB", false}, - #define AP_NOTIFY_PX4_TONE_LOUD_3 18 + #define AP_NOTIFY_TONE_LOUD_3 18 { "MFT100L8>BBB", false}, - #define AP_NOTIFY_PX4_TONE_LOUD_4 19 + #define AP_NOTIFY_TONE_LOUD_4 19 { "MFT100L8>BBBB", false}, - #define AP_NOTIFY_PX4_TONE_LOUD_5 20 + #define AP_NOTIFY_TONE_LOUD_5 20 { "MFT100L8>BBBBB", false}, - #define AP_NOTIFY_PX4_TONE_LOUD_6 21 + #define AP_NOTIFY_TONE_LOUD_6 21 { "MFT100L8>BBBBBB", false}, - #define AP_NOTIFY_PX4_TONE_LOUD_7 22 + #define AP_NOTIFY_TONE_LOUD_7 22 { "MFT100L8>BBBBBBB", false}, - #define AP_NOTIFY_PX4_TONE_TUNING_START 23 + #define AP_NOTIFY_TONE_TUNING_START 23 { "MFT100L20>C#D#", false}, - #define AP_NOTIFY_PX4_TONE_TUNING_SAVE 24 + #define AP_NOTIFY_TONE_TUNING_SAVE 24 { "MFT100L10DBDB>", false}, - #define AP_NOTIFY_PX4_TONE_TUNING_ERROR 25 + #define AP_NOTIFY_TONE_TUNING_ERROR 25 { "MFT100L10>BBBBBBBB", false}, - #define AP_NOTIFY_PX4_TONE_LEAK_DETECTED 26 + #define AP_NOTIFY_TONE_LEAK_DETECTED 26 { "MBT255L8>A+AA-", true}, - #define AP_NOTIFY_PX4_TONE_QUIET_SHUTDOWN 27 + #define AP_NOTIFY_TONE_QUIET_SHUTDOWN 27 { "MFMST200L32O3ceP32cdP32ceP32ccc>cprintf("ToneAlarm_PX4: Unable to open " TONEALARM0_DEVICE_PATH); + if (!hal.util->toneAlarm_init()) { return false; } - + + _sem = hal.util->new_semaphore(); + // set initial boot states. This prevents us issuing a arming // warning in plane and rover on every boot flags.armed = AP_Notify::flags.armed; flags.failsafe_battery = AP_Notify::flags.failsafe_battery; flags.pre_arm_check = 1; _cont_tone_playing = -1; + hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &ToneAlarm::_timer_task, void)); + play_tone(AP_NOTIFY_TONE_STARTUP); return true; } // play_tune - play one of the pre-defined tunes -void ToneAlarm_PX4::play_tone(const uint8_t tone_index) +void ToneAlarm::play_tone(const uint8_t tone_index) { uint32_t tnow_ms = AP_HAL::millis(); const Tone &tone_requested = _tones[tone_index]; @@ -129,11 +129,26 @@ void ToneAlarm_PX4::play_tone(const uint8_t tone_index) play_string(tone_requested.str); } -void ToneAlarm_PX4::play_string(const char *str) { - write(_tonealarm_fd, str, strlen(str) + 1); +void ToneAlarm::_timer_task() { + if (_sem) { + _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER); + _mml_player.update(); + _sem->give(); + } } -void ToneAlarm_PX4::stop_cont_tone() { +void ToneAlarm::play_string(const char *str) { + if (_sem) { + _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER); + _mml_player.stop(); + strncpy(_tone_buf, str, AP_NOTIFY_TONEALARM_TONE_BUF_SIZE); + _tone_buf[AP_NOTIFY_TONEALARM_TONE_BUF_SIZE-1] = 0; + _mml_player.play(_tone_buf); + _sem->give(); + } +} + +void ToneAlarm::stop_cont_tone() { if(_cont_tone_playing == _tone_playing) { play_string(""); _tone_playing = -1; @@ -141,7 +156,7 @@ void ToneAlarm_PX4::stop_cont_tone() { _cont_tone_playing = -1; } -void ToneAlarm_PX4::check_cont_tone() { +void ToneAlarm::check_cont_tone() { uint32_t tnow_ms = AP_HAL::millis(); // if we are supposed to be playing a continuous tone, // and it was interrupted, and the interrupting tone has timed out, @@ -153,7 +168,7 @@ void ToneAlarm_PX4::check_cont_tone() { } // update - updates led according to timed_updated. Should be called at 50Hz -void ToneAlarm_PX4::update() +void ToneAlarm::update() { // exit if buzzer is not enabled if (pNotify->buzzer_enabled() == false) { @@ -161,10 +176,10 @@ void ToneAlarm_PX4::update() } check_cont_tone(); - + if (AP_Notify::flags.powering_off) { if (!flags.powering_off) { - play_tone(AP_NOTIFY_PX4_TONE_QUIET_SHUTDOWN); + play_tone(AP_NOTIFY_TONE_QUIET_SHUTDOWN); } flags.powering_off = AP_Notify::flags.powering_off; return; @@ -172,10 +187,10 @@ void ToneAlarm_PX4::update() if (AP_Notify::flags.compass_cal_running != flags.compass_cal_running) { if(AP_Notify::flags.compass_cal_running) { - play_tone(AP_NOTIFY_PX4_TONE_QUIET_COMPASS_CALIBRATING_CTS); - play_tone(AP_NOTIFY_PX4_TONE_QUIET_POS_FEEDBACK); + play_tone(AP_NOTIFY_TONE_QUIET_COMPASS_CALIBRATING_CTS); + play_tone(AP_NOTIFY_TONE_QUIET_POS_FEEDBACK); } else { - if(_cont_tone_playing == AP_NOTIFY_PX4_TONE_QUIET_COMPASS_CALIBRATING_CTS) { + if(_cont_tone_playing == AP_NOTIFY_TONE_QUIET_COMPASS_CALIBRATING_CTS) { stop_cont_tone(); } } @@ -183,22 +198,22 @@ void ToneAlarm_PX4::update() flags.compass_cal_running = AP_Notify::flags.compass_cal_running; if (AP_Notify::events.compass_cal_canceled) { - play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEU_FEEDBACK); + play_tone(AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK); return; } if (AP_Notify::events.initiated_compass_cal) { - play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEU_FEEDBACK); + play_tone(AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK); return; } if (AP_Notify::events.compass_cal_saved) { - play_tone(AP_NOTIFY_PX4_TONE_QUIET_READY_OR_FINISHED); + play_tone(AP_NOTIFY_TONE_QUIET_READY_OR_FINISHED); return; } if (AP_Notify::events.compass_cal_failed) { - play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEG_FEEDBACK); + play_tone(AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK); return; } @@ -209,45 +224,45 @@ void ToneAlarm_PX4::update() // notify the user when autotune or mission completes if (AP_Notify::flags.armed && (AP_Notify::events.autotune_complete || AP_Notify::events.mission_complete)) { - play_tone(AP_NOTIFY_PX4_TONE_LOUD_READY_OR_FINISHED); + play_tone(AP_NOTIFY_TONE_LOUD_READY_OR_FINISHED); } //notify the user when autotune fails if (AP_Notify::flags.armed && (AP_Notify::events.autotune_failed)) { - play_tone(AP_NOTIFY_PX4_TONE_LOUD_NEG_FEEDBACK); + play_tone(AP_NOTIFY_TONE_LOUD_NEG_FEEDBACK); } // notify the user when a waypoint completes if (AP_Notify::events.waypoint_complete) { - play_tone(AP_NOTIFY_PX4_TONE_LOUD_WP_COMPLETE); + play_tone(AP_NOTIFY_TONE_LOUD_WP_COMPLETE); } // notify the user when their mode change was successful if (AP_Notify::events.user_mode_change) { if (AP_Notify::flags.armed) { - play_tone(AP_NOTIFY_PX4_TONE_LOUD_NEU_FEEDBACK); + play_tone(AP_NOTIFY_TONE_LOUD_NEU_FEEDBACK); } else { - play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEU_FEEDBACK); + play_tone(AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK); } } // notify the user when their mode change failed if (AP_Notify::events.user_mode_change_failed) { if (AP_Notify::flags.armed) { - play_tone(AP_NOTIFY_PX4_TONE_LOUD_NEG_FEEDBACK); + play_tone(AP_NOTIFY_TONE_LOUD_NEG_FEEDBACK); } else { - play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEG_FEEDBACK); + play_tone(AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK); } } // failsafe initiated mode change if(AP_Notify::events.failsafe_mode_change) { - play_tone(AP_NOTIFY_PX4_TONE_LOUD_ATTENTION_NEEDED); + play_tone(AP_NOTIFY_TONE_LOUD_ATTENTION_NEEDED); } // notify the user when arming fails if (AP_Notify::events.arming_failed) { - play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEG_FEEDBACK); + play_tone(AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK); } // notify the user when RC contact is lost @@ -256,13 +271,13 @@ void ToneAlarm_PX4::update() if (flags.failsafe_radio) { // armed case handled by events.failsafe_mode_change if (!AP_Notify::flags.armed) { - play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEG_FEEDBACK); + play_tone(AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK); } } else { if (AP_Notify::flags.armed) { - play_tone(AP_NOTIFY_PX4_TONE_LOUD_POS_FEEDBACK); + play_tone(AP_NOTIFY_TONE_LOUD_POS_FEEDBACK); } else { - play_tone(AP_NOTIFY_PX4_TONE_QUIET_POS_FEEDBACK); + play_tone(AP_NOTIFY_TONE_QUIET_POS_FEEDBACK); } } } @@ -271,12 +286,12 @@ void ToneAlarm_PX4::update() if (flags.pre_arm_check != AP_Notify::flags.pre_arm_check) { flags.pre_arm_check = AP_Notify::flags.pre_arm_check; if (flags.pre_arm_check) { - play_tone(AP_NOTIFY_PX4_TONE_QUIET_READY_OR_FINISHED); + play_tone(AP_NOTIFY_TONE_QUIET_READY_OR_FINISHED); _have_played_ready_tone = true; } else { // only play sad tone if we've ever played happy tone: if (_have_played_ready_tone) { - play_tone(AP_NOTIFY_PX4_TONE_QUIET_NOT_READY_OR_NOT_FINISHED); + play_tone(AP_NOTIFY_TONE_QUIET_NOT_READY_OR_NOT_FINISHED); } } } @@ -286,12 +301,12 @@ void ToneAlarm_PX4::update() flags.armed = AP_Notify::flags.armed; if (flags.armed) { // arming tune - play_tone(AP_NOTIFY_PX4_TONE_QUIET_ARMING_WARNING); + play_tone(AP_NOTIFY_TONE_QUIET_ARMING_WARNING); }else{ // disarming tune - play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEU_FEEDBACK); + play_tone(AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK); if (!flags.leak_detected) { - stop_cont_tone(); + stop_cont_tone(); } } } @@ -301,7 +316,7 @@ void ToneAlarm_PX4::update() flags.failsafe_battery = AP_Notify::flags.failsafe_battery; if (flags.failsafe_battery) { // battery warning tune - play_tone(AP_NOTIFY_PX4_TONE_LOUD_BATTERY_ALERT_CTS); + play_tone(AP_NOTIFY_TONE_LOUD_BATTERY_ALERT_CTS); } } @@ -310,7 +325,7 @@ void ToneAlarm_PX4::update() flags.parachute_release = AP_Notify::flags.parachute_release; if (flags.parachute_release) { // parachute release warning tune - play_tone(AP_NOTIFY_PX4_TONE_LOUD_ATTENTION_NEEDED); + play_tone(AP_NOTIFY_TONE_LOUD_ATTENTION_NEEDED); } } @@ -318,7 +333,7 @@ void ToneAlarm_PX4::update() if (flags.vehicle_lost != AP_Notify::flags.vehicle_lost) { flags.vehicle_lost = AP_Notify::flags.vehicle_lost; if (flags.vehicle_lost) { - play_tone(AP_NOTIFY_PX4_TONE_LOUD_VEHICLE_LOST_CTS); + play_tone(AP_NOTIFY_TONE_LOUD_VEHICLE_LOST_CTS); } else { stop_cont_tone(); } @@ -328,7 +343,7 @@ void ToneAlarm_PX4::update() if (flags.waiting_for_throw != AP_Notify::flags.waiting_for_throw) { flags.waiting_for_throw = AP_Notify::flags.waiting_for_throw; if (flags.waiting_for_throw) { - play_tone(AP_NOTIFY_PX4_TONE_WAITING_FOR_THROW); + play_tone(AP_NOTIFY_TONE_WAITING_FOR_THROW); } else { stop_cont_tone(); } @@ -337,44 +352,44 @@ void ToneAlarm_PX4::update() if (flags.leak_detected != AP_Notify::flags.leak_detected) { flags.leak_detected = AP_Notify::flags.leak_detected; if (flags.leak_detected) { - play_tone(AP_NOTIFY_PX4_TONE_LEAK_DETECTED); + play_tone(AP_NOTIFY_TONE_LEAK_DETECTED); } else { stop_cont_tone(); } } if (AP_Notify::events.tune_started) { - play_tone(AP_NOTIFY_PX4_TONE_TUNING_START); - AP_Notify::events.tune_started = 0; + play_tone(AP_NOTIFY_TONE_TUNING_START); + AP_Notify::events.tune_started = 0; } if (AP_Notify::events.tune_next) { // signify which parameter in the set is starting - play_tone(AP_NOTIFY_PX4_TONE_LOUD_1 + (AP_Notify::events.tune_next-1)); - AP_Notify::events.tune_next = 0; + play_tone(AP_NOTIFY_TONE_LOUD_1 + (AP_Notify::events.tune_next-1)); + AP_Notify::events.tune_next = 0; } if (AP_Notify::events.tune_save) { - play_tone(AP_NOTIFY_PX4_TONE_TUNING_SAVE); + play_tone(AP_NOTIFY_TONE_TUNING_SAVE); AP_Notify::events.tune_save = 0; } if (AP_Notify::events.tune_error) { - play_tone(AP_NOTIFY_PX4_TONE_TUNING_ERROR); + play_tone(AP_NOTIFY_TONE_TUNING_ERROR); AP_Notify::events.tune_error = 0; } } /* - handle a PLAY_TUNE message -*/ -void ToneAlarm_PX4::handle_play_tune(mavlink_message_t *msg) + * handle a PLAY_TUNE message + */ +void ToneAlarm::handle_play_tune(mavlink_message_t *msg) { // decode mavlink message mavlink_play_tune_t packet; - + mavlink_msg_play_tune_decode(msg, &packet); play_string(packet.tune); } -#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4 +#endif // HAL_PWM_ALARM diff --git a/libraries/AP_Notify/ToneAlarm_PX4.h b/libraries/AP_Notify/ToneAlarm.h similarity index 67% rename from libraries/AP_Notify/ToneAlarm_PX4.h rename to libraries/AP_Notify/ToneAlarm.h index 565388c422..80741c39e4 100644 --- a/libraries/AP_Notify/ToneAlarm_PX4.h +++ b/libraries/AP_Notify/ToneAlarm.h @@ -1,28 +1,30 @@ /* - ToneAlarm PX4 driver -*/ + * ToneAlarm PX4 driver + */ /* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . */ #pragma once #include "NotifyDevice.h" +#include "MMLPlayer.h" // wait 2 seconds before assuming a tone is done and continuing the continuous tone #define AP_NOTIFY_PX4_MAX_TONE_LENGTH_MS 2000 +#define AP_NOTIFY_TONEALARM_TONE_BUF_SIZE 512 -class ToneAlarm_PX4: public NotifyDevice +class ToneAlarm: public NotifyDevice { public: /// init - initialised the tone alarm @@ -33,7 +35,7 @@ public: // handle a PLAY_TUNE message void handle_play_tune(mavlink_message_t *msg); - + private: /// play_tune - play one of the pre-defined tunes void play_tone(const uint8_t tone_index); @@ -47,7 +49,8 @@ private: // check_cont_tone - check if we should begin playing a continuous tone void check_cont_tone(); - int _tonealarm_fd; // file descriptor for the tone alarm + // timer task - runs at 1khz + void _timer_task(); /// tonealarm_type - bitmask of states we track struct tonealarm_type { @@ -74,4 +77,8 @@ private: }; const static Tone _tones[]; + + AP_HAL::Semaphore* _sem; + MMLPlayer _mml_player; + char _tone_buf[AP_NOTIFY_TONEALARM_TONE_BUF_SIZE]; }; diff --git a/libraries/AP_Notify/ToneAlarm_ChibiOS.cpp b/libraries/AP_Notify/ToneAlarm_ChibiOS.cpp deleted file mode 100644 index b4791936de..0000000000 --- a/libraries/AP_Notify/ToneAlarm_ChibiOS.cpp +++ /dev/null @@ -1,128 +0,0 @@ -/* - ToneAlarm Linux driver -*/ -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS -#include "ToneAlarm_ChibiOS.h" -#include -#include -#include "AP_Notify.h" - -extern const AP_HAL::HAL& hal; - -bool ToneAlarm_ChibiOS::init() -{ - // open the tone alarm device - bool _initialized = hal.util->toneAlarm_init(); - if (!_initialized) { - hal.console->printf("AP_Notify: Failed to initialise ToneAlarm"); - return false; - } - - // set initial boot states. This prevents us issuing a arming - // warning in plane and rover on every boot - flags.armed = AP_Notify::flags.armed; - flags.failsafe_battery = AP_Notify::flags.failsafe_battery; - return true; -} - -// play_tune - play one of the pre-defined tunes -bool ToneAlarm_ChibiOS::play_tune(uint8_t tune_number) -{ - hal.util->toneAlarm_set_tune(tune_number); - return true; -} - - -// update - updates led according to timed_updated. Should be called at 50Hz -void ToneAlarm_ChibiOS::update() -{ - // exit if buzzer is not enabled - if (pNotify->buzzer_enabled() == false) { - return; - } - - // check for arming failure - if (AP_Notify::events.arming_failed) { - play_tune(TONE_ARMING_FAILURE_TUNE); - } - - // check if arming status has changed - if (flags.armed != AP_Notify::flags.armed) { - flags.armed = AP_Notify::flags.armed; - if (flags.armed) { - // arming tune - play_tune(TONE_ARMING_WARNING_TUNE); - }else{ - // disarming tune - play_tune(TONE_NOTIFY_NEUTRAL_TUNE); - } - } - - // notify the user when their mode change was successful - if (AP_Notify::events.user_mode_change) { - if (AP_Notify::flags.armed) { - play_tune(TONE_NOTIFY_MODE_CHANGE_LOUD); - } else { - play_tune(TONE_NOTIFY_MODE_CHANGE_SOFT); - } - } - - // check if battery status has changed - if (flags.failsafe_battery != AP_Notify::flags.failsafe_battery) { - flags.failsafe_battery = AP_Notify::flags.failsafe_battery; - if (flags.failsafe_battery) { - // low battery warning tune - play_tune(TONE_BATTERY_WARNING_FAST_TUNE); - } - } - - // check parachute release - if (flags.parachute_release != AP_Notify::flags.parachute_release) { - flags.parachute_release = AP_Notify::flags.parachute_release; - if (flags.parachute_release) { - // parachute release warning tune - play_tune(TONE_PARACHUTE_RELEASE_TUNE); - } - } -} - -/* - handle a PLAY_TUNE message -*/ -void ToneAlarm_ChibiOS::handle_play_tune(mavlink_message_t *msg) -{ - // decode mavlink message - mavlink_play_tune_t packet; - - mavlink_msg_play_tune_decode(msg, &packet); - - char str[100]; - strncpy(str, packet.tune, MIN(sizeof(packet.tune), sizeof(str)-1)); - str[sizeof(str)-1] = 0; - uint8_t len = strlen(str); - uint8_t len2 = strnlen(packet.tune2, sizeof(packet.tune2)); - len2 = MIN((sizeof(str)-1)-len, len2); - strncpy(str+len, packet.tune2, len2); - str[sizeof(str)-1] = 0; - - hal.util->toneAlarm_set_tune_string(str); -} - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS diff --git a/libraries/AP_Notify/ToneAlarm_ChibiOS.h b/libraries/AP_Notify/ToneAlarm_ChibiOS.h deleted file mode 100644 index a80636b96e..0000000000 --- a/libraries/AP_Notify/ToneAlarm_ChibiOS.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - ToneAlarm Linux driver -*/ -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ -#pragma once - -#include "NotifyDevice.h" - -class ToneAlarm_ChibiOS: public NotifyDevice -{ -public: - /// init - initialised the tone alarm - bool init(void); - - /// update - updates led according to timed_updated. Should be called at 50Hz - void update(); - - // handle a PLAY_TUNE message - void handle_play_tune(mavlink_message_t *msg); - -private: - /// play_tune - play one of the pre-defined tunes - bool play_tune(uint8_t tune_number); - - /// tonealarm_type - bitmask of states we track - struct tonealarm_type { - bool armed : 1; // false = disarmed, true = armed - bool failsafe_battery : 1; // true if battery failsafe - bool parachute_release : 1; // true if parachute is being released - } flags; -}; diff --git a/libraries/AP_Notify/ToneAlarm_Linux.cpp b/libraries/AP_Notify/ToneAlarm_Linux.cpp deleted file mode 100644 index a23c6f8d5a..0000000000 --- a/libraries/AP_Notify/ToneAlarm_Linux.cpp +++ /dev/null @@ -1,105 +0,0 @@ -/* - ToneAlarm Linux driver -*/ -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX -#include "ToneAlarm_Linux.h" - -#include -#include -#include -#include -#include -#include - -#include - -#include "AP_Notify.h" - -extern const AP_HAL::HAL& hal; - -bool ToneAlarm_Linux::init() -{ - // open the tone alarm device - bool initialized = hal.util->toneAlarm_init(); - if (!initialized) { - hal.console->printf("AP_Notify: Failed to initialise ToneAlarm"); - return false; - } - - // set initial boot states. This prevents us issuing a arming - // warning in plane and rover on every boot - flags.armed = AP_Notify::flags.armed; - flags.failsafe_battery = AP_Notify::flags.failsafe_battery; - return true; -} - -// play_tune - play one of the pre-defined tunes -bool ToneAlarm_Linux::play_tune(uint8_t tune_number) -{ - hal.util->toneAlarm_set_tune(tune_number); - return true; -} - - -// update - updates led according to timed_updated. Should be called at 50Hz -void ToneAlarm_Linux::update() -{ - // exit if buzzer is not enabled - if (pNotify->buzzer_enabled() == false) { - return; - } - - // check for arming failure - if (AP_Notify::events.arming_failed) { - play_tune(TONE_ARMING_FAILURE_TUNE); - } - - // check if arming status has changed - if (flags.armed != AP_Notify::flags.armed) { - flags.armed = AP_Notify::flags.armed; - if (flags.armed) { - // arming tune - play_tune(TONE_ARMING_WARNING_TUNE); - }else{ - // disarming tune - play_tune(TONE_NOTIFY_NEUTRAL_TUNE); - } - } - - // check if battery status has changed - if (flags.failsafe_battery != AP_Notify::flags.failsafe_battery) { - flags.failsafe_battery = AP_Notify::flags.failsafe_battery; - if (flags.failsafe_battery) { - // low battery warning tune - play_tune(TONE_BATTERY_WARNING_FAST_TUNE); - } - } - - // check parachute release - if (flags.parachute_release != AP_Notify::flags.parachute_release) { - flags.parachute_release = AP_Notify::flags.parachute_release; - if (flags.parachute_release) { - // parachute release warning tune - play_tune(TONE_PARACHUTE_RELEASE_TUNE); - } - } -} - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4 diff --git a/libraries/AP_Notify/ToneAlarm_Linux.h b/libraries/AP_Notify/ToneAlarm_Linux.h deleted file mode 100644 index b3049c6796..0000000000 --- a/libraries/AP_Notify/ToneAlarm_Linux.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - ToneAlarm Linux driver -*/ -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ -#pragma once - -#include "NotifyDevice.h" - -class ToneAlarm_Linux: public NotifyDevice -{ -public: - /// init - initialised the tone alarm - bool init(void); - - /// update - updates led according to timed_updated. Should be called at 50Hz - void update(); - -private: - /// play_tune - play one of the pre-defined tunes - bool play_tune(uint8_t tune_number); - - bool _initialized = false; - - /// tonealarm_type - bitmask of states we track - struct tonealarm_type { - bool armed : 1; // false = disarmed, true = armed - bool failsafe_battery : 1; // true if battery failsafe - bool parachute_release : 1; // true if parachute is being released - } flags; -};