HAL_Linux: make tonealarm generation a passthrough operation

get rid of all delays and while loops(that may turn into infinite loop)

libraries/AP_HAL_Linux/ToneAlarmDriver.cpp

@@ -14,6 +14,7 @@
 using namespace Linux;
 
 extern const AP_HAL::HAL& hal;
+static int state;
 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,
@@ -23,17 +24,17 @@ NOTE_C7, NOTE_CS7, NOTE_D7, NOTE_DS7, NOTE_E7, NOTE_F7, NOTE_FS7, NOTE_G7, NOTE_
 
 //List of RTTTL tones 
 const char* LinuxUtil::tune[TONE_NUMBER_OF_TUNES] = { 
-                                "Startup:d=8,o=6,b=240:a,d7,c7,a,d7,c7,a,d7,16d7,16c7,16d7,16c7,16d7,16c7,16d7,16c7",
+                                "Startup:d=8,o=6,b=480:a,d7,c7,a,d7,c7,a,d7,16d7,16c7,16d7,16c7,16d7,16c7,16d7,16c7",
-                                "Error:d=4,o=6,b=200:8a,8a,8a,p,a,a,a,p",
+                                "Error:d=4,o=6,b=400:8a,8a,8a,p,a,a,a,p",
-                                "notify_pos:d=4,o=6,b=200:8e,8e,a",
+                                "notify_pos:d=4,o=6,b=400:8e,8e,a",
-                                "notify_neut:d=4,o=6,b=200:8e,e",
+                                "notify_neut:d=4,o=6,b=400:8e,e",
-                                "notify_neg:d=4,o=6,b=200:8e,8c,8e,8c,8e,8c",
+                                "notify_neg:d=4,o=6,b=400:8e,8c,8e,8c,8e,8c",
                                 "arming_warn:d=1,o=4,b=75:g",
-                                "batt_war_slow:d=4,o=6,b=100:8a",
+                                "batt_war_slow:d=4,o=6,b=200:8a",
-                                "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",
+                                "batt_war_fast:d=4,o=6,b=512:8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a",
-                                "GPS_war:d=4,o=6,b=255:a,a,a,1f#",
+                                "GPS_war:d=4,o=6,b=512:a,a,a,1f#",
-                                "Arm_fail:d=4,o=4,b=255:b,a,p",
+                                "Arm_fail:d=4,o=4,b=512:b,a,p",
-                                "para_rel:d=16,o=6,b=255:a,g,a,g,a,g,a,g"};
+                                "para_rel:d=16,o=6,b=512:a,g,a,g,a,g,a,g"};
 //Tune Repeat true: play rtttl tune in loop, false: play only once
 bool LinuxUtil::tune_repeat[TONE_NUMBER_OF_TUNES] = {false,true,false,false,false,false,true,true,false,false,false};
 
@@ -44,6 +45,7 @@ void LinuxUtil::toneAlarm()
     run_fd = open("/sys/devices/ocp.3/pwm_test_P8_36.12/run",O_WRONLY);
     
     tune_num = -1;                    //initialy no tune to play
+    tune_pos = 0;
 }
 int8_t LinuxUtil::toneAlarm_init()
 {
@@ -60,106 +62,59 @@ void LinuxUtil::stop()
     write(run_fd,"0",sizeof(char));
 }
 
-void LinuxUtil::play(int tone,int duration)
+bool LinuxUtil::play()
 {
+    uint16_t cur_time = hal.scheduler->millis();
     if(tune_num != prev_tune_num){
         tune_changed = true;
-        return;
+        return true;
     }
-    if(tone != 0){
+    if(cur_note != 0){
         dprintf(run_fd,"0");
-        dprintf(period_fd,"%u",1000000000/tone);    
+        dprintf(period_fd,"%u",1000000000/cur_note);    
-        dprintf(duty_fd,"%u",500000000/tone);
+        dprintf(duty_fd,"%u",500000000/cur_note);
         dprintf(run_fd,"1");
+        cur_note =0;
+        prev_time = cur_time;
     }
-    hal.scheduler->delay(duration);
+    if((cur_time - prev_time) > duration){
+        stop();
+        if(tune[tune_num][tune_pos] == NULL){
+            if(!tune_repeat[tune_num]){
+                tune_num = -1;
+            }
+
+            tune_pos = 0;
+            state = 0;
+            return false;
+        }
+        return true;
+    }
+    return false;
 }
 
 void LinuxUtil::toneAlarm_set_tune(uint8_t tone)
 {
     tune_num = tone;
 }
-
+bool LinuxUtil::set_note(){
-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;
+    uint16_t scale,note,num =0;
-            while(isdigit(tune[tune_num][p])){
+    duration = 0;
-                num = (num * 10) + (tune[tune_num][p++] - '0');
+
+    while(isdigit(tune[tune_num][tune_pos])){                   //this is a safe while loop as it can't go further than 
+                                                                //the length of the rtttl tone string
+        num = (num * 10) + (tune[tune_num][tune_pos++] - '0');
     }
     if(num){
         duration = wholenote / num;
     } else{
-                duration = wholenote / default_dur;  // we will need to check if we are a dotted note after
+        duration = wholenote / 4;  // we will need to check if we are a dotted note after
     }        
     // now get the note
     note = 0;
 
-            switch(tune[tune_num][p]){
+    switch(tune[tune_num][tune_pos]){
         case 'c':
         note = 1;
         break;
@@ -186,60 +141,124 @@ void LinuxUtil::play_tune()
         note = 0;
     }
 
-            p++;
+    tune_pos++;
 
     // now, get optional '#' sharp
-            if(tune[tune_num][p] == '#'){
+    if(tune[tune_num][tune_pos] == '#'){
         note++;
-                p++;
+        tune_pos++;
     }
 
     // now, get optional '.' dotted note
 
-            if(tune[tune_num][p] == '.'){
+    if(tune[tune_num][tune_pos] == '.'){
         duration += duration/2;
-                p++;
+        tune_pos++;
     }
 
     // now, get scale
 
-            if(isdigit(tune[tune_num][p])){
+    if(isdigit(tune[tune_num][tune_pos])){
-                scale = tune[tune_num][p] - '0';
+        scale = tune[tune_num][tune_pos] - '0';
-                p++;
+        tune_pos++;
     } else{
         scale = default_oct;
     }
 
     scale += OCTAVE_OFFSET;
 
-            if(tune[tune_num][p] == ','){
+    if(tune[tune_num][tune_pos] == ','){
-                p++;       // skip comma for next note (or we may be at the end)
+        tune_pos++;       // 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_pos =0;
             tune_changed = false;
-                    return;
         }
-                stop();
+        cur_note = notes[(scale - 4) * 12 + note];
+        return true;
     } else{
-                hal.scheduler->delay(duration);
+        cur_note = 0;
-            }
+        return true;
     }
     
-        if(tune_repeat[tune_num]){
+}
-            continue;
+
-        } else{
+bool LinuxUtil::init_tune(){
-            tune_num = -1;
+
-            return;
+    uint16_t num;
+    default_dur = 4;
+    default_oct = 6;
+    bpm = 63;
+    prev_tune_num = tune_num;
+    if(tune_num <0 || tune_num > TONE_NUMBER_OF_TUNES){
+        return false;
     }
+    
+    while(tune[tune_num][tune_pos] != ':'){
+        if(tune[tune_num][tune_pos] == NULL){
+            return false;
         }
+        tune_pos++;
+    }
+    tune_pos++;                     
+    
+    if(tune[tune_num][tune_pos] == 'd'){
+        tune_pos+=2;             
+        num = 0;
+
+        while(isdigit(tune[tune_num][tune_pos])){
+            num = (num * 10) + (tune[tune_num][tune_pos++] - '0');
+        }
+        if(num > 0){ 
+            default_dur = num;
+        }        
+        tune_pos++;                   // skip comma
+    }
+
+
+    // get default octave
+
+    if(tune[tune_num][tune_pos] == 'o')
+    {
+        tune_pos+=2;              // skip "o="
+        num = tune[tune_num][tune_pos++] - '0';
+        if(num >= 3 && num <=7){
+            default_oct = num;
+        }
+        tune_pos++;                   // skip comma
+    }
+
+    // get BPM
+
+    if(tune[tune_num][tune_pos] == 'b'){
+        tune_pos+=2;              // skip "b="
+        num = 0;
+        while(isdigit(tune[tune_num][tune_pos])){
+            num = (num * 10) + (tune[tune_num][tune_pos++] - '0');
+        }
+        bpm = num;
+        tune_pos++;                   // 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)
+    return true;
 }
 
 void LinuxUtil::_toneAlarm_timer_tick(){
-    play_tune();
+    if(state == 0){
+        state = state + init_tune();
+    }else if(state == 1){
+        state = state + set_note();
+    }
+    if(state == 2){
+        state = state + play();
+    }else if(state == 3){
+        state = 1;
+    }
+    
 }
 #endif

libraries/AP_HAL_Linux/Util.h

@@ -135,17 +135,26 @@ public:
     private:
     void toneAlarm(void);
     void stop();
-    void play(int tone,int duration);
+    bool play();
     void play_tune();
-
+    bool set_note();
+	bool init_tune();
     static const char *tune[TONE_NUMBER_OF_TUNES];
     static bool tune_repeat[TONE_NUMBER_OF_TUNES];
     bool tune_changed;
+	uint8_t default_oct;
+	uint8_t default_dur;
+	uint16_t bpm;
+    uint16_t wholenote;
+    uint16_t cur_note;
+    uint16_t duration;
     int32_t prev_tune_num;
+	uint32_t prev_time;
     int32_t period_fd;
     int32_t duty_fd;
     int32_t run_fd;
     int8_t tune_num;
+    uint8_t tune_pos;
     
     int saved_argc;
     char* const *saved_argv;