diff --git a/mk/mcu/STM32F4.mk b/mk/mcu/STM32F4.mk
index e95af2e40f..b273f9f3d6 100644
--- a/mk/mcu/STM32F4.mk
+++ b/mk/mcu/STM32F4.mk
@@ -181,6 +181,7 @@ MCU_COMMON_SRC = \
drivers/stm32/timer_stm32f4xx.c \
drivers/stm32/transponder_ir_io_stdperiph.c \
drivers/stm32/usbd_msc_desc.c \
+ drivers/stm32/camera_control.c \
startup/system_stm32f4xx.c
ifeq ($(PERIPH_DRIVER), HAL)
diff --git a/mk/mcu/STM32F7.mk b/mk/mcu/STM32F7.mk
index 804303b773..dc7f7031b2 100644
--- a/mk/mcu/STM32F7.mk
+++ b/mk/mcu/STM32F7.mk
@@ -185,6 +185,7 @@ MCU_COMMON_SRC = \
drivers/stm32/timer_hal.c \
drivers/stm32/timer_stm32f7xx.c \
drivers/stm32/transponder_ir_io_hal.c \
+ drivers/stm32/camera_control.c \
startup/system_stm32f7xx.c
MCU_EXCLUDES = \
diff --git a/mk/mcu/STM32G4.mk b/mk/mcu/STM32G4.mk
index 72f5724fb6..e824504020 100644
--- a/mk/mcu/STM32G4.mk
+++ b/mk/mcu/STM32G4.mk
@@ -171,6 +171,7 @@ MCU_COMMON_SRC = \
drivers/stm32/timer_hal.c \
drivers/stm32/timer_stm32g4xx.c \
drivers/stm32/transponder_ir_io_hal.c \
+ drivers/stm32/camera_control.c \
startup/system_stm32g4xx.c
MCU_EXCLUDES = \
diff --git a/mk/mcu/STM32H7.mk b/mk/mcu/STM32H7.mk
index ac3f32d786..8ed04e2635 100644
--- a/mk/mcu/STM32H7.mk
+++ b/mk/mcu/STM32H7.mk
@@ -328,6 +328,7 @@ MCU_COMMON_SRC = \
drivers/stm32/timer_hal.c \
drivers/stm32/timer_stm32h7xx.c \
drivers/stm32/transponder_ir_io_hal.c \
+ drivers/stm32/camera_control.c \
startup/system_stm32h7xx.c
MCU_EXCLUDES = \
diff --git a/mk/source.mk b/mk/source.mk
index 0b5117d5ef..3193f786e5 100644
--- a/mk/source.mk
+++ b/mk/source.mk
@@ -73,7 +73,6 @@ COMMON_SRC = \
target/config_helper.c \
fc/init.c \
fc/controlrate_profile.c \
- drivers/camera_control.c \
drivers/accgyro/gyro_sync.c \
drivers/rx/rx_spi.c \
drivers/rx/rx_xn297.c \
diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c
index 4264c537a0..083a3ac5ab 100644
--- a/src/main/cli/cli.c
+++ b/src/main/cli/cli.c
@@ -70,7 +70,7 @@ bool cliMode = false;
#include "drivers/dshot_command.h"
#include "drivers/dshot_dpwm.h"
#include "drivers/pwm_output_dshot_shared.h"
-#include "drivers/camera_control.h"
+#include "drivers/camera_control_impl.h"
#include "drivers/compass/compass.h"
#include "drivers/display.h"
#include "drivers/dma.h"
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index 5fe42f9f6d..353241c8ac 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -39,7 +39,7 @@
#include "drivers/bus_i2c.h"
#include "drivers/bus_spi.h"
#include "drivers/dshot_command.h"
-#include "drivers/camera_control.h"
+#include "drivers/camera_control_impl.h"
#include "drivers/light_led.h"
#include "drivers/mco.h"
#include "drivers/pinio.h"
diff --git a/src/main/drivers/at32/camera_control.c b/src/main/drivers/at32/camera_control.c
new file mode 100644
index 0000000000..3930e41eaa
--- /dev/null
+++ b/src/main/drivers/at32/camera_control.c
@@ -0,0 +1,269 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 "platform.h"
+
+#ifdef USE_CAMERA_CONTROL
+
+#ifndef CAMERA_CONTROL_PIN
+#define CAMERA_CONTROL_PIN NONE
+#endif
+
+#include
+
+#include "drivers/camera_control_impl.h"
+#include "drivers/rcc.h"
+#include "drivers/io.h"
+#include "drivers/nvic.h"
+#include "drivers/pwm_output.h"
+#include "drivers/time.h"
+#include "pg/pg_ids.h"
+
+#define CAMERA_CONTROL_PWM_RESOLUTION 128
+#define CAMERA_CONTROL_SOFT_PWM_RESOLUTION 448
+
+#ifdef CURRENT_TARGET_CPU_VOLTAGE
+#define ADC_VOLTAGE CURRENT_TARGET_CPU_VOLTAGE
+#else
+#define ADC_VOLTAGE 3.3f
+#endif
+
+#if !defined(STM32F411xE) && !defined(STM32F7) && !defined(STM32H7) && !defined(STM32G4)
+#define CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
+#include "build/atomic.h"
+#endif
+
+#define CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
+#include "drivers/timer.h"
+
+#ifdef USE_OSD
+#include "osd/osd.h"
+#endif
+
+PG_REGISTER_WITH_RESET_FN(cameraControlConfig_t, cameraControlConfig, PG_CAMERA_CONTROL_CONFIG, 0);
+
+void pgResetFn_cameraControlConfig(cameraControlConfig_t *cameraControlConfig)
+{
+ cameraControlConfig->mode = CAMERA_CONTROL_MODE_HARDWARE_PWM;
+ cameraControlConfig->refVoltage = 330;
+ cameraControlConfig->keyDelayMs = 180;
+ cameraControlConfig->internalResistance = 470;
+ cameraControlConfig->ioTag = IO_TAG(CAMERA_CONTROL_PIN);
+ cameraControlConfig->inverted = 0; // Output is inverted externally
+ cameraControlConfig->buttonResistanceValues[CAMERA_CONTROL_KEY_ENTER] = 450;
+ cameraControlConfig->buttonResistanceValues[CAMERA_CONTROL_KEY_LEFT] = 270;
+ cameraControlConfig->buttonResistanceValues[CAMERA_CONTROL_KEY_UP] = 150;
+ cameraControlConfig->buttonResistanceValues[CAMERA_CONTROL_KEY_RIGHT] = 68;
+ cameraControlConfig->buttonResistanceValues[CAMERA_CONTROL_KEY_DOWN] = 0;
+}
+
+static struct {
+ bool enabled;
+ IO_t io;
+ timerChannel_t channel;
+ uint32_t period;
+ uint8_t inverted;
+} cameraControlRuntime;
+
+static uint32_t endTimeMillis;
+
+#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
+static void cameraControlHi(void)
+{
+ if (cameraControlRuntime.inverted) {
+ IOLo(cameraControlRuntime.io);
+ } else {
+ IOHi(cameraControlRuntime.io);
+ }
+}
+
+static void cameraControlLo(void)
+{
+ if (cameraControlRuntime.inverted) {
+ IOHi(cameraControlRuntime.io);
+ } else {
+ IOLo(cameraControlRuntime.io);
+ }
+}
+
+void TIM6_DAC_IRQHandler(void)
+{
+ cameraControlHi();
+
+ tmr_flag_clear(TMR6, TMR_OVF_FLAG);
+}
+
+void TIM7_IRQHandler(void)
+{
+ cameraControlLo();
+
+ tmr_flag_clear(TMR7, TMR_OVF_FLAG);
+
+}
+#endif
+
+void cameraControlInit(void)
+{
+ if (cameraControlConfig()->ioTag == IO_TAG_NONE)
+ return;
+
+ cameraControlRuntime.inverted = cameraControlConfig()->inverted;
+ cameraControlRuntime.io = IOGetByTag(cameraControlConfig()->ioTag);
+ IOInit(cameraControlRuntime.io, OWNER_CAMERA_CONTROL, 0);
+
+ if (CAMERA_CONTROL_MODE_HARDWARE_PWM == cameraControlConfig()->mode) {
+#ifdef CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
+ const timerHardware_t *timerHardware = timerAllocate(cameraControlConfig()->ioTag, OWNER_CAMERA_CONTROL, 0);
+
+ if (!timerHardware) {
+ return;
+ }
+
+ IOConfigGPIOAF(cameraControlRuntime.io, IOCFG_AF_PP, timerHardware->alternateFunction);
+
+ pwmOutConfig(&cameraControlRuntime.channel, timerHardware, timerClock(TMR6), CAMERA_CONTROL_PWM_RESOLUTION, 0, cameraControlRuntime.inverted);
+
+
+ cameraControlRuntime.period = CAMERA_CONTROL_PWM_RESOLUTION;
+ *cameraControlRuntime.channel.ccr = cameraControlRuntime.period;
+ cameraControlRuntime.enabled = true;
+#endif
+ } else if (CAMERA_CONTROL_MODE_SOFTWARE_PWM == cameraControlConfig()->mode) {
+#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
+
+ IOConfigGPIO(cameraControlRuntime.io, IOCFG_OUT_PP);
+ cameraControlHi();
+
+ cameraControlRuntime.period = CAMERA_CONTROL_SOFT_PWM_RESOLUTION;
+ cameraControlRuntime.enabled = true;
+
+ nvic_irq_enable(TMR6_DAC_GLOBAL_IRQn, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER));
+ RCC_ClockCmd(RCC_APB1(TMR6), ENABLE);
+ tmr_div_value_set(TMR6, 0);
+
+ nvic_irq_enable(TMR7_GLOBAL_IRQn, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER));
+ RCC_ClockCmd(RCC_APB1(TMR7), ENABLE);
+ tmr_div_value_set(TMR7, 0);
+
+#endif
+ } else if (CAMERA_CONTROL_MODE_DAC == cameraControlConfig()->mode) {
+ // @todo not yet implemented
+ }
+}
+
+void cameraControlProcess(uint32_t currentTimeUs)
+{
+ if (endTimeMillis && currentTimeUs >= 1000 * endTimeMillis) {
+ if (CAMERA_CONTROL_MODE_HARDWARE_PWM == cameraControlConfig()->mode) {
+ *cameraControlRuntime.channel.ccr = cameraControlRuntime.period;
+ } else if (CAMERA_CONTROL_MODE_SOFTWARE_PWM == cameraControlConfig()->mode) {
+
+ }
+
+ endTimeMillis = 0;
+ }
+}
+
+static float calculateKeyPressVoltage(const cameraControlKey_e key)
+{
+ const int buttonResistance = cameraControlConfig()->buttonResistanceValues[key] * 100;
+ return 1.0e-2f * cameraControlConfig()->refVoltage * buttonResistance / (100 * cameraControlConfig()->internalResistance + buttonResistance);
+}
+
+#if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE) || defined(CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE)
+static float calculatePWMDutyCycle(const cameraControlKey_e key)
+{
+ const float voltage = calculateKeyPressVoltage(key);
+
+ return voltage / ADC_VOLTAGE;
+}
+#endif
+
+void cameraControlKeyPress(cameraControlKey_e key, uint32_t holdDurationMs)
+{
+ if (!cameraControlRuntime.enabled)
+ return;
+
+ if (key >= CAMERA_CONTROL_KEYS_COUNT)
+ return;
+
+#if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE) || defined(CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE)
+ const float dutyCycle = calculatePWMDutyCycle(key);
+#else
+ (void) holdDurationMs;
+#endif
+
+#ifdef USE_OSD
+ // Force OSD timeout so we are alone on the display.
+ resumeRefreshAt = 0;
+#endif
+
+ if (CAMERA_CONTROL_MODE_HARDWARE_PWM == cameraControlConfig()->mode) {
+#ifdef CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
+ *cameraControlRuntime.channel.ccr = lrintf(dutyCycle * cameraControlRuntime.period);
+ endTimeMillis = millis() + cameraControlConfig()->keyDelayMs + holdDurationMs;
+#endif
+ } else if (CAMERA_CONTROL_MODE_SOFTWARE_PWM == cameraControlConfig()->mode) {
+#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
+ const uint32_t hiTime = lrintf(dutyCycle * cameraControlRuntime.period);
+
+ if (0 == hiTime) {
+ cameraControlLo();
+ delay(cameraControlConfig()->keyDelayMs + holdDurationMs);
+ cameraControlHi();
+ } else {
+ tmr_counter_value_set(TMR6, hiTime);
+ tmr_period_value_set(TMR6, cameraControlRuntime.period);
+
+ tmr_counter_value_set(TMR7, 0);
+ tmr_period_value_set(TMR7, cameraControlRuntime.period);
+
+ ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
+ tmr_counter_enable(TMR6, TRUE);
+ tmr_counter_enable(TMR7, TRUE);
+ }
+
+ tmr_interrupt_enable(TMR6, TMR_OVF_INT, TRUE);
+ tmr_interrupt_enable(TMR7, TMR_OVF_INT, TRUE);
+
+ const uint32_t endTime = millis() + cameraControlConfig()->keyDelayMs + holdDurationMs;
+
+ // Wait to give the camera a chance at registering the key press
+ while (millis() < endTime);
+
+ // Disable timers and interrupt generation
+ ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
+ tmr_counter_enable(TMR6, FALSE);
+ tmr_counter_enable(TMR7, FALSE);
+ }
+
+ tmr_interrupt_enable(TMR6, TMR_OVF_INT, FALSE);
+ tmr_interrupt_enable(TMR7, TMR_OVF_INT, FALSE);
+
+ // Reset to idle state
+ IOHi(cameraControlRuntime.io);
+ }
+#endif
+ } else if (CAMERA_CONTROL_MODE_DAC == cameraControlConfig()->mode) {
+ // @todo not yet implemented
+ }
+}
+
+#endif
diff --git a/src/main/drivers/camera_control.h b/src/main/drivers/camera_control_impl.h
similarity index 100%
rename from src/main/drivers/camera_control.h
rename to src/main/drivers/camera_control_impl.h
diff --git a/src/main/drivers/camera_control.c b/src/main/drivers/stm32/camera_control.c
similarity index 97%
rename from src/main/drivers/camera_control.c
rename to src/main/drivers/stm32/camera_control.c
index c5601e7f60..2e92243ed2 100644
--- a/src/main/drivers/camera_control.c
+++ b/src/main/drivers/stm32/camera_control.c
@@ -28,11 +28,12 @@
#include
-#include "camera_control.h"
-#include "io.h"
-#include "nvic.h"
-#include "pwm_output.h"
-#include "time.h"
+#include "drivers/camera_control_impl.h"
+#include "drivers/rcc.h"
+#include "drivers/io.h"
+#include "drivers/nvic.h"
+#include "drivers/pwm_output.h"
+#include "drivers/time.h"
#include "pg/pg_ids.h"
#define CAMERA_CONTROL_PWM_RESOLUTION 128
@@ -50,7 +51,7 @@
#endif
#define CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
-#include "timer.h"
+#include "drivers/timer.h"
#ifdef USE_OSD
#include "osd/osd.h"
@@ -105,7 +106,6 @@ static void cameraControlLo(void)
void TIM6_DAC_IRQHandler(void)
{
cameraControlHi();
-
TIM6->SR = 0;
}
@@ -151,6 +151,7 @@ void cameraControlInit(void)
cameraControlRuntime.period = CAMERA_CONTROL_SOFT_PWM_RESOLUTION;
cameraControlRuntime.enabled = true;
+
NVIC_InitTypeDef nvicTIM6 = {
TIM6_DAC_IRQn, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER), ENABLE
};
diff --git a/src/main/fc/init.c b/src/main/fc/init.c
index 88a3470abe..14e0917e7e 100644
--- a/src/main/fc/init.c
+++ b/src/main/fc/init.c
@@ -51,7 +51,7 @@
#include "drivers/bus_quadspi.h"
#include "drivers/bus_spi.h"
#include "drivers/buttons.h"
-#include "drivers/camera_control.h"
+#include "drivers/camera_control_impl.h"
#include "drivers/compass/compass.h"
#include "drivers/dma.h"
#include "drivers/exti.h"
diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c
index 842efa02cf..d4c222dec7 100644
--- a/src/main/fc/rc_controls.c
+++ b/src/main/fc/rc_controls.c
@@ -35,7 +35,7 @@
#include "config/feature.h"
-#include "drivers/camera_control.h"
+#include "drivers/camera_control_impl.h"
#include "config/config.h"
#include "fc/core.h"
diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c
index 5c24e93009..bc3b554183 100644
--- a/src/main/fc/tasks.c
+++ b/src/main/fc/tasks.c
@@ -36,7 +36,7 @@
#include "config/feature.h"
#include "drivers/accgyro/accgyro.h"
-#include "drivers/camera_control.h"
+#include "drivers/camera_control_impl.h"
#include "drivers/compass/compass.h"
#include "drivers/sensor.h"
#include "drivers/serial.h"
diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index 187f339cdf..a10d5e6cc5 100644
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@@ -53,7 +53,7 @@
#include "drivers/accgyro/accgyro.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus_spi.h"
-#include "drivers/camera_control.h"
+#include "drivers/camera_control_impl.h"
#include "drivers/compass/compass.h"
#include "drivers/display.h"
#include "drivers/dshot.h"
diff --git a/src/main/target/AT32F435G/target.h b/src/main/target/AT32F435G/target.h
index 6571e726ab..b93914d561 100644
--- a/src/main/target/AT32F435G/target.h
+++ b/src/main/target/AT32F435G/target.h
@@ -90,7 +90,6 @@
#undef USE_DSHOT_DMAR
#define USE_BEEPER
-#undef USE_CAMERA_CONTROL
#undef USE_RX_PPM
#undef USE_RX_PWM
#undef USE_RX_SPI
diff --git a/src/main/target/AT32F435M/target.h b/src/main/target/AT32F435M/target.h
index cf8c1580a1..b83922c5ad 100644
--- a/src/main/target/AT32F435M/target.h
+++ b/src/main/target/AT32F435M/target.h
@@ -90,7 +90,6 @@
#undef USE_DSHOT_DMAR
#define USE_BEEPER
-#undef USE_CAMERA_CONTROL
#undef USE_RX_PPM
#undef USE_RX_PWM
#undef USE_RX_SPI