1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 11:29:58 +03:00

Move beeper pwm functions to platform. (#14478)

This commit is contained in:
mjs1441 2025-06-30 11:57:16 +01:00 committed by GitHub
parent c74787ea98
commit a1b8ad17d0
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
7 changed files with 83 additions and 44 deletions

View file

@ -64,8 +64,6 @@ typedef struct servoDevConfig_s {
void servoDevInit(const servoDevConfig_t *servoDevConfig); void servoDevInit(const servoDevConfig_t *servoDevConfig);
void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion); void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion);
void pwmWriteServo(uint8_t index, float value); void pwmWriteServo(uint8_t index, float value);

View file

@ -34,48 +34,6 @@
static IO_t beeperIO = DEFIO_IO(NONE); static IO_t beeperIO = DEFIO_IO(NONE);
static bool beeperInverted = false; static bool beeperInverted = false;
static uint16_t beeperFrequency = 0; static uint16_t beeperFrequency = 0;
#ifdef USE_PWM_OUTPUT
static pwmOutputPort_t beeperPwm;
static uint16_t freqBeep = 0;
static void pwmWriteBeeper(bool on)
{
if (!beeperPwm.io) {
return;
}
if (on) {
*beeperPwm.channel.ccr = (PWM_TIMER_1MHZ / freqBeep) / 2;
beeperPwm.enabled = true;
} else {
*beeperPwm.channel.ccr = 0;
beeperPwm.enabled = false;
}
}
static void pwmToggleBeeper(void)
{
pwmWriteBeeper(!beeperPwm.enabled);
}
static void beeperPwmInit(const ioTag_t tag, uint16_t frequency)
{
const timerHardware_t *timer = timerAllocate(tag, OWNER_BEEPER, 0);
IO_t beeperIO = IOGetByTag(tag);
if (beeperIO && timer) {
beeperPwm.io = beeperIO;
IOInit(beeperPwm.io, OWNER_BEEPER, 0);
IOConfigGPIOAF(beeperPwm.io, IOCFG_AF_PP, timer->alternateFunction);
freqBeep = frequency;
pwmOutConfig(&beeperPwm.channel, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / freqBeep, (PWM_TIMER_1MHZ / freqBeep) / 2, 0);
*beeperPwm.channel.ccr = 0;
beeperPwm.enabled = false;
}
}
#endif
#endif #endif
void systemBeep(bool onoff) void systemBeep(bool onoff)

View file

@ -20,6 +20,8 @@
#pragma once #pragma once
#include "drivers/io_types.h"
#ifdef USE_BEEPER #ifdef USE_BEEPER
#define BEEP_TOGGLE systemBeepToggle() #define BEEP_TOGGLE systemBeepToggle()
#define BEEP_OFF systemBeep(false) #define BEEP_OFF systemBeep(false)
@ -34,3 +36,8 @@ void systemBeep(bool on);
void systemBeepToggle(void); void systemBeepToggle(void);
struct beeperDevConfig_s; struct beeperDevConfig_s;
void beeperInit(const struct beeperDevConfig_s *beeperDevConfig); void beeperInit(const struct beeperDevConfig_s *beeperDevConfig);
// platform implementation
void pwmWriteBeeper(bool on);
void pwmToggleBeeper(void);
void beeperPwmInit(const ioTag_t tag, uint16_t frequency);

View file

@ -154,6 +154,7 @@ MCU_COMMON_SRC = \
APM32/startup/system_apm32f4xx.c \ APM32/startup/system_apm32f4xx.c \
drivers/inverter.c \ drivers/inverter.c \
drivers/dshot_bitbang_decode.c \ drivers/dshot_bitbang_decode.c \
common/stm32/pwm_output_beeper.c \
common/stm32/pwm_output_dshot_shared.c \ common/stm32/pwm_output_dshot_shared.c \
common/stm32/dshot_dpwm.c \ common/stm32/dshot_dpwm.c \
common/stm32/dshot_bitbang_shared.c \ common/stm32/dshot_bitbang_shared.c \
@ -221,6 +222,7 @@ SIZE_OPTIMISED_SRC += \
drivers/bus_spi_config.c \ drivers/bus_spi_config.c \
common/stm32/bus_i2c_pinconfig.c \ common/stm32/bus_i2c_pinconfig.c \
common/stm32/bus_spi_pinconfig.c \ common/stm32/bus_spi_pinconfig.c \
common/stm32/pwm_output_beeper.c \
common/stm32/serial_uart_pinconfig.c \ common/stm32/serial_uart_pinconfig.c \
drivers/serial_escserial.c \ drivers/serial_escserial.c \
drivers/serial_pinconfig.c drivers/serial_pinconfig.c

View file

@ -115,6 +115,7 @@ MCU_COMMON_SRC = \
drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_mpu.c \
drivers/dshot_bitbang_decode.c \ drivers/dshot_bitbang_decode.c \
drivers/inverter.c \ drivers/inverter.c \
common/stm32/pwm_output_beeper.c \
common/stm32/pwm_output_dshot_shared.c \ common/stm32/pwm_output_dshot_shared.c \
common/stm32/dshot_dpwm.c \ common/stm32/dshot_dpwm.c \
common/stm32/dshot_bitbang_shared.c \ common/stm32/dshot_bitbang_shared.c \
@ -148,6 +149,7 @@ SIZE_OPTIMISED_SRC += \
drivers/bus_spi_config.c \ drivers/bus_spi_config.c \
common/stm32/bus_i2c_pinconfig.c \ common/stm32/bus_i2c_pinconfig.c \
common/stm32/bus_spi_pinconfig.c \ common/stm32/bus_spi_pinconfig.c \
common/stm32/pwm_output_beeper.c \
common/stm32/serial_uart_pinconfig.c \ common/stm32/serial_uart_pinconfig.c \
drivers/serial_escserial.c \ drivers/serial_escserial.c \
drivers/serial_pinconfig.c drivers/serial_pinconfig.c

View file

@ -16,6 +16,7 @@ MCU_COMMON_SRC += \
common/stm32/dshot_dpwm.c \ common/stm32/dshot_dpwm.c \
STM32/pwm_output_hw.c \ STM32/pwm_output_hw.c \
common/stm32/pwm_output_dshot_shared.c \ common/stm32/pwm_output_dshot_shared.c \
common/stm32/pwm_output_beeper.c \
common/stm32/dshot_bitbang_shared.c common/stm32/dshot_bitbang_shared.c
SIZE_OPTIMISED_SRC += \ SIZE_OPTIMISED_SRC += \
@ -24,6 +25,7 @@ SIZE_OPTIMISED_SRC += \
common/stm32/bus_i2c_pinconfig.c \ common/stm32/bus_i2c_pinconfig.c \
common/stm32/config_flash.c \ common/stm32/config_flash.c \
common/stm32/bus_spi_pinconfig.c \ common/stm32/bus_spi_pinconfig.c \
common/stm32/pwm_output_beeper.c \
common/stm32/serial_uart_pinconfig.c common/stm32/serial_uart_pinconfig.c
SPEED_OPTIMISED_SRC += \ SPEED_OPTIMISED_SRC += \

View file

@ -0,0 +1,70 @@
/*
* This file is part of Betaflight.
*
* Betaflight is free software. You can redistribute this software
* and/or modify this software under the terms of the GNU General
* Public License as published by the Free Software Foundation,
* either version 3 of the License, or (at your option) any later
* version.
*
* Betaflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public
* License along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#if defined(USE_BEEPER) && defined(USE_PWM_OUTPUT)
#include "drivers/pwm_output.h"
static pwmOutputPort_t beeperPwm;
static uint16_t freqBeep = 0;
void pwmWriteBeeper(bool on)
{
if (!beeperPwm.io || freqBeep == 0) {
return;
}
if (on) {
*beeperPwm.channel.ccr = (PWM_TIMER_1MHZ / freqBeep) / 2;
beeperPwm.enabled = true;
} else {
*beeperPwm.channel.ccr = 0;
beeperPwm.enabled = false;
}
}
void pwmToggleBeeper(void)
{
pwmWriteBeeper(!beeperPwm.enabled);
}
void beeperPwmInit(const ioTag_t tag, uint16_t frequency)
{
const timerHardware_t *timer = timerAllocate(tag, OWNER_BEEPER, 0);
IO_t beeperIO = IOGetByTag(tag);
if (beeperIO && timer) {
beeperPwm.io = beeperIO;
IOInit(beeperPwm.io, OWNER_BEEPER, 0);
IOConfigGPIOAF(beeperPwm.io, IOCFG_AF_PP, timer->alternateFunction);
freqBeep = frequency;
pwmOutConfig(&beeperPwm.channel, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / freqBeep, (PWM_TIMER_1MHZ / freqBeep) / 2, 0);
*beeperPwm.channel.ccr = 0;
beeperPwm.enabled = false;
}
}
#endif