From b1b6061ad4b2cfecd5a95e09e58b33c9315edf5d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 1 Aug 2018 20:16:19 +1000 Subject: [PATCH] HAL_ChibiOS: updated to new tonealarm system --- libraries/AP_HAL_ChibiOS/Scheduler.cpp | 27 --- libraries/AP_HAL_ChibiOS/Scheduler.h | 3 - libraries/AP_HAL_ChibiOS/ToneAlarm.cpp | 270 ------------------------- libraries/AP_HAL_ChibiOS/ToneAlarm.h | 157 -------------- libraries/AP_HAL_ChibiOS/Util.cpp | 39 ++-- libraries/AP_HAL_ChibiOS/Util.h | 16 +- 6 files changed, 21 insertions(+), 491 deletions(-) delete mode 100644 libraries/AP_HAL_ChibiOS/ToneAlarm.cpp delete mode 100644 libraries/AP_HAL_ChibiOS/ToneAlarm.h diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp index 47d3d0f04a..6c98a234ad 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp +++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp @@ -41,9 +41,6 @@ using namespace ChibiOS; extern const AP_HAL::HAL& hal; THD_WORKING_AREA(_timer_thread_wa, 2048); THD_WORKING_AREA(_rcin_thread_wa, 512); -#ifdef HAL_PWM_ALARM -THD_WORKING_AREA(_toneAlarm_thread_wa, 512); -#endif THD_WORKING_AREA(_io_thread_wa, 2048); THD_WORKING_AREA(_storage_thread_wa, 2048); #if HAL_WITH_UAVCAN @@ -80,14 +77,6 @@ void Scheduler::init() _rcin_thread, /* Thread function. */ this); /* Thread parameter. */ - // the toneAlarm thread runs at a medium priority -#ifdef HAL_PWM_ALARM - _toneAlarm_thread_ctx = chThdCreateStatic(_toneAlarm_thread_wa, - sizeof(_toneAlarm_thread_wa), - APM_TONEALARM_PRIORITY, /* Initial priority. */ - _toneAlarm_thread, /* Thread function. */ - this); /* Thread parameter. */ -#endif // the IO thread runs at lower priority _io_thread_ctx = chThdCreateStatic(_io_thread_wa, sizeof(_io_thread_wa), @@ -333,23 +322,7 @@ void Scheduler::_rcin_thread(void *arg) ((RCInput *)hal.rcin)->_timer_tick(); } } -#ifdef HAL_PWM_ALARM -void Scheduler::_toneAlarm_thread(void *arg) -{ - Scheduler *sched = (Scheduler *)arg; - chRegSetThreadName("toneAlarm"); - while (!sched->_hal_initialized) { - sched->delay_microseconds(20000); - } - while (true) { - sched->delay_microseconds(20000); - - // process tone command - Util::from(hal.util)->_toneAlarm_timer_tick(); - } -} -#endif 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 451e955045..440d5c634c 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.h +++ b/libraries/AP_HAL_ChibiOS/Scheduler.h @@ -25,7 +25,6 @@ #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 @@ -118,7 +117,6 @@ 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 @@ -131,7 +129,6 @@ 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 deleted file mode 100644 index 565fb54bc6..0000000000 --- a/libraries/AP_HAL_ChibiOS/ToneAlarm.cpp +++ /dev/null @@ -1,270 +0,0 @@ -#include -#include - -#ifdef HAL_PWM_ALARM - -#include "ToneAlarm.h" - -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", - "modechangeloud:d=4,o=6,b=400:8e", - "modechangesoft:d=4,o=6,b=400:8e", -}; - -//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; //initially 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, pwm_group.chan); - -} - -bool ToneAlarm::play() -{ - const uint32_t cur_time = AP_HAL::millis(); - if(tune_num != prev_tune_num) { - stop(); - tune_changed = true; - tune_pos = 0; - tune_comp = true; - return false; - } - 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, pwm_group.chan, - (pwm_group.pwm_cfg.frequency/2)/cur_note); - - cur_note = 0; - prev_time = cur_time; - } - // has note duration elapsed? - if((cur_time - prev_time) > duration) { - // yes, stop the PWM signal - stop(); - // 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; -} - -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){ - 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; - tune_changed = false; - 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; -} -#endif \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/ToneAlarm.h b/libraries/AP_HAL_ChibiOS/ToneAlarm.h deleted file mode 100644 index 05a110dedd..0000000000 --- a/libraries/AP_HAL_ChibiOS/ToneAlarm.h +++ /dev/null @@ -1,157 +0,0 @@ -#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_NOTIFY_MODE_CHANGE_LOUD 11 -#define TONE_NOTIFY_MODE_CHANGE_SOFT 12 - -#define TONE_NUMBER_OF_TUNES 13 - -#ifdef HAL_PWM_ALARM - -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 { - pwmchannel_t chan; - PWMConfig pwm_cfg; - PWMDriver* pwm_drv; - }; - static pwmGroup pwm_group; -}; - -} -#endif // HAL_PWM_ALARM diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index af3fc7a01f..80ced9910d 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -19,7 +19,6 @@ #include "Util.h" #include -#include "ToneAlarm.h" #include "RCOutput.h" #include "hwdef/common/stm32_util.h" #include "hwdef/common/flash.h" @@ -144,40 +143,26 @@ void Util::set_imu_target_temp(int8_t *target) } #ifdef HAL_PWM_ALARM -static int state; -ToneAlarm Util::_toneAlarm; +struct Util::ToneAlarmPwmGroup Util::_toneAlarm_pwm_group = HAL_PWM_ALARM; bool Util::toneAlarm_init() { - return _toneAlarm.init(); + _toneAlarm_pwm_group.pwm_cfg.period = 1000; + pwmStart(_toneAlarm_pwm_group.pwm_drv, &_toneAlarm_pwm_group.pwm_cfg); + + return true; } -void Util::toneAlarm_set_tune(uint8_t tone) +void Util::toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t duration_ms) { - _toneAlarm.set_tune(tone); -} + if (is_zero(frequency) || is_zero(volume)) { + pwmDisableChannel(_toneAlarm_pwm_group.pwm_drv, _toneAlarm_pwm_group.chan); + } else { + pwmChangePeriod(_toneAlarm_pwm_group.pwm_drv, + roundf(_toneAlarm_pwm_group.pwm_cfg.frequency/frequency)); -// (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(); - } else if (state == 1) { - state = state + _toneAlarm.set_note(); + pwmEnableChannel(_toneAlarm_pwm_group.pwm_drv, _toneAlarm_pwm_group.chan, roundf(volume*_toneAlarm_pwm_group.pwm_cfg.frequency/frequency)/2); } - if (state == 2) { - state = state + _toneAlarm.play(); - } else if (state == 3) { - state = 1; - } - - if (_toneAlarm.is_tune_comp()) { - state = 0; - } - } #endif // HAL_PWM_ALARM diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index 3f88fa35a8..50ffd30142 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -19,7 +19,6 @@ #include #include "AP_HAL_ChibiOS_Namespace.h" #include "Semaphores.h" -#include "ToneAlarm.h" class ChibiOS::Util : public AP_HAL::Util { public: @@ -48,16 +47,19 @@ public: bool get_system_id(char buf[40]) override; #ifdef HAL_PWM_ALARM - bool toneAlarm_init(); - void toneAlarm_set_tune(uint8_t tone); - void _toneAlarm_timer_tick(); - - static ToneAlarm& get_ToneAlarm() { return _toneAlarm; } + bool toneAlarm_init() override; + void toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t duration_ms) override; #endif private: #ifdef HAL_PWM_ALARM - static ToneAlarm _toneAlarm; + struct ToneAlarmPwmGroup { + pwmchannel_t chan; + PWMConfig pwm_cfg; + PWMDriver* pwm_drv; + }; + + static ToneAlarmPwmGroup _toneAlarm_pwm_group; #endif void* try_alloc_from_ccm_ram(size_t size); uint32_t available_memory_in_ccm_ram(void);