diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp index 775a0d5651..56509c7e35 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp +++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp @@ -37,7 +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(_toneAlarm_thread_wa, 512); THD_WORKING_AREA(_io_thread_wa, 2048); THD_WORKING_AREA(_storage_thread_wa, 2048); #if HAL_WITH_UAVCAN @@ -315,10 +315,10 @@ void Scheduler::_toneAlarm_thread(void *arg) Scheduler *sched = (Scheduler *)arg; sched->_toneAlarm_thread_ctx->name = "toneAlarm"; while (!sched->_hal_initialized) { - sched->delay_microseconds(10000); + sched->delay_microseconds(20000); } while (true) { - sched->delay_microseconds(10000); + sched->delay_microseconds(20000); // process tone command Util::from(hal.util)->_toneAlarm_timer_tick(); diff --git a/libraries/AP_HAL_ChibiOS/ToneAlarm.cpp b/libraries/AP_HAL_ChibiOS/ToneAlarm.cpp index 7545374384..fa38b73d4e 100644 --- a/libraries/AP_HAL_ChibiOS/ToneAlarm.cpp +++ b/libraries/AP_HAL_ChibiOS/ToneAlarm.cpp @@ -36,7 +36,7 @@ bool ToneAlarm::tune_repeat[TONE_NUMBER_OF_TUNES] = {false,true,false,false,fals ToneAlarm::ToneAlarm() { - tune_num = -1; //initialy no tune to play + tune_num = -1; //initially no tune to play tune_pos = 0; } @@ -63,40 +63,49 @@ bool ToneAlarm::is_tune_comp() void ToneAlarm::stop() { - pwmDisableChannel(pwm_group.pwm_drv, ALARM_CHANNEL); + pwmDisableChannel(pwm_group.pwm_drv, pwm_group.chan); } bool ToneAlarm::play() { uint16_t cur_time = AP_HAL::millis(); - if(tune_num != prev_tune_num){ + if(tune_num != prev_tune_num) { + stop(); tune_changed = true; - return true; + tune_pos = 0; + tune_comp = true; + return false; } - 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 + if(cur_note != 0) { + // specify alarm timer and channel in hwdef.dat pwmChangePeriod(pwm_group.pwm_drv, pwm_group.pwm_cfg.frequency/cur_note); - pwmEnableChannel(pwm_group.pwm_drv, ALARM_CHANNEL, + pwmEnableChannel(pwm_group.pwm_drv, pwm_group.chan, (pwm_group.pwm_cfg.frequency/2)/cur_note); - cur_note =0; + cur_note = 0; prev_time = cur_time; } - if((cur_time - prev_time) > duration){ + // has note duration elapsed? + if((cur_time - prev_time) > duration) { + // yes, stop the PWM signal stop(); - if(tune[tune_num][tune_pos] == '\0'){ + // was that the last note? + if(tune[tune_num][tune_pos] == '\0') { + // this was the last note + // if this is not a repeating tune, disable playback if(!tune_repeat[tune_num]){ tune_num = -1; } - + // reset tune spec index to zero: this is the only place tune_pos is reset tune_pos = 0; tune_comp = true; + // indicate tune is complete by returning false return false; } + // indicate tune is still playing by returning true return true; } return false; @@ -179,10 +188,6 @@ bool ToneAlarm::set_note() // 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{ @@ -199,6 +204,7 @@ bool ToneAlarm::init_tune() default_oct = 6; bpm = 63; prev_tune_num = tune_num; + tune_changed = false; if(tune_num <0 || tune_num > TONE_NUMBER_OF_TUNES){ return false; } @@ -252,5 +258,6 @@ bool ToneAlarm::init_tune() // 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 index 95baf782da..4fc3d33e89 100644 --- a/libraries/AP_HAL_ChibiOS/ToneAlarm.h +++ b/libraries/AP_HAL_ChibiOS/ToneAlarm.h @@ -114,33 +114,34 @@ 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(); + 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; + 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 { + pwmchannel_t chan; PWMConfig pwm_cfg; PWMDriver* pwm_drv; }; diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 4111a95198..433af91022 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -151,9 +151,13 @@ bool Util::toneAlarm_init() void Util::toneAlarm_set_tune(uint8_t tone) { _toneAlarm.set_tune(tone); - hal.console->printf("set_tune: %d\n", tone); } +// (state 0) if init_tune() -> (state 1) complete=false +// (state 1) if set_note -> (state 2) -> if play -> (state 3) +// play returns true if tune has changed or tune is complete (repeating tunes never complete) +// (state 3) -> (state 1) +// (on every tick) if (complete) -> (state 0) void Util::_toneAlarm_timer_tick() { if(state == 0) { state = state + _toneAlarm.init_tune(); diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index 642e9dac7a..4db21390fe 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -50,6 +50,8 @@ public: void toneAlarm_set_tune(uint8_t tone); void _toneAlarm_timer_tick(); + static ToneAlarm& get_ToneAlarm() { return _toneAlarm; } + private: static ToneAlarm _toneAlarm; void* try_alloc_from_ccm_ram(size_t size); diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 9999051dc3..6a68c67431 100755 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -568,7 +568,7 @@ def write_PWM_config(f): if (n < 1): error("Bad timer number %u for ALARM PWM %s" % (chan, p)) - f.write('// Alarm PWM output config\n') + f.write('\n// 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) @@ -583,25 +583,29 @@ def write_PWM_config(f): 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('''#define HAL_PWM_ALARM \\ + { /* pwmGroup */ \\ + %u, /* Timer channel */ \\ + { /* PWMConfig */ \\ + %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 /* PWMDriver* */ \\ + }\n''' % + (chan-1, 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')