diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 1f72eebe80..a155f8bcb8 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -22,6 +22,7 @@ #include #include #include "GPIO.h" +#include "Util.h" #include "hwdef/common/stm32_util.h" #include "hwdef/common/watchdog.h" #include @@ -80,6 +81,18 @@ void RCOutput::init() uint8_t chan = group.chan[j]; if (SRV_Channels::is_GPIO(chan+chan_offset)) { group.chan[j] = CHAN_DISABLED; + } else if (SRV_Channels::is_alarm(chan+chan_offset) + || SRV_Channels::is_alarm_inverted(chan+chan_offset)) { + // alarm takes the whole timer + group.ch_mask = 0; + group.current_mode = MODE_PWM_NONE; + for (uint8_t k = 0; k < 4; k++) { + group.chan[k] = CHAN_DISABLED; + group.pwm_cfg.channels[k].mode = PWM_OUTPUT_DISABLED; + } + ChibiOS::Util::from(hal.util)->toneAlarm_init(group.pwm_cfg, group.pwm_drv, j, + SRV_Channels::is_alarm(chan+chan_offset)); + break; } #endif if (group.chan[j] != CHAN_DISABLED) { diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index a26679cb9e..0b91931d78 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -29,7 +29,7 @@ #include #include "sdcard.h" #include "shared_dma.h" -#if defined(HAL_PWM_ALARM) || HAL_DSHOT_ALARM || HAL_CANMANAGER_ENABLED +#if defined(HAL_PWM_ALARM) || HAL_DSHOT_ALARM || HAL_CANMANAGER_ENABLED || HAL_USE_PWM == TRUE #include #endif #if HAL_ENABLE_SAVE_PERSISTENT_PARAMS @@ -158,6 +158,8 @@ Util::safety_state Util::safety_switch_state(void) #ifdef HAL_PWM_ALARM struct Util::ToneAlarmPwmGroup Util::_toneAlarm_pwm_group = HAL_PWM_ALARM; +#elif HAL_USE_PWM == TRUE +struct Util::ToneAlarmPwmGroup Util::_toneAlarm_pwm_group = {}; #endif uint8_t Util::_toneAlarm_types = 0; @@ -170,7 +172,7 @@ bool Util::toneAlarm_init(uint8_t types) #endif _toneAlarm_types = types; -#if !defined(HAL_PWM_ALARM) && !HAL_DSHOT_ALARM && !HAL_CANMANAGER_ENABLED +#if HAL_USE_PWM != TRUE && !HAL_DSHOT_ALARM && !HAL_CANMANAGER_ENABLED // Nothing to do return false; #else @@ -178,18 +180,36 @@ bool Util::toneAlarm_init(uint8_t types) #endif } -void Util::toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t duration_ms) +#if HAL_USE_PWM == TRUE +bool Util::toneAlarm_init(const PWMConfig& pwm_cfg, PWMDriver* pwm_drv, pwmchannel_t chan, bool active_high) { #ifdef HAL_PWM_ALARM - 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)); + pwmStop(_toneAlarm_pwm_group.pwm_drv); +#endif + _toneAlarm_pwm_group.pwm_cfg = pwm_cfg; + _toneAlarm_pwm_group.pwm_drv = pwm_drv; + _toneAlarm_pwm_group.pwm_cfg.period = 1000; + _toneAlarm_pwm_group.pwm_cfg.channels[chan].mode = active_high ? PWM_OUTPUT_ACTIVE_HIGH : PWM_OUTPUT_ACTIVE_LOW; + _toneAlarm_pwm_group.chan = chan; + pwmStart(_toneAlarm_pwm_group.pwm_drv, &_toneAlarm_pwm_group.pwm_cfg); + return true; +} +#endif - pwmEnableChannel(_toneAlarm_pwm_group.pwm_drv, _toneAlarm_pwm_group.chan, roundf(volume*_toneAlarm_pwm_group.pwm_cfg.frequency/frequency)/2); +void Util::toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t duration_ms) +{ +#if HAL_USE_PWM == TRUE + if (_toneAlarm_pwm_group.pwm_drv != nullptr) { + 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)); + + pwmEnableChannel(_toneAlarm_pwm_group.pwm_drv, _toneAlarm_pwm_group.chan, roundf(volume*_toneAlarm_pwm_group.pwm_cfg.frequency/frequency)/2); + } } -#endif // HAL_PWM_ALARM +#endif // HAL_USE_PWM #if HAL_DSHOT_ALARM // don't play the motors while flying if (!(_toneAlarm_types & AP_Notify::Notify_Buzz_DShot) || get_soft_armed() || hal.rcout->get_dshot_esc_type() != RCOutput::DSHOT_ESC_BLHELI) { diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index 7ca209f882..d5072477e2 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -60,6 +60,9 @@ public: bool get_system_id_unformatted(uint8_t buf[], uint8_t &len) override; bool toneAlarm_init(uint8_t types) override; +#if HAL_USE_PWM == TRUE + bool toneAlarm_init(const PWMConfig& pwm_cfg, PWMDriver* pwm_drv, pwmchannel_t chan, bool active_high); +#endif void toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t duration_ms) override; static uint8_t _toneAlarm_types; @@ -104,7 +107,7 @@ public: void set_soft_armed(const bool b) override; private: -#ifdef HAL_PWM_ALARM +#if HAL_USE_PWM == TRUE struct ToneAlarmPwmGroup { pwmchannel_t chan; PWMConfig pwm_cfg; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini/hwdef.dat index f7159849bd..7748d92e31 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini/hwdef.dat @@ -38,6 +38,7 @@ PC13 BUZZER OUTPUT LOW PULLDOWN GPIO(80) define HAL_BUZZER_PIN 80 define HAL_BUZZER_ON 1 define HAL_BUZZER_OFF 0 +define HAL_PWM_ALT_ALARM # PA10 IO-debug-console PA11 OTG_FS_DM OTG1