diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp index 76560422a7..775a0d5651 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp +++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp @@ -19,6 +19,7 @@ #include "AP_HAL_ChibiOS.h" #include "Scheduler.h" +#include "Util.h" #include #include @@ -36,6 +37,7 @@ using namespace ChibiOS; extern const AP_HAL::HAL& hal; THD_WORKING_AREA(_timer_thread_wa, 2048); THD_WORKING_AREA(_rcin_thread_wa, 512); +THD_WORKING_AREA(_toneAlarm_thread_wa, 2048); THD_WORKING_AREA(_io_thread_wa, 2048); THD_WORKING_AREA(_storage_thread_wa, 2048); #if HAL_WITH_UAVCAN @@ -69,6 +71,13 @@ void Scheduler::init() _rcin_thread, /* Thread function. */ this); /* Thread parameter. */ + // the toneAlarm thread runs at a medium priority + _toneAlarm_thread_ctx = chThdCreateStatic(_toneAlarm_thread_wa, + sizeof(_toneAlarm_thread_wa), + APM_TONEALARM_PRIORITY, /* Initial priority. */ + _toneAlarm_thread, /* Thread function. */ + this); /* Thread parameter. */ + // the IO thread runs at lower priority _io_thread_ctx = chThdCreateStatic(_io_thread_wa, sizeof(_io_thread_wa), @@ -301,6 +310,21 @@ void Scheduler::_rcin_thread(void *arg) } } +void Scheduler::_toneAlarm_thread(void *arg) +{ + Scheduler *sched = (Scheduler *)arg; + sched->_toneAlarm_thread_ctx->name = "toneAlarm"; + while (!sched->_hal_initialized) { + sched->delay_microseconds(10000); + } + while (true) { + sched->delay_microseconds(10000); + + // process tone command + Util::from(hal.util)->_toneAlarm_timer_tick(); + } +} + void Scheduler::_run_io(void) { if (_in_io_proc) { diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.h b/libraries/AP_HAL_ChibiOS/Scheduler.h index 557c24616a..b3ce71ec4e 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.h +++ b/libraries/AP_HAL_ChibiOS/Scheduler.h @@ -26,6 +26,7 @@ #define APM_MAIN_PRIORITY 180 #define APM_TIMER_PRIORITY 178 #define APM_RCIN_PRIORITY 177 +#define APM_TONEALARM_PRIORITY 61 #define APM_UART_PRIORITY 60 #define APM_STORAGE_PRIORITY 59 #define APM_IO_PRIORITY 58 @@ -111,6 +112,7 @@ private: thread_t* _rcin_thread_ctx; thread_t* _io_thread_ctx; thread_t* _storage_thread_ctx; + thread_t* _toneAlarm_thread_ctx; #if HAL_WITH_UAVCAN thread_t* _uavcan_thread_ctx; #endif @@ -119,6 +121,7 @@ private: static void _io_thread(void *arg); static void _storage_thread(void *arg); static void _uart_thread(void *arg); + static void _toneAlarm_thread(void *arg); #if HAL_WITH_UAVCAN static void _uavcan_thread(void *arg); #endif diff --git a/libraries/AP_HAL_ChibiOS/ToneAlarm.cpp b/libraries/AP_HAL_ChibiOS/ToneAlarm.cpp new file mode 100644 index 0000000000..7545374384 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/ToneAlarm.cpp @@ -0,0 +1,256 @@ +#include "ToneAlarm.h" +#include +#include + +using namespace ChibiOS; + +struct ToneAlarm::pwmGroup ToneAlarm::pwm_group = HAL_PWM_ALARM; + +#define isdigit(n) (n >= '0' && n <= '9') + +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() +{ + tune_num = -1; //initialy no tune to play + tune_pos = 0; +} + +bool ToneAlarm::init() +{ + // start PWM driver + pwm_group.pwm_cfg.period = 1000; + pwmStart(pwm_group.pwm_drv, &pwm_group.pwm_cfg); + + tune_num = 0; //play startup tune + + return true; +} + +void ToneAlarm::set_tune(uint8_t tone) +{ + tune_num = tone; +} + +bool ToneAlarm::is_tune_comp() +{ + return tune_comp; +} + +void ToneAlarm::stop() +{ + pwmDisableChannel(pwm_group.pwm_drv, ALARM_CHANNEL); + +} + +bool ToneAlarm::play() +{ + uint16_t cur_time = AP_HAL::millis(); + if(tune_num != prev_tune_num){ + tune_changed = true; + return true; + } + if(cur_note != 0){ + hal.console->printf("cur_note: %d, duration: %d\n", cur_note, duration); + // specify buzzer timer and channel with defines in hwdef.dat + pwmChangePeriod(pwm_group.pwm_drv, + pwm_group.pwm_cfg.frequency/cur_note); + + pwmEnableChannel(pwm_group.pwm_drv, ALARM_CHANNEL, + (pwm_group.pwm_cfg.frequency/2)/cur_note); + + 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_ChibiOS/ToneAlarm.h b/libraries/AP_HAL_ChibiOS/ToneAlarm.h new file mode 100644 index 0000000000..95baf782da --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/ToneAlarm.h @@ -0,0 +1,150 @@ +#pragma once + +#include "AP_HAL_ChibiOS.h" +#include "ch.h" +#include "hal.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 + +namespace ChibiOS { + +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; + +private: + struct pwmGroup { + PWMConfig pwm_cfg; + PWMDriver* pwm_drv; + }; + static pwmGroup pwm_group; +}; + +} diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 2d0015cd76..4111a95198 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -20,6 +20,7 @@ #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #include "Util.h" #include +#include "ToneAlarm.h" #if HAL_WITH_IO_MCU #include @@ -27,8 +28,13 @@ extern AP_IOMCU iomcu; #endif +extern const AP_HAL::HAL& hal; + using namespace ChibiOS; +static int state; +ToneAlarm Util::_toneAlarm; + extern "C" { size_t mem_available(void); void *malloc_ccm(size_t size); @@ -137,4 +143,33 @@ void Util::set_imu_target_temp(int8_t *target) #endif } +bool Util::toneAlarm_init() +{ + return _toneAlarm.init(); +} + +void Util::toneAlarm_set_tune(uint8_t tone) +{ + _toneAlarm.set_tune(tone); + hal.console->printf("set_tune: %d\n", 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; + } + +} + #endif //CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index 14a72569d5..642e9dac7a 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -19,12 +19,17 @@ #include #include "AP_HAL_ChibiOS_Namespace.h" #include "Semaphores.h" +#include "ToneAlarm.h" // this checks an address is in main memory and 16 bit aligned #define IS_DMA_SAFE(addr) ((uint32_t(addr) & 0xF0000001) == 0x20000000) class ChibiOS::Util : public AP_HAL::Util { public: + static Util *from(AP_HAL::Util *util) { + return static_cast(util); + } + bool run_debug_shell(AP_HAL::BetterStream *stream) { return false; } AP_HAL::Semaphore *new_semaphore(void) override { return new ChibiOS::Semaphore; } uint32_t available_memory() override; @@ -41,8 +46,12 @@ public: // IMU temperature control void set_imu_temp(float current); void set_imu_target_temp(int8_t *target); + bool toneAlarm_init(); + void toneAlarm_set_tune(uint8_t tone); + void _toneAlarm_timer_tick(); private: + static ToneAlarm _toneAlarm; void* try_alloc_from_ccm_ram(size_t size); uint32_t available_memory_in_ccm_ram(void); diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/fmuv4/hwdef.dat index 918f6f3cd8..cfd4589a2a 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/fmuv4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv4/hwdef.dat @@ -13,6 +13,12 @@ OSCILLATOR_HZ 24000000 # board voltage STM32_VDD 330U +# ChibiOS system timer +STM32_ST_USE_TIMER 5 + +# ArduPilot highres timer +HRT_TIMER 6 + # flash size FLASH_SIZE_KB 2048 @@ -50,7 +56,9 @@ PA12 OTG_FS_DP OTG1 PA13 JTMS-SWDIO SWD PA14 JTCK-SWCLK SWD -# PA15 ALARM +# PWM output for buzzer +PA15 TIM2_CH1 TIM2 GPIO(77) ALARM + PB0 INPUT PULLUP # RC Input PPM PB1 LED_GREEN OUTPUT GPIO(0) PB2 BOOT1 INPUT diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 5234ed2bb2..9999051dc3 100755 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -532,6 +532,8 @@ def write_PWM_config(f): if p.type.startswith('TIM'): if p.has_extra('RCIN'): rc_in = p + elif p.has_extra('ALARM'): + alarm = p else: if p.extra_value('PWM', type=int) is not None: pwm_out.append(p) @@ -557,6 +559,51 @@ def write_PWM_config(f): '#define RCIN_ICU_CHANNEL ICU_CHANNEL_%u\n' % int(chan_str)) f.write('#define STM32_RCIN_DMA_CHANNEL %u' % dma_chan) f.write('\n') + if alarm is not None: + n_str = alarm.label[3] + if not is_int(n_str): + error("Bad timer number for ALARM PWM %s" % p) + n = int(n_str) + # could probably also use timers 10-14 on STM32 + if (n < 1): + error("Bad timer number %u for ALARM PWM %s" % (chan, p)) + + f.write('// Alarm PWM output config\n') + f.write('#define STM32_PWM_USE_TIM%u TRUE\n' % n) + f.write('#define STM32_TIM%u_SUPPRESS_ISR\n' % n) + + chan_mode = [ + 'PWM_OUTPUT_DISABLED', 'PWM_OUTPUT_DISABLED', + 'PWM_OUTPUT_DISABLED', 'PWM_OUTPUT_DISABLED' + ] + chan_str = alarm.label[7] + if not is_int(chan_str): + error("Bad channel for ALARM PWM %s" % p) + chan = int(chan_str) + if chan not in [1, 2, 3, 4]: + error("Bad channel number %u for ALARM PWM %s" % (chan, p)) + chan_mode[chan - 1] = 'PWM_OUTPUT_ACTIVE_HIGH' + f.write('#define ALARM_CHANNEL %u\n' % (chan-1)); + + pwm_clock = 1000000 + period = 1000 + + f.write('''#define HAL_PWM_ALARM { \\ + { \\ + %u, /* PWM clock frequency. */ \\ + %u, /* Initial PWM period 20ms. */ \\ + NULL, /* no callback */ \\ + { \\ + /* Channel Config */ \\ + {%s, NULL}, \\ + {%s, NULL}, \\ + {%s, NULL}, \\ + {%s, NULL} \\ + }, 0, 0}, &PWMD%u}\n''' % + (pwm_clock, period, chan_mode[0], + chan_mode[1], chan_mode[2], chan_mode[3], n)) + f.write('\n') + f.write('// PWM timer config\n') for t in sorted(pwm_timers): n = int(t[3])