diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp index 70b59f05cd..02b3e3394e 100644 --- a/libraries/AP_HAL_Linux/Scheduler.cpp +++ b/libraries/AP_HAL_Linux/Scheduler.cpp @@ -6,6 +6,7 @@ #include "Storage.h" #include "RCInput.h" #include "UARTDriver.h" +#include "Util.h" #include #include #include @@ -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, ¶m); + 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); diff --git a/libraries/AP_HAL_Linux/Scheduler.h b/libraries/AP_HAL_Linux/Scheduler.h index ddc48365d8..6b24e8d0e0 100644 --- a/libraries/AP_HAL_Linux/Scheduler.h +++ b/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); diff --git a/libraries/AP_HAL_Linux/ToneAlarmDriver.cpp b/libraries/AP_HAL_Linux/ToneAlarmDriver.cpp new file mode 100644 index 0000000000..e674e5c352 --- /dev/null +++ b/libraries/AP_HAL_Linux/ToneAlarmDriver.cpp @@ -0,0 +1,264 @@ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX + +#include "Util.h" +#include +#include +#include +#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 +}; + +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 \ No newline at end of file diff --git a/libraries/AP_HAL_Linux/Util.h b/libraries/AP_HAL_Linux/Util.h index 8b5eecba9f..eec7025f1d 100644 --- a/libraries/AP_HAL_Linux/Util.h +++ b/libraries/AP_HAL_Linux/Util.h @@ -5,13 +5,121 @@ #include #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__