diff --git a/src/config b/src/config
index 535b2472cf..24b283a878 160000
--- a/src/config
+++ b/src/config
@@ -1 +1 @@
-Subproject commit 535b2472cffdc9891241a815eb9a2947b330d2c2
+Subproject commit 24b283a8784c0249bb66a994a23aee792e01015b
diff --git a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c
index 60ce63d5dc..4cbbc11b83 100644
--- a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c
+++ b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c
@@ -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
diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h
index 38198a1837..01c0612c5a 100644
--- a/src/main/drivers/pwm_output.h
+++ b/src/main/drivers/pwm_output.h
@@ -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);
diff --git a/src/main/drivers/sound_beeper.c b/src/main/drivers/sound_beeper.c
index 5d52bf0fc6..4e0d077754 100644
--- a/src/main/drivers/sound_beeper.c
+++ b/src/main/drivers/sound_beeper.c
@@ -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)
diff --git a/src/main/drivers/sound_beeper.h b/src/main/drivers/sound_beeper.h
index 33ac879d1d..0806c5d0f8 100644
--- a/src/main/drivers/sound_beeper.h
+++ b/src/main/drivers/sound_beeper.h
@@ -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);
diff --git a/src/platform/APM32/mk/APM32F4.mk b/src/platform/APM32/mk/APM32F4.mk
index 1f4eae9f0a..7affd8f0e5 100644
--- a/src/platform/APM32/mk/APM32F4.mk
+++ b/src/platform/APM32/mk/APM32F4.mk
@@ -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
diff --git a/src/platform/AT32/mk/AT32F4.mk b/src/platform/AT32/mk/AT32F4.mk
index 5dd73b15d8..e9ab808694 100644
--- a/src/platform/AT32/mk/AT32F4.mk
+++ b/src/platform/AT32/mk/AT32F4.mk
@@ -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
diff --git a/src/platform/STM32/mk/STM32_COMMON.mk b/src/platform/STM32/mk/STM32_COMMON.mk
index 6bae4a84a1..da565fd38c 100644
--- a/src/platform/STM32/mk/STM32_COMMON.mk
+++ b/src/platform/STM32/mk/STM32_COMMON.mk
@@ -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 += \
diff --git a/src/platform/common/stm32/pwm_output_beeper.c b/src/platform/common/stm32/pwm_output_beeper.c
new file mode 100644
index 0000000000..5d308ad6d4
--- /dev/null
+++ b/src/platform/common/stm32/pwm_output_beeper.c
@@ -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 .
+ */
+
+#include
+#include
+#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