AP_Notify: merge ToneAlarm files, use new AP_HAL tonealarm interface

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.h"
-#include "ToneAlarm_ChibiOS.h"
-#include "ToneAlarm_PX4.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

libraries/AP_Notify/MMLPlayer.cpp

@@ -0,0 +1,274 @@
+#include "MMLPlayer.h"
+
+#include <ctype.h>
+#include <math.h>
+#include <AP_HAL/AP_HAL.h>
+
+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);
+}

libraries/AP_Notify/MMLPlayer.h

@@ -0,0 +1,50 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdlib.h>
+
+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();
+};
+
+
+
+
+
+
+
+
+
+

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
+ *   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
+ *   it under the terms of the GNU General Public License as published by
-   the Free Software Foundation, either version 3 of the License, or
+ *   the Free Software Foundation, either version 3 of the License, or
-   (at your option) any later version.
+ *   (at your option) any later version.
-
+ *
-   This program is distributed in the hope that it will be useful,
+ *   This program is distributed in the hope that it will be useful,
-   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
-   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-   GNU General Public License for more details.
+ *   GNU General Public License for more details.
-
+ *
-   You should have received a copy of the GNU General Public License
+ *   You should have received a copy of the GNU General Public License
-   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
 #include <AP_HAL/AP_HAL.h>
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#ifdef HAL_PWM_ALARM
-#include "ToneAlarm_PX4.h"
+#include "ToneAlarm.h"
 #include "AP_Notify.h"
 
-#include <sys/types.h>
+// #include <sys/types.h>
-#include <sys/stat.h>
+// #include <sys/stat.h>
-#include <fcntl.h>
+// #include <unistd.h>
-#include <unistd.h>
 #include <string.h>
 
-#include <drivers/drv_tone_alarm.h>
 #include <stdio.h>
-#include <errno.h>
 
 extern const AP_HAL::HAL& hal;
 
-const ToneAlarm_PX4::Tone ToneAlarm_PX4::_tones[] {
+const ToneAlarm::Tone ToneAlarm::_tones[] {
-    #define AP_NOTIFY_PX4_TONE_QUIET_NEG_FEEDBACK 0
+    #define AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK 0
     { "MFT200L4<<<B#A#2", false },
-    #define AP_NOTIFY_PX4_TONE_LOUD_NEG_FEEDBACK 1
+    #define AP_NOTIFY_TONE_LOUD_NEG_FEEDBACK 1
     { "MFT100L4>B#A#2P8B#A#2", false },
-    #define AP_NOTIFY_PX4_TONE_QUIET_NEU_FEEDBACK 2
+    #define AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK 2
     { "MFT200L4<B#", false },
-    #define AP_NOTIFY_PX4_TONE_LOUD_NEU_FEEDBACK 3
+    #define AP_NOTIFY_TONE_LOUD_NEU_FEEDBACK 3
     { "MFT100L4>B#", false },
-    #define AP_NOTIFY_PX4_TONE_QUIET_POS_FEEDBACK 4
+    #define AP_NOTIFY_TONE_QUIET_POS_FEEDBACK 4
     { "MFT200L4<A#B#", false },
-    #define AP_NOTIFY_PX4_TONE_LOUD_POS_FEEDBACK 5
+    #define AP_NOTIFY_TONE_LOUD_POS_FEEDBACK 5
     { "MFT100L4>A#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
     { "MFT200L4<G#6A#6B#4", false },
-    #define AP_NOTIFY_PX4_TONE_LOUD_ATTENTION_NEEDED 8
+    #define AP_NOTIFY_TONE_LOUD_ATTENTION_NEEDED 8
     { "MFT100L4>A#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
     { "MBNT255<C16P2", true },
-    #define AP_NOTIFY_PX4_TONE_WAITING_FOR_THROW 15
+    #define AP_NOTIFY_TONE_WAITING_FOR_THROW 15
     { "MBNT90L4O2A#O3DFN0N0N0", true},
-    #define AP_NOTIFY_PX4_TONE_LOUD_1 16
+    #define AP_NOTIFY_TONE_LOUD_1 16
     { "MFT100L8>B", 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
     { "MFMST200L32O3ceP32cdP32ceP32c<c>c<cccP8L32>c>c<P32<c<c", false },
-    #define AP_NOTIFY_PX4_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 },
+    #define AP_NOTIFY_TONE_STARTUP 29
+    { "MFT240L8O4aO5dcO4aO5dcO4aO5dcL16dcdcdcdc", false },
 };
 
-bool ToneAlarm_PX4::init()
+bool ToneAlarm::init()
 {
-    // open the tone alarm device
+    if (!hal.util->toneAlarm_init()) {
-    _tonealarm_fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY);
-    if (_tonealarm_fd == -1) {
-        hal.console->printf("ToneAlarm_PX4: Unable to open " TONEALARM0_DEVICE_PATH);
         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) {
+void ToneAlarm::_timer_task() {
-    write(_tonealarm_fd, str, strlen(str) + 1);
+    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_TONE_QUIET_COMPASS_CALIBRATING_CTS);
-            play_tone(AP_NOTIFY_PX4_TONE_QUIET_POS_FEEDBACK);
+            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);
+        play_tone(AP_NOTIFY_TONE_TUNING_START);
-        AP_Notify::events.tune_started = 0;        
+        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));
+        play_tone(AP_NOTIFY_TONE_LOUD_1 + (AP_Notify::events.tune_next-1));
-        AP_Notify::events.tune_next = 0;        
+        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
+ *  handle a PLAY_TUNE message
-*/
+ */
-void ToneAlarm_PX4::handle_play_tune(mavlink_message_t *msg)
+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

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
+ *   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
+ *   it under the terms of the GNU General Public License as published by
-   the Free Software Foundation, either version 3 of the License, or
+ *   the Free Software Foundation, either version 3 of the License, or
-   (at your option) any later version.
+ *   (at your option) any later version.
-
+ *
-   This program is distributed in the hope that it will be useful,
+ *   This program is distributed in the hope that it will be useful,
-   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
-   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-   GNU General Public License for more details.
+ *   GNU General Public License for more details.
-
+ *
-   You should have received a copy of the GNU General Public License
+ *   You should have received a copy of the GNU General Public License
-   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 #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];
 };

libraries/AP_Notify/ToneAlarm_ChibiOS.cpp

@@ -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 <http://www.gnu.org/licenses/>.
- */
-
-#include <AP_HAL/AP_HAL.h>
-
-#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
-#include "ToneAlarm_ChibiOS.h"
-#include <AP_HAL_ChibiOS/ToneAlarm.h>
-#include <AP_Math/AP_Math.h>
-#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

libraries/AP_Notify/ToneAlarm_ChibiOS.h

@@ -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 <http://www.gnu.org/licenses/>.
- */
-#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;
-};

libraries/AP_Notify/ToneAlarm_Linux.cpp

@@ -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 <http://www.gnu.org/licenses/>.
- */
-
-#include <AP_HAL/AP_HAL.h>
-
-#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
-#include "ToneAlarm_Linux.h"
-
-#include <errno.h>
-#include <fcntl.h>
-#include <stdio.h>
-#include <sys/stat.h>
-#include <sys/types.h>
-#include <unistd.h>
-
-#include <AP_HAL_Linux/Util.h>
-
-#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

libraries/AP_Notify/ToneAlarm_Linux.h

@@ -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 <http://www.gnu.org/licenses/>.
- */
-#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;
-};