mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-12 19:10:32 +03:00
parent
f21854cda1
commit
dd9a51a108
9 changed files with 85 additions and 46 deletions
|
@ -1 +1 @@
|
|||
Subproject commit 535b2472cffdc9891241a815eb9a2947b330d2c2
|
||||
Subproject commit 24b283a8784c0249bb66a994a23aee792e01015b
|
|
@ -61,7 +61,7 @@
|
|||
#define ICM426XX_CLKIN_FREQ 32000
|
||||
|
||||
// Soft Reset
|
||||
#define ICM426XX_RA_DEVICE_CONFIG 0x17
|
||||
#define ICM426XX_RA_DEVICE_CONFIG 0x11
|
||||
#define DEVICE_CONFIG_SOFT_RESET_BIT (1 << 0) // Soft reset bit
|
||||
|
||||
#define ICM426XX_RA_REG_BANK_SEL 0x76
|
||||
|
|
|
@ -64,8 +64,6 @@ typedef struct servoDevConfig_s {
|
|||
|
||||
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 pwmWriteServo(uint8_t index, float value);
|
||||
|
|
|
@ -34,48 +34,6 @@
|
|||
static IO_t beeperIO = DEFIO_IO(NONE);
|
||||
static bool beeperInverted = false;
|
||||
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
|
||||
|
||||
void systemBeep(bool onoff)
|
||||
|
|
|
@ -20,6 +20,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "drivers/io_types.h"
|
||||
|
||||
#ifdef USE_BEEPER
|
||||
#define BEEP_TOGGLE systemBeepToggle()
|
||||
#define BEEP_OFF systemBeep(false)
|
||||
|
@ -34,3 +36,8 @@ void systemBeep(bool on);
|
|||
void systemBeepToggle(void);
|
||||
struct beeperDevConfig_s;
|
||||
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);
|
||||
|
|
|
@ -154,6 +154,7 @@ MCU_COMMON_SRC = \
|
|||
APM32/startup/system_apm32f4xx.c \
|
||||
drivers/inverter.c \
|
||||
drivers/dshot_bitbang_decode.c \
|
||||
common/stm32/pwm_output_beeper.c \
|
||||
common/stm32/pwm_output_dshot_shared.c \
|
||||
common/stm32/dshot_dpwm.c \
|
||||
common/stm32/dshot_bitbang_shared.c \
|
||||
|
@ -221,6 +222,7 @@ SIZE_OPTIMISED_SRC += \
|
|||
drivers/bus_spi_config.c \
|
||||
common/stm32/bus_i2c_pinconfig.c \
|
||||
common/stm32/bus_spi_pinconfig.c \
|
||||
common/stm32/pwm_output_beeper.c \
|
||||
common/stm32/serial_uart_pinconfig.c \
|
||||
drivers/serial_escserial.c \
|
||||
drivers/serial_pinconfig.c
|
||||
|
|
|
@ -115,6 +115,7 @@ MCU_COMMON_SRC = \
|
|||
drivers/accgyro/accgyro_mpu.c \
|
||||
drivers/dshot_bitbang_decode.c \
|
||||
drivers/inverter.c \
|
||||
common/stm32/pwm_output_beeper.c \
|
||||
common/stm32/pwm_output_dshot_shared.c \
|
||||
common/stm32/dshot_dpwm.c \
|
||||
common/stm32/dshot_bitbang_shared.c \
|
||||
|
@ -148,6 +149,7 @@ SIZE_OPTIMISED_SRC += \
|
|||
drivers/bus_spi_config.c \
|
||||
common/stm32/bus_i2c_pinconfig.c \
|
||||
common/stm32/bus_spi_pinconfig.c \
|
||||
common/stm32/pwm_output_beeper.c \
|
||||
common/stm32/serial_uart_pinconfig.c \
|
||||
drivers/serial_escserial.c \
|
||||
drivers/serial_pinconfig.c
|
||||
|
|
|
@ -16,6 +16,7 @@ MCU_COMMON_SRC += \
|
|||
common/stm32/dshot_dpwm.c \
|
||||
STM32/pwm_output_hw.c \
|
||||
common/stm32/pwm_output_dshot_shared.c \
|
||||
common/stm32/pwm_output_beeper.c \
|
||||
common/stm32/dshot_bitbang_shared.c
|
||||
|
||||
SIZE_OPTIMISED_SRC += \
|
||||
|
@ -24,6 +25,7 @@ SIZE_OPTIMISED_SRC += \
|
|||
common/stm32/bus_i2c_pinconfig.c \
|
||||
common/stm32/config_flash.c \
|
||||
common/stm32/bus_spi_pinconfig.c \
|
||||
common/stm32/pwm_output_beeper.c \
|
||||
common/stm32/serial_uart_pinconfig.c
|
||||
|
||||
SPEED_OPTIMISED_SRC += \
|
||||
|
|
70
src/platform/common/stm32/pwm_output_beeper.c
Normal file
70
src/platform/common/stm32/pwm_output_beeper.c
Normal 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
|
Loading…
Add table
Add a link
Reference in a new issue