From 66d1d11aec545faf2a19abbd1866d020080640f9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 1 Aug 2018 20:15:41 +1000 Subject: [PATCH] AP_Notify: updated to new tonealarm system --- libraries/AP_Notify/AP_Notify.cpp | 20 +- libraries/AP_Notify/MMLPlayer.cpp | 287 ++++++++++++++++++ libraries/AP_Notify/MMLPlayer.h | 50 +++ .../{ToneAlarm_PX4.cpp => ToneAlarm.cpp} | 252 ++++++++------- .../{ToneAlarm_PX4.h => ToneAlarm.h} | 46 +-- libraries/AP_Notify/ToneAlarm_ChibiOS.cpp | 105 ------- libraries/AP_Notify/ToneAlarm_ChibiOS.h | 41 --- libraries/AP_Notify/ToneAlarm_Linux.cpp | 105 ------- libraries/AP_Notify/ToneAlarm_Linux.h | 43 --- 9 files changed, 509 insertions(+), 440 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} (54%) rename libraries/AP_Notify/{ToneAlarm_PX4.h => ToneAlarm.h} (64%) 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 9c86f83aae..00cf42c448 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -23,9 +23,7 @@ #include "NavioLED_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" @@ -141,7 +139,7 @@ void AP_Notify::add_backends(void) ADD_BACKEND(new AP_BoardLED()); ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL)); - ADD_BACKEND(new ToneAlarm_PX4()); + ADD_BACKEND(new AP_ToneAlarm()); ADD_BACKEND(new Display()); // Oreo LED enable/disable by NTF_OREO_THEME parameter @@ -153,14 +151,14 @@ void AP_Notify::add_backends(void) ADD_BACKEND(new PixRacerLED()); ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL)); - ADD_BACKEND(new ToneAlarm_PX4()); + ADD_BACKEND(new AP_ToneAlarm()); ADD_BACKEND(new Display()); #else // All other px4 boards use standard devices. ADD_BACKEND(new AP_BoardLED()); ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL)); - ADD_BACKEND(new ToneAlarm_PX4()); + ADD_BACKEND(new AP_ToneAlarm()); ADD_BACKEND(new Display()); #endif @@ -177,7 +175,7 @@ void AP_Notify::add_backends(void) ADD_BACKEND(new Buzzer()); #endif #ifdef HAL_PWM_ALARM - ADD_BACKEND(new ToneAlarm_ChibiOS()); + ADD_BACKEND(new AP_ToneAlarm()); #endif ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL)); @@ -189,13 +187,13 @@ void AP_Notify::add_backends(void) ADD_BACKEND(new AP_BoardLED()); ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL)); - ADD_BACKEND(new ToneAlarm_PX4()); + ADD_BACKEND(new AP_ToneAlarm()); ADD_BACKEND(new ExternalLED()); #else ADD_BACKEND(new VRBoard_LED()); ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL)); - ADD_BACKEND(new ToneAlarm_PX4()); + ADD_BACKEND(new AP_ToneAlarm()); ADD_BACKEND(new ExternalLED()); #endif @@ -242,7 +240,7 @@ void AP_Notify::add_backends(void) #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO ADD_BACKEND(new DiscoLED()); - ADD_BACKEND(new ToneAlarm_Linux()); + ADD_BACKEND(new AP_ToneAlarm()); #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RST_ZYNQ ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); @@ -251,7 +249,7 @@ void AP_Notify::add_backends(void) ADD_BACKEND(new AP_BoardLED()); ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL)); - ADD_BACKEND(new ToneAlarm_Linux()); + ADD_BACKEND(new AP_ToneAlarm()); #endif #elif CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT diff --git a/libraries/AP_Notify/MMLPlayer.cpp b/libraries/AP_Notify/MMLPlayer.cpp new file mode 100644 index 0000000000..39da08cccb --- /dev/null +++ b/libraries/AP_Notify/MMLPlayer.cpp @@ -0,0 +1,287 @@ +#include "MMLPlayer.h" + +#include +#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,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); +} + +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': { + 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/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; + + note_frequency = constrain_float(note_frequency, 10, 22000); + + 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 54% rename from libraries/AP_Notify/ToneAlarm_PX4.cpp rename to libraries/AP_Notify/ToneAlarm.cpp index 7061a0faf7..6a9edd4fa7 100644 --- a/libraries/AP_Notify/ToneAlarm_PX4.cpp +++ b/libraries/AP_Notify/ToneAlarm.cpp @@ -1,125 +1,122 @@ /* - ToneAlarm PX4 driver -*/ + * AP_ToneAlarm 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 +#include -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include "ToneAlarm_PX4.h" +#include "ToneAlarm.h" #include "AP_Notify.h" -#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 AP_ToneAlarm::Tone AP_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, &AP_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 AP_ToneAlarm::play_tone(const uint8_t tone_index) { uint32_t tnow_ms = AP_HAL::millis(); const Tone &tone_requested = _tones[tone_index]; - if(tone_requested.continuous) { + if (tone_requested.continuous) { _cont_tone_playing = tone_index; } @@ -129,31 +126,48 @@ 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 AP_ToneAlarm::_timer_task() +{ + if (_sem && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + _mml_player.update(); + _sem->give(); + } } -void ToneAlarm_PX4::stop_cont_tone() { - if(_cont_tone_playing == _tone_playing) { +void AP_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 AP_ToneAlarm::stop_cont_tone() +{ + if (_cont_tone_playing == _tone_playing) { play_string(""); _tone_playing = -1; } _cont_tone_playing = -1; } -void ToneAlarm_PX4::check_cont_tone() { +void AP_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, // resume the continuous tone - if (_cont_tone_playing != -1 && _tone_playing != _cont_tone_playing && tnow_ms-_tone_beginning_ms > AP_NOTIFY_PX4_MAX_TONE_LENGTH_MS) { + if (_cont_tone_playing != -1 && _tone_playing != _cont_tone_playing && tnow_ms-_tone_beginning_ms > AP_NOTIFY_TONEALARM_MAX_TONE_LENGTH_MS) { play_tone(_cont_tone_playing); } } // update - updates led according to timed_updated. Should be called at 50Hz -void ToneAlarm_PX4::update() +void AP_ToneAlarm::update() { // exit if buzzer is not enabled if (pNotify->buzzer_enabled() == false) { @@ -161,21 +175,21 @@ 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; } 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); + if (AP_Notify::flags.compass_cal_running) { + 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 +197,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 +223,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); + if (AP_Notify::events.failsafe_mode_change) { + 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 +270,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 +285,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 +300,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); - }else{ + 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 +315,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 +324,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 +332,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 +342,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 +351,52 @@ 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 AP_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); + if (_sem && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + _mml_player.stop(); + + strncpy(_tone_buf, packet.tune, MIN(sizeof(packet.tune), sizeof(_tone_buf)-1)); + _tone_buf[sizeof(_tone_buf)-1] = 0; + uint8_t len = strlen(_tone_buf); + uint8_t len2 = strnlen(packet.tune2, sizeof(packet.tune2)); + len2 = MIN((sizeof(_tone_buf)-1)-len, len2); + strncpy(_tone_buf+len, packet.tune2, len2); + _tone_buf[sizeof(_tone_buf)-1] = 0; + _sem->give(); + } } - - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4 diff --git a/libraries/AP_Notify/ToneAlarm_PX4.h b/libraries/AP_Notify/ToneAlarm.h similarity index 64% rename from libraries/AP_Notify/ToneAlarm_PX4.h rename to libraries/AP_Notify/ToneAlarm.h index 565388c422..f8f41db6ad 100644 --- a/libraries/AP_Notify/ToneAlarm_PX4.h +++ b/libraries/AP_Notify/ToneAlarm.h @@ -1,39 +1,40 @@ /* - 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_MAX_TONE_LENGTH_MS 2000 +#define AP_NOTIFY_TONEALARM_TONE_BUF_SIZE 100 -class ToneAlarm_PX4: public NotifyDevice -{ +class AP_ToneAlarm: public NotifyDevice { public: /// init - initialised the tone alarm - bool init(void); + bool init(void) override; /// 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 void play_tone(const uint8_t tone_index); @@ -47,7 +48,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 +76,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 fe43f2689b..0000000000 --- a/libraries/AP_Notify/ToneAlarm_ChibiOS.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_CHIBIOS -#include "ToneAlarm_ChibiOS.h" -#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); - } - } -} - -#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 8d7a5faa18..0000000000 --- a/libraries/AP_Notify/ToneAlarm_ChibiOS.h +++ /dev/null @@ -1,41 +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(); - -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 a54161ead8..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; -};