diff --git a/libraries/AP_HAL_Linux/ToneAlarmDriver.cpp b/libraries/AP_HAL_Linux/ToneAlarmDriver.cpp index 2ce6b62a65..5e2d620d38 100644 --- a/libraries/AP_HAL_Linux/ToneAlarmDriver.cpp +++ b/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", - "Error:d=4,o=6,b=200:8a,8a,8a,p,a,a,a,p", - "notify_pos:d=4,o=6,b=200:8e,8e,a", - "notify_neut:d=4,o=6,b=200:8e,e", - "notify_neg:d=4,o=6,b=200:8e,8c,8e,8c,8e,8c", + "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=400:8a,8a,8a,p,a,a,a,p", + "notify_pos:d=4,o=6,b=400:8e,8e,a", + "notify_neut:d=4,o=6,b=400:8e,e", + "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_fast:d=4,o=6,b=255: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#", - "Arm_fail:d=4,o=4,b=255:b,a,p", - "para_rel:d=16,o=6,b=255:a,g,a,g,a,g,a,g"}; + "batt_war_slow:d=4,o=6,b=200: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=512:a,a,a,1f#", + "Arm_fail:d=4,o=4,b=512:b,a,p", + "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,186 +62,203 @@ 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(duty_fd,"%u",500000000/tone); + dprintf(period_fd,"%u",1000000000/cur_note); + 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(){ + // first, get note duration, if available + uint16_t scale,note,num =0; + duration = 0; -void LinuxUtil::play_tune() -{ - if(tune_num < 0 || tune_num > 10){ - return; + 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 / 4; // we will need to check if we are a dotted note after + } + // now get the note + note = 0; + + switch(tune[tune_num][tune_pos]){ + 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; + } + + tune_pos++; + + // now, get optional '#' sharp + if(tune[tune_num][tune_pos] == '#'){ + note++; + tune_pos++; + } + + // now, get optional '.' dotted note + + if(tune[tune_num][tune_pos] == '.'){ + duration += duration/2; + tune_pos++; + } + + // now, get scale + + if(isdigit(tune[tune_num][tune_pos])){ + scale = tune[tune_num][tune_pos] - '0'; + tune_pos++; + } else{ + scale = default_oct; + } + + scale += OCTAVE_OFFSET; + + if(tune[tune_num][tune_pos] == ','){ + tune_pos++; // skip comma for next note (or we may be at the end) + } + // now play the note + + if(note){ + if(tune_changed == true){ + tune_pos =0; + tune_changed = false; + } + cur_note = notes[(scale - 4) * 12 + note]; + return true; + } else{ + cur_note = 0; + return true; } - uint32_t p = 0; - uint8_t default_dur = 4; - uint8_t default_oct = 6; - uint16_t bpm = 63; +} + +bool LinuxUtil::init_tune(){ + uint16_t num; - uint32_t wholenote; - uint32_t duration; - uint8_t note; - uint8_t scale; + default_dur = 4; + default_oct = 6; + bpm = 63; 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; - } + 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 diff --git a/libraries/AP_HAL_Linux/Util.h b/libraries/AP_HAL_Linux/Util.h index 87631933f5..74bd9546f6 100644 --- a/libraries/AP_HAL_Linux/Util.h +++ b/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;