diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp index da20e9133b..a3844bad79 100644 --- a/libraries/AP_HAL_Linux/Scheduler.cpp +++ b/libraries/AP_HAL_Linux/Scheduler.cpp @@ -32,7 +32,6 @@ extern const AP_HAL::HAL& hal; #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 #define APM_LINUX_TIMER_RATE 1000 @@ -43,11 +42,9 @@ extern const AP_HAL::HAL& hal; CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI #define APM_LINUX_RCIN_RATE 2000 -#define APM_LINUX_TONEALARM_RATE 100 #define APM_LINUX_IO_RATE 50 #else #define APM_LINUX_RCIN_RATE 100 -#define APM_LINUX_TONEALARM_RATE 100 #define APM_LINUX_IO_RATE 50 #endif @@ -76,7 +73,6 @@ void Scheduler::init() SCHED_THREAD(timer, TIMER), SCHED_THREAD(uart, UART), SCHED_THREAD(rcin, RCIN), - SCHED_THREAD(tonealarm, TONEALARM), SCHED_THREAD(io, IO), }; @@ -128,8 +124,7 @@ void Scheduler::_debug_stack() _timer_thread.get_stack_usage(), _io_thread.get_stack_usage(), _rcin_thread.get_stack_usage(), - _uart_thread.get_stack_usage(), - _tonealarm_thread.get_stack_usage()); + _uart_thread.get_stack_usage()); _last_stack_debug_msec = now; } } @@ -280,12 +275,6 @@ void Scheduler::_uart_task() _run_uarts(); } -void Scheduler::_tonealarm_task() -{ - // process tone command - Util::from(hal.util)->_toneAlarm_timer_tick(); -} - void Scheduler::_io_task() { // process any pending storage writes @@ -345,13 +334,11 @@ void Scheduler::teardown() _io_thread.stop(); _rcin_thread.stop(); _uart_thread.stop(); - _tonealarm_thread.stop(); _timer_thread.join(); _io_thread.join(); _rcin_thread.join(); _uart_thread.join(); - _tonealarm_thread.join(); } /* diff --git a/libraries/AP_HAL_Linux/Scheduler.h b/libraries/AP_HAL_Linux/Scheduler.h index b8c9290144..d49196f9f1 100644 --- a/libraries/AP_HAL_Linux/Scheduler.h +++ b/libraries/AP_HAL_Linux/Scheduler.h @@ -86,13 +86,11 @@ private: SchedulerThread _io_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_io_task, void), *this}; SchedulerThread _rcin_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_rcin_task, void), *this}; SchedulerThread _uart_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_uart_task, void), *this}; - SchedulerThread _tonealarm_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_tonealarm_task, void), *this}; void _timer_task(); void _io_task(); void _rcin_task(); void _uart_task(); - void _tonealarm_task(); void _run_io(); void _run_uarts(); diff --git a/libraries/AP_HAL_Linux/ToneAlarm.cpp b/libraries/AP_HAL_Linux/ToneAlarm.cpp index 02b1a82c11..fcc26ddf4d 100644 --- a/libraries/AP_HAL_Linux/ToneAlarm.cpp +++ b/libraries/AP_HAL_Linux/ToneAlarm.cpp @@ -8,33 +8,12 @@ #include #include +#include #include 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 -}; - -//List of RTTTL tones -const char* ToneAlarm::tune[TONE_NUMBER_OF_TUNES] = { - "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=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 ToneAlarm::tune_repeat[TONE_NUMBER_OF_TUNES] = {false,true,false,false,false,false,true,true,false,false,false}; ToneAlarm::ToneAlarm() { @@ -45,14 +24,10 @@ ToneAlarm::ToneAlarm() duty_fd = open("/sys/devices/ocp.3/pwm_test_P8_36.12/duty",O_WRONLY|O_CLOEXEC); run_fd = open("/sys/devices/ocp.3/pwm_test_P8_36.12/run",O_WRONLY|O_CLOEXEC); #endif - - tune_num = -1; //initialy no tune to play - tune_pos = 0; } bool ToneAlarm::init() { - tune_num = 0; //play startup tune if ((period_fd == -1) || (duty_fd == -1) || (run_fd == -1)) { #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || \ @@ -64,200 +39,14 @@ bool ToneAlarm::init() return true; } -void ToneAlarm::set_tune(uint8_t tone) +void ToneAlarm::set_buzzer_tone(float frequency, float volume, uint32_t duration_ms) { - tune_num = tone; -} - -bool ToneAlarm::is_tune_comp() -{ - return tune_comp; -} - -void ToneAlarm::stop() -{ - dprintf(run_fd,"0"); -} - -bool ToneAlarm::play() -{ - const uint32_t cur_time = AP_HAL::millis(); - if(tune_num != prev_tune_num){ - tune_changed = true; - return true; - } - if(cur_note != 0){ + if (is_zero(frequency) || is_zero(volume)) { dprintf(run_fd,"0"); - dprintf(period_fd,"%u",1000000000/cur_note); - dprintf(duty_fd,"%u",500000000/cur_note); + } else { + dprintf(run_fd,"0"); + dprintf(period_fd,"%u",(unsigned int)roundf(1000000000/frequency)); + dprintf(duty_fd,"%u",(unsigned int)roundf(volume*500000000/frequency)); dprintf(run_fd,"1"); - cur_note =0; - prev_time = cur_time; } - if((cur_time - prev_time) > duration){ - stop(); - if(tune[tune_num][tune_pos] == '\0'){ - if(!tune_repeat[tune_num]){ - tune_num = -1; - } - - tune_pos = 0; - tune_comp = true; - return false; - } - return true; - } - return false; -} - -bool ToneAlarm::set_note(){ - // first, get note duration, if available - uint16_t scale,note,num =0; - duration = 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 / 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; - } - -} - -bool ToneAlarm::init_tune(){ - - 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; - } - - tune_comp = false; - while(tune[tune_num][tune_pos] != ':'){ - if(tune[tune_num][tune_pos] == '\0'){ - 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; } diff --git a/libraries/AP_HAL_Linux/ToneAlarm.h b/libraries/AP_HAL_Linux/ToneAlarm.h index 6471fcc886..72e3f7d14f 100644 --- a/libraries/AP_HAL_Linux/ToneAlarm.h +++ b/libraries/AP_HAL_Linux/ToneAlarm.h @@ -2,140 +2,13 @@ #include -#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 - namespace Linux { class ToneAlarm { public: ToneAlarm(); - void set_tune(uint8_t tone); virtual bool init(); - virtual void stop(); - virtual bool play(); - bool is_tune_comp(); - bool set_note(); - bool init_tune(); - -protected: - bool tune_comp; - 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; - int8_t tune_num; - uint8_t tune_pos; + virtual void set_buzzer_tone(float frequency, float volume, uint32_t duration_ms); private: int32_t period_fd; diff --git a/libraries/AP_HAL_Linux/ToneAlarm_Disco.cpp b/libraries/AP_HAL_Linux/ToneAlarm_Disco.cpp index fa23a78ee8..a9fd39f2dc 100644 --- a/libraries/AP_HAL_Linux/ToneAlarm_Disco.cpp +++ b/libraries/AP_HAL_Linux/ToneAlarm_Disco.cpp @@ -20,63 +20,28 @@ #include #include #include "ToneAlarm_Disco.h" +#include extern const AP_HAL::HAL &hal; using namespace Linux; -ToneAlarm_Disco::ToneAlarm_Disco() -{ - // initialy no tune to play - tune_num = -1; - tune_pos = 0; -} +ToneAlarm_Disco::ToneAlarm_Disco(){} bool ToneAlarm_Disco::init() { - // play startup tune - tune_num = 0; - bebop_out = RCOutput_Disco::from(hal.rcout); return true; } -void ToneAlarm_Disco::stop() +void ToneAlarm_Disco::set_buzzer_tone(float frequency, float volume, uint32_t duration_ms) { - bebop_out->play_note(0, 0, 0); -} - -bool ToneAlarm_Disco::play() -{ - uint32_t cur_time = AP_HAL::millis(); - - if (tune_num != prev_tune_num){ - tune_changed = true; - return true; + if (is_zero(frequency) || is_zero(volume)) { + bebop_out->play_note(0, 0, 0); + } else { + bebop_out->play_note(TONEALARM_PWM_POWER, (uint16_t)roundf(frequency), duration_ms); } - - if (cur_note != 0){ - bebop_out->play_note(TONEALARM_PWM_POWER, cur_note, duration); - cur_note = 0; - prev_time = cur_time; - } - - if ((cur_time - prev_time) > duration){ - stop(); - if (tune[tune_num][tune_pos] == '\0'){ - if (!tune_repeat[tune_num]){ - tune_num = -1; - } - - tune_pos = 0; - tune_comp = true; - return false; - } - return true; - } - - return false; } #endif diff --git a/libraries/AP_HAL_Linux/ToneAlarm_Disco.h b/libraries/AP_HAL_Linux/ToneAlarm_Disco.h index e237c00c93..52bf083499 100644 --- a/libraries/AP_HAL_Linux/ToneAlarm_Disco.h +++ b/libraries/AP_HAL_Linux/ToneAlarm_Disco.h @@ -13,8 +13,7 @@ class ToneAlarm_Disco : public ToneAlarm { public: ToneAlarm_Disco(); bool init() override; - void stop() override; - bool play() override; + void set_buzzer_tone(float frequency, float volume, uint32_t duration_ms) override; private: RCOutput_Bebop *bebop_out; diff --git a/libraries/AP_HAL_Linux/Util.cpp b/libraries/AP_HAL_Linux/Util.cpp index 77f64f677e..378ee0e2c4 100644 --- a/libraries/AP_HAL_Linux/Util.cpp +++ b/libraries/AP_HAL_Linux/Util.cpp @@ -17,7 +17,6 @@ using namespace Linux; extern const AP_HAL::HAL& hal; -static int state; #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO ToneAlarm_Disco Util::_toneAlarm; #else @@ -63,34 +62,6 @@ void Util::commandline_arguments(uint8_t &argc, char * const *&argv) argv = saved_argv; } -bool Util::toneAlarm_init() -{ - return _toneAlarm.init(); -} - -void Util::toneAlarm_set_tune(uint8_t tone) -{ - _toneAlarm.set_tune(tone); -} - -void Util::_toneAlarm_timer_tick() { - if(state == 0) { - state = state + _toneAlarm.init_tune(); - } else if (state == 1) { - state = state + _toneAlarm.set_note(); - } - if (state == 2) { - state = state + _toneAlarm.play(); - } else if (state == 3) { - state = 1; - } - - if (_toneAlarm.is_tune_comp()) { - state = 0; - } - -} - void Util::set_hw_rtc(uint64_t time_utc_usec) { #if CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE diff --git a/libraries/AP_HAL_Linux/Util.h b/libraries/AP_HAL_Linux/Util.h index 7d83e94b15..13ff1801c1 100644 --- a/libraries/AP_HAL_Linux/Util.h +++ b/libraries/AP_HAL_Linux/Util.h @@ -36,11 +36,6 @@ public: */ void commandline_arguments(uint8_t &argc, char * const *&argv); - bool toneAlarm_init(); - void toneAlarm_set_tune(uint8_t tune); - - void _toneAlarm_timer_tick(); - /* set system clock in UTC microseconds */ @@ -100,6 +95,11 @@ public: int get_hw_arm32(); + bool toneAlarm_init() override { return _toneAlarm.init(); } + void toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t duration_ms) override { + _toneAlarm.set_buzzer_tone(frequency, volume, duration_ms); + } + private: #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO static ToneAlarm_Disco _toneAlarm;