HAL_Linux: Add ToneAlarm Driver to Linux_HAL

ToneAlarm is setup for Beaglebone Black using pwm, toneAlarm thread runs on second last priority.

libraries/AP_HAL_Linux/Scheduler.cpp

@@ -6,6 +6,7 @@
 #include "Storage.h"
 #include "RCInput.h"
 #include "UARTDriver.h"
+#include "Util.h"
 #include <sys/time.h>
 #include <poll.h>
 #include <unistd.h>
@@ -18,11 +19,12 @@ using namespace Linux;
 
 extern const AP_HAL::HAL& hal;
 
-#define APM_LINUX_TIMER_PRIORITY    14
-#define APM_LINUX_UART_PRIORITY     13
-#define APM_LINUX_RCIN_PRIORITY     12
-#define APM_LINUX_MAIN_PRIORITY     11
-#define APM_LINUX_IO_PRIORITY       10
+#define APM_LINUX_TIMER_PRIORITY        15
+#define APM_LINUX_UART_PRIORITY         14
+#define APM_LINUX_RCIN_PRIORITY         13
+#define APM_LINUX_MAIN_PRIORITY         12
+#define APM_LINUX_TONEALARM_PRIORITY    11
+#define APM_LINUX_IO_PRIORITY           10
 
 LinuxScheduler::LinuxScheduler()
 {}
@@ -76,7 +78,15 @@ void LinuxScheduler::init(void* machtnichts)
     pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
 
     pthread_create(&_rcin_thread_ctx, &thread_attr, (pthread_startroutine_t)&Linux::LinuxScheduler::_rcin_thread, this);
-  
+    
+    // the Tone Alarm thread runs at highest priority
+    param.sched_priority = APM_LINUX_TONEALARM_PRIORITY;
+    pthread_attr_init(&thread_attr);
+    (void)pthread_attr_setschedparam(&thread_attr, &param);
+    pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
+
+    pthread_create(&_tonealarm_thread_ctx, &thread_attr, (pthread_startroutine_t)&Linux::LinuxScheduler::_tonealarm_thread, this);
+    
     // the IO thread runs at lower priority
     pthread_attr_init(&thread_attr);
     param.sched_priority = APM_LINUX_IO_PRIORITY;
@@ -308,6 +318,21 @@ void *LinuxScheduler::_uart_thread(void)
     return NULL;
 }
 
+void *LinuxScheduler::_tonealarm_thread(void)
+{
+    _setup_realtime(32768);
+    while (system_initializing()) {
+        poll(NULL, 0, 1);        
+    }
+    while (true) {
+        _microsleep(20000);
+
+        // process tone command
+        ((LinuxUtil *)hal.util)->_toneAlarm_timer_tick();
+    }
+    return NULL;
+}
+
 void *LinuxScheduler::_io_thread(void)
 {
     _setup_realtime(32768);

libraries/AP_HAL_Linux/Scheduler.h

@@ -71,11 +71,13 @@ private:
     pthread_t _io_thread_ctx;
     pthread_t _rcin_thread_ctx;
     pthread_t _uart_thread_ctx;
+    pthread_t _tonealarm_thread_ctx;
 
     void *_timer_thread(void);
     void *_io_thread(void);
     void *_rcin_thread(void);
     void *_uart_thread(void);
+    void *_tonealarm_thread(void);
 
     void _run_timers(bool called_from_timer_thread);
     void _run_io(void);

libraries/AP_HAL_Linux/ToneAlarmDriver.cpp

@@ -0,0 +1,264 @@
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+
+#include "Util.h"
+#include <stdio.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <iostream>
+
+using namespace Linux;
+
+extern const AP_HAL::HAL& hal;
+static uint16_t notes[] = { 0,
+NOTE_C4, NOTE_CS4, NOTE_D4, NOTE_DS4, NOTE_E4, NOTE_F4, NOTE_FS4, NOTE_G4, NOTE_GS4, NOTE_A4, NOTE_AS4, NOTE_B4,
+NOTE_C5, NOTE_CS5, NOTE_D5, NOTE_DS5, NOTE_E5, NOTE_F5, NOTE_FS5, NOTE_G5, NOTE_GS5, NOTE_A5, NOTE_AS5, NOTE_B5,
+NOTE_C6, NOTE_CS6, NOTE_D6, NOTE_DS6, NOTE_E6, NOTE_F6, NOTE_FS6, NOTE_G6, NOTE_GS6, NOTE_A6, NOTE_AS6, NOTE_B6,
+NOTE_C7, NOTE_CS7, NOTE_D7, NOTE_DS7, NOTE_E7, NOTE_F7, NOTE_FS7, NOTE_G7, NOTE_GS7, NOTE_A7, NOTE_AS7, NOTE_B7
+};
+
+void LinuxUtil::toneAlarm()
+{
+    tune[TONE_STARTUP_TUNE] = "Startup:d=8,o=6,b=240:a,d7,c7,a,d7,c7,a,d7,16d7,16c7,16d7,16c7,16d7,16c7,16d7,16c7";
+    tune[TONE_ERROR_TUNE] = "Error:d=4,o=6,b=200:8a,8a,8a,p,a,a,a,p";
+    tune[TONE_NOTIFY_POSITIVE_TUNE] = "notify_pos:d=4,o=6,b=200:8e,8e,a";
+    tune[TONE_NOTIFY_NEUTRAL_TUNE] = "notify_neut:d=4,o=6,b=200:8e,e";
+    tune[TONE_NOTIFY_NEGATIVE_TUNE] = "notify_neg:d=4,o=6,b=200:8e,8c,8e,8c,8e,8c";
+    tune[TONE_ARMING_WARNING_TUNE] = "arming_warn:d=1,o=4,b=75:g";
+    tune[TONE_BATTERY_WARNING_SLOW_TUNE] = "batt_war_slow:d=4,o=6,b=100:8a";
+    tune[TONE_BATTERY_WARNING_FAST_TUNE] = "batt_war_fast:d=4,o=6,b=255:8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a";
+    tune[TONE_GPS_WARNING_TUNE] = "GPS_war:d=4,o=6,b=255:a,a,a,1f#";
+    tune[TONE_ARMING_FAILURE_TUNE] = "Arm_fail:d=4,o=4,b=255:b,a,p";
+    tune[TONE_PARACHUTE_RELEASE_TUNE] = "para_rel:d=16,o=6,b=255:a,g,a,g,a,g,a,g";
+    
+    tune_repeat[TONE_STARTUP_TUNE] = false;
+    tune_repeat[TONE_ERROR_TUNE] = true;
+    tune_repeat[TONE_NOTIFY_POSITIVE_TUNE] = false; 
+    tune_repeat[TONE_NOTIFY_NEUTRAL_TUNE] = false;
+    tune_repeat[TONE_NOTIFY_NEGATIVE_TUNE] = false;
+    tune_repeat[TONE_ARMING_WARNING_TUNE] = false;
+    tune_repeat[TONE_BATTERY_WARNING_SLOW_TUNE] = true;
+    tune_repeat[TONE_BATTERY_WARNING_FAST_TUNE] = true;
+    tune_repeat[TONE_GPS_WARNING_TUNE] = false;
+    tune_repeat[TONE_ARMING_FAILURE_TUNE] = false;
+    tune_repeat[TONE_PARACHUTE_RELEASE_TUNE] = false;
+    
+    char str[]="/sys/devices/ocp.3/pwm_test_P8_36.12/period";
+    char str1[]="/sys/devices/ocp.3/pwm_test_P8_36.12/duty";
+    char str2[]="/sys/devices/ocp.3/pwm_test_P8_36.12/run";
+    
+    period_fd = open(str,O_WRONLY);
+    duty_fd = open(str1,O_WRONLY);
+    run_fd = open(str2,O_WRONLY);
+    
+    tune_num = -1;                    //initialy no tune to play
+}
+uint8_t LinuxUtil::toneAlarm_init()
+{
+    tune_num = 0;                    //play startup tune
+    if((period_fd == -1) || (duty_fd == -1) || (run_fd == -1)){
+        return -1;
+    }
+    return 0;
+}
+
+void LinuxUtil::stop()
+{
+
+    write(run_fd,"0",sizeof(char));
+}
+
+void LinuxUtil::play(int tone,int duration)
+{
+
+    char buf[10];
+    if(tune_num != prev_tune_num){
+        tune_changed = true;
+        return;
+    }
+    if(tone != 0){
+        write(run_fd,"0",sizeof(char));
+    
+        sprintf(buf,"%u",1000000000/tone);    
+        write(period_fd,buf,sizeof(buf));
+        
+        sprintf(buf,"%u",500000000/tone);
+        write(duty_fd,buf,sizeof(buf));
+        
+        write(run_fd,"1",sizeof(char));
+    }
+    hal.scheduler->delay(duration);
+}
+
+void LinuxUtil::toneAlarm_set_tune(uint8_t tone)
+{
+    tune_num = tone;
+}
+
+void LinuxUtil::play_tune()
+{
+    if(tune_num < 0 || tune_num > 10){
+        return;
+    }
+    
+    uint32_t p = 0;
+    uint8_t default_dur = 4;
+    uint8_t default_oct = 6;
+    uint16_t bpm = 63;
+    uint16_t num;
+    uint32_t wholenote;
+    uint32_t duration;
+    uint8_t note;
+    uint8_t scale;
+    prev_tune_num = tune_num;
+    while(1){
+        while(tune[tune_num][p] != ':'){
+            p++;
+        }
+        p++;                     
+
+        if(tune[tune_num][p] == 'd'){
+            p+=2;              
+            num = 0;
+
+            while(isdigit(tune[tune_num][p])){
+                num = (num * 10) + (tune[tune_num][p++] - '0');
+            }
+            if(num > 0){ 
+                default_dur = num;
+            }        
+            p++;                   // skip comma
+        }
+
+
+        // get default octave
+
+        if(tune[tune_num][p] == 'o')
+        {
+            p+=2;              // skip "o="
+            num = tune[tune_num][p++] - '0';
+            if(num >= 3 && num <=7){
+                default_oct = num;
+            }
+            p++;                   // skip comma
+        }
+
+        // get BPM
+
+        if(tune[tune_num][p] == 'b'){
+            p+=2;              // skip "b="
+            num = 0;
+            while(isdigit(tune[tune_num][p])){
+                num = (num * 10) + (tune[tune_num][p++] - '0');
+            }
+            bpm = num;
+            p++;                   // skip colon
+        }
+
+        // BPM usually expresses the number of quarter notes per minute
+        wholenote = (60 * 1000L / bpm) * 4;  // this is the time for whole note (in milliseconds)
+
+
+        // now begin note loop
+        while(tune[tune_num][p]){
+            // first, get note duration, if available
+            num = 0;
+            while(isdigit(tune[tune_num][p])){
+                num = (num * 10) + (tune[tune_num][p++] - '0');
+            }
+            if(num){
+                duration = wholenote / num;
+            } else{
+                duration = wholenote / default_dur;  // we will need to check if we are a dotted note after
+            }        
+            // now get the note
+            note = 0;
+
+            switch(tune[tune_num][p]){
+                case 'c':
+                note = 1;
+                break;
+                case 'd':
+                note = 3;
+                break;
+                case 'e':
+                note = 5;
+                break;
+                case 'f':
+                note = 6;
+                break;
+                case 'g':
+                note = 8;
+                break;
+                case 'a':
+                note = 10;
+                break;
+                case 'b':
+                note = 12;
+                break;
+                case 'p':
+                default:
+                note = 0;
+            }
+
+            p++;
+
+            // now, get optional '#' sharp
+            if(tune[tune_num][p] == '#'){
+                note++;
+                p++;
+            }
+
+            // now, get optional '.' dotted note
+
+            if(tune[tune_num][p] == '.'){
+                duration += duration/2;
+                p++;
+            }
+
+            // now, get scale
+
+            if(isdigit(tune[tune_num][p])){
+                scale = tune[tune_num][p] - '0';
+                p++;
+            } else{
+                scale = default_oct;
+            }
+
+            scale += OCTAVE_OFFSET;
+
+            if(tune[tune_num][p] == ','){
+                p++;       // skip comma for next note (or we may be at the end)
+            }
+
+            // now play the note
+
+            if(note){
+                play(notes[(scale - 4) * 12 + note],duration);
+                if(tune_changed == true){
+                    tune_changed = false;
+                    return;
+                }
+                stop();
+            } else{
+                hal.scheduler->delay(duration);
+            }
+        }
+        
+        if(tune_repeat[tune_num]){
+            continue;
+        } else{
+            tune_num = -1;
+            return;
+        }
+    }
+}
+
+void LinuxUtil::_toneAlarm_timer_tick(){
+    play_tune();
+}
+#endif

libraries/AP_HAL_Linux/Util.h

@@ -5,13 +5,121 @@
 #include <AP_HAL.h>
 #include "AP_HAL_Linux_Namespace.h"
 
+#define OCTAVE_OFFSET 0
+
+#define NOTE_B0  31
+#define NOTE_C1  33
+#define NOTE_CS1 35
+#define NOTE_D1  37
+#define NOTE_DS1 39
+#define NOTE_E1  41
+#define NOTE_F1  44
+#define NOTE_FS1 46
+#define NOTE_G1  49
+#define NOTE_GS1 52
+#define NOTE_A1  55
+#define NOTE_AS1 58
+#define NOTE_B1  62
+#define NOTE_C2  65
+#define NOTE_CS2 69
+#define NOTE_D2  73
+#define NOTE_DS2 78
+#define NOTE_E2  82
+#define NOTE_F2  87
+#define NOTE_FS2 93
+#define NOTE_G2  98
+#define NOTE_GS2 104
+#define NOTE_A2  110
+#define NOTE_AS2 117
+#define NOTE_B2  123
+#define NOTE_C3  131
+#define NOTE_CS3 139
+#define NOTE_D3  147
+#define NOTE_DS3 156
+#define NOTE_E3  165
+#define NOTE_F3  175
+#define NOTE_FS3 185
+#define NOTE_G3  196
+#define NOTE_GS3 208
+#define NOTE_A3  220
+#define NOTE_AS3 233
+#define NOTE_B3  247
+#define NOTE_C4  262
+#define NOTE_CS4 277
+#define NOTE_D4  294
+#define NOTE_DS4 311
+#define NOTE_E4  330
+#define NOTE_F4  349
+#define NOTE_FS4 370
+#define NOTE_G4  392
+#define NOTE_GS4 415
+#define NOTE_A4  440
+#define NOTE_AS4 466
+#define NOTE_B4  494
+#define NOTE_C5  523
+#define NOTE_CS5 554
+#define NOTE_D5  587
+#define NOTE_DS5 622
+#define NOTE_E5  659
+#define NOTE_F5  698
+#define NOTE_FS5 740
+#define NOTE_G5  784
+#define NOTE_GS5 831
+#define NOTE_A5  880
+#define NOTE_AS5 932
+#define NOTE_B5  988
+#define NOTE_C6  1047
+#define NOTE_CS6 1109
+#define NOTE_D6  1175
+#define NOTE_DS6 1245
+#define NOTE_E6  1319
+#define NOTE_F6  1397
+#define NOTE_FS6 1480
+#define NOTE_G6  1568
+#define NOTE_GS6 1661
+#define NOTE_A6  1760
+#define NOTE_AS6 1865
+#define NOTE_B6  1976
+#define NOTE_C7  2093
+#define NOTE_CS7 2217
+#define NOTE_D7  2349
+#define NOTE_DS7 2489
+#define NOTE_E7  2637
+#define NOTE_F7  2794
+#define NOTE_FS7 2960
+#define NOTE_G7  3136
+#define NOTE_GS7 3322
+#define NOTE_A7  3520
+#define NOTE_AS7 3729
+#define NOTE_B7  3951
+#define NOTE_C8  4186
+#define NOTE_CS8 4435
+#define NOTE_D8  4699
+#define NOTE_DS8 4978
+
+#define     TONE_STARTUP_TUNE                   0
+#define     TONE_ERROR_TUNE                     1
+#define     TONE_NOTIFY_POSITIVE_TUNE           2
+#define     TONE_NOTIFY_NEUTRAL_TUNE            3
+#define     TONE_NOTIFY_NEGATIVE_TUNE           4
+#define     TONE_ARMING_WARNING_TUNE            5
+#define     TONE_BATTERY_WARNING_SLOW_TUNE      6
+#define     TONE_BATTERY_WARNING_FAST_TUNE      7
+#define     TONE_GPS_WARNING_TUNE               8
+#define     TONE_ARMING_FAILURE_TUNE            9    
+#define     TONE_PARACHUTE_RELEASE_TUNE         10
+
+#define TONE_NUMBER_OF_TUNES 11
+
 class Linux::LinuxUtil : public AP_HAL::Util {
 public:
     void init(int argc, char * const *argv) {
         saved_argc = argc;
         saved_argv = argv;
+        toneAlarm();
     }
 
+
     bool run_debug_shell(AP_HAL::BetterStream *stream) { return false; }
 
     /**
@@ -19,9 +127,30 @@ public:
      */
     void commandline_arguments(uint8_t &argc, char * const *&argv);
 
-private:
+    uint8_t toneAlarm_init();
+    void toneAlarm_set_tune(uint8_t tune);
+    
+    void _toneAlarm_timer_tick();
+    
+    private:
+    void toneAlarm(void);
+    void stop();
+    void play(int tone,int duration);
+    void play_tune();
+
+    const char *tune[TONE_NUMBER_OF_TUNES];
+    bool tune_repeat[TONE_NUMBER_OF_TUNES];
+    bool tune_changed;
+    uint32_t prev_tune_num;
+    int32_t period_fd;
+    int32_t duty_fd;
+    int32_t run_fd;
+    uint8_t tune_num;
+    
     int saved_argc;
     char* const *saved_argv;
 };
 
+
+
 #endif // __AP_HAL_LINUX_UTIL_H__