Implemented Camera Control using Hardware and Software PWM

Makefile

@@ -787,6 +787,7 @@ FC_SRC = \
             cms/cms_menu_osd.c \
             common/colorconversion.c \
             common/gps_conversion.c \
+            drivers/camera_control.c \
             drivers/display_ug2864hsweg01.c \
             drivers/light_ws2811strip.c \
             drivers/serial_escserial.c \
@@ -887,6 +888,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
             drivers/bus_i2c_config.c \
             drivers/bus_spi_config.c \
             drivers/bus_spi_pinconfig.c \
+            drivers/camera_control.c \
             drivers/serial_escserial.c \
             drivers/serial_pinconfig.c \
             drivers/serial_uart_init.c \

src/main/config/parameter_group_ids.h

@@ -110,7 +110,8 @@
 #define PG_DASHBOARD_CONFIG 519
 #define PG_SPI_PIN_CONFIG 520
 #define PG_ESCSERIAL_CONFIG 521
-#define PG_BETAFLIGHT_END 521
+#define PG_CAMERA_CONTROL_CONFIG 522
+#define PG_BETAFLIGHT_END 522
 
 
 // OSD configuration (subject to change)

src/main/drivers/camera_control.c

@@ -0,0 +1,238 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it 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 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <platform.h>
+
+#ifdef USE_CAMERA_CONTROL
+
+#include "camera_control.h"
+#include "io.h"
+#include "math.h"
+#include "nvic.h"
+#include "pwm_output.h"
+#include "time.h"
+#include "config/parameter_group_ids.h"
+
+#if defined(STM32F40_41xxx)
+#define CAMERA_CONTROL_TIMER_MHZ   84
+#elif defined(STM32F7)
+#define CAMERA_CONTROL_TIMER_MHZ   108
+#else
+#define CAMERA_CONTROL_TIMER_MHZ   72
+#endif
+
+#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)
+#define CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
+#include "build/atomic.h"
+#endif
+
+#define CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
+#include "timer.h"
+
+
+PG_REGISTER_WITH_RESET_TEMPLATE(cameraControlConfig_t, cameraControlConfig, PG_CAMERA_CONTROL_CONFIG, 0);
+
+PG_RESET_TEMPLATE(cameraControlConfig_t, cameraControlConfig,
+    .mode = CAMERA_CONTROL_MODE_HARDWARE_PWM,
+    .refVoltage = 330,
+    .keyDelayMs = 150,
+    .ioTag = IO_TAG_NONE
+);
+
+static struct {
+    bool enabled;
+    IO_t io;
+    timerChannel_t channel;
+    uint32_t period;
+} cameraControlRuntime;
+
+static uint32_t endTimeMillis;
+
+#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
+void TIM6_DAC_IRQHandler()
+{
+    IOHi(cameraControlRuntime.io);
+
+    TIM6->SR = 0;
+}
+
+void TIM7_IRQHandler()
+{
+    IOLo(cameraControlRuntime.io);
+
+    TIM7->SR = 0;
+}
+#endif
+
+void cameraControlInit()
+{
+    if (cameraControlConfig()->ioTag == IO_TAG_NONE)
+        return;
+
+    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 = timerGetByTag(cameraControlConfig()->ioTag, TIM_USE_ANY);
+
+        if (!timerHardware) {
+            return;
+        }
+
+        #ifdef USE_HAL_DRIVER
+        IOConfigGPIOAF(cameraControlRuntime.io, IOCFG_AF_PP, timerHardware->alternateFunction);
+        #else
+        IOConfigGPIO(cameraControlRuntime.io, IOCFG_AF_PP);
+        #endif
+
+        pwmOutConfig(&cameraControlRuntime.channel, timerHardware, CAMERA_CONTROL_TIMER_MHZ * 1000000, CAMERA_CONTROL_PWM_RESOLUTION, 0, 0);
+
+        *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);
+        IOHi(cameraControlRuntime.io);
+
+        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
+        };
+        NVIC_Init(&nvicTIM6);
+        NVIC_InitTypeDef nvicTIM7 = {
+            TIM7_IRQn, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER), ENABLE
+        };
+        NVIC_Init(&nvicTIM7);
+
+        RCC->APB1ENR |= RCC_APB1Periph_TIM6 | RCC_APB1Periph_TIM7;
+        TIM6->PSC = 0;
+        TIM7->PSC = 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 const int cameraPullUpResistance = 47000;
+static const int buttonResistanceValues[] = { 45000, 27000, 15000, 6810, 0 };
+
+static float calculateKeyPressVoltage(const cameraControlKey_e key)
+{
+    const int buttonResistance = buttonResistanceValues[key];
+    return 1.0e-2f * cameraControlConfig()->refVoltage * buttonResistance / (cameraPullUpResistance + 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
+
+    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) {
+            IOLo(cameraControlRuntime.io);
+            delay(cameraControlConfig()->keyDelayMs + holdDurationMs);
+            IOHi(cameraControlRuntime.io);
+        } else {
+            TIM6->CNT = hiTime;
+            TIM6->ARR = cameraControlRuntime.period;
+
+            TIM7->CNT = 0;
+            TIM7->ARR = cameraControlRuntime.period;
+
+            // Start two timers as simultaneously as possible
+            ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
+                TIM6->CR1 = TIM_CR1_CEN;
+                TIM7->CR1 = TIM_CR1_CEN;
+            }
+
+            // Enable interrupt generation
+            TIM6->DIER = TIM_IT_Update;
+            TIM7->DIER = TIM_IT_Update;
+
+            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
+            TIM6->CR1 &= ~TIM_CR1_CEN;
+            TIM7->CR1 &= ~TIM_CR1_CEN;
+            TIM6->DIER = 0;
+            TIM7->DIER = 0;
+
+            // Reset to idle state
+            IOHi(cameraControlRuntime.io);
+        }
+#endif
+    } else if (CAMERA_CONTROL_MODE_DAC == cameraControlConfig()->mode) {
+        // @todo not yet implemented
+    }
+}
+
+#endif

src/main/drivers/camera_control.h

@@ -0,0 +1,52 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it 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 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "io_types.h"
+#include "config/parameter_group.h"
+
+typedef enum {
+    CAMERA_CONTROL_KEY_ENTER,
+    CAMERA_CONTROL_KEY_LEFT,
+    CAMERA_CONTROL_KEY_UP,
+    CAMERA_CONTROL_KEY_RIGHT,
+    CAMERA_CONTROL_KEY_DOWN,
+    CAMERA_CONTROL_KEYS_COUNT
+} cameraControlKey_e;
+
+typedef enum {
+    CAMERA_CONTROL_MODE_HARDWARE_PWM,
+    CAMERA_CONTROL_MODE_SOFTWARE_PWM,
+    CAMERA_CONTROL_MODE_DAC,
+    CAMERA_CONTROL_MODES_COUNT
+} cameraControlMode_e;
+
+typedef struct cameraControlConfig_s {
+    cameraControlMode_e mode;
+    uint16_t refVoltage;
+    uint16_t keyDelayMs;
+
+    ioTag_t ioTag;
+} cameraControlConfig_t;
+
+PG_DECLARE(cameraControlConfig_t, cameraControlConfig);
+
+void cameraControlInit();
+
+void cameraControlProcess(uint32_t currentTimeUs);
+void cameraControlKeyPress(cameraControlKey_e key, uint32_t holdDurationMs);

src/main/drivers/pwm_output.c

@@ -96,7 +96,7 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8
 #endif
 }
 
-static void pwmOutConfig(pwmOutputPort_t *port, 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)
 {
 #if defined(USE_HAL_DRIVER)
     TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim);
@@ -121,11 +121,11 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard
     TIM_Cmd(timerHardware->tim, ENABLE);
 #endif
 
-    port->ccr = timerChCCR(timerHardware);
+    channel->ccr = timerChCCR(timerHardware);
 
-    port->tim = timerHardware->tim;
+    channel->tim = timerHardware->tim;
 
-    *port->ccr = 0;
+    *channel->ccr = 0;
 }
 
 static void pwmWriteUnused(uint8_t index, float value)
@@ -137,7 +137,7 @@ static void pwmWriteUnused(uint8_t index, float value)
 static void pwmWriteStandard(uint8_t index, float value)
 {
     /* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */
-    *motors[index].ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset);
+    *motors[index].channel.ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset);
 }
 
 #ifdef USE_DSHOT
@@ -176,8 +176,8 @@ void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
 {
     for (int index = 0; index < motorCount; index++) {
         // Set the compare register to 0, which stops the output pulsing if the timer overflows
-        if (motors[index].ccr) {
+        if (motors[index].channel.ccr) {
-            *motors[index].ccr = 0;
+            *motors[index].channel.ccr = 0;
         }
     }
 }
@@ -208,11 +208,11 @@ static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
 {
     for (int index = 0; index < motorCount; index++) {
         if (motors[index].forceOverflow) {
-            timerForceOverflow(motors[index].tim);
+            timerForceOverflow(motors[index].channel.tim);
         }
         // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again.
         // This compare register will be set to the output value on the next main loop.
-        *motors[index].ccr = 0;
+        *motors[index].channel.ccr = 0;
     }
 }
 
@@ -327,11 +327,11 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
         motors[motorIndex].pulseScale = ((motorConfig->motorPwmProtocol == PWM_TYPE_BRUSHED) ? period : (sLen * hz)) / 1000.0f;
         motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000);
   
-        pwmOutConfig(&motors[motorIndex], timerHardware, hz, period, idlePulse, motorConfig->motorPwmInversion);
+        pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorPwmInversion);
 
         bool timerAlreadyUsed = false;
         for (int i = 0; i < motorIndex; i++) {
-            if (motors[i].tim == motors[motorIndex].tim) {
+            if (motors[i].channel.tim == motors[motorIndex].channel.tim) {
                 timerAlreadyUsed = true;
                 break;
             }
@@ -424,8 +424,8 @@ uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, const uint16_t value)
 #ifdef USE_SERVOS
 void pwmWriteServo(uint8_t index, float value)
 {
-    if (index < MAX_SUPPORTED_SERVOS && servos[index].ccr) {
+    if (index < MAX_SUPPORTED_SERVOS && servos[index].channel.ccr) {
-        *servos[index].ccr = lrintf(value);
+        *servos[index].channel.ccr = lrintf(value);
     }
 }
 
@@ -453,7 +453,7 @@ void servoDevInit(const servoDevConfig_t *servoConfig)
             /* flag failure and disable ability to arm */
             break;
         }
-        pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
+        pwmOutConfig(&servos[servoIndex].channel, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
         servos[servoIndex].enabled = true;
     }
 }
@@ -466,10 +466,10 @@ void pwmWriteBeeper(bool onoffBeep)
         if (!beeperPwm.io)
             return;
         if (onoffBeep == true) {
-            *beeperPwm.ccr = (PWM_TIMER_1MHZ / freqBeep) / 2;
+            *beeperPwm.channel.ccr = (PWM_TIMER_1MHZ / freqBeep) / 2;
             beeperPwm.enabled = true;
         } else {
-            *beeperPwm.ccr = 0;
+            *beeperPwm.channel.ccr = 0;
             beeperPwm.enabled = false;
         }
 }
@@ -492,9 +492,9 @@ void beeperPwmInit(IO_t io, uint16_t frequency)
             IOConfigGPIO(beeperPwm.io, IOCFG_AF_PP);
 #endif
             freqBeep = frequency;
-            pwmOutConfig(&beeperPwm, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / freqBeep, (PWM_TIMER_1MHZ / freqBeep) / 2, 0);
+            pwmOutConfig(&beeperPwm.channel, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / freqBeep, (PWM_TIMER_1MHZ / freqBeep) / 2, 0);
         }
-        *beeperPwm.ccr = 0;
+        *beeperPwm.channel.ccr = 0;
         beeperPwm.enabled = false;
 }
 #endif

src/main/drivers/pwm_output.h

@@ -132,6 +132,10 @@ typedef void pwmCompleteWriteFunc(uint8_t motorCount);   // function pointer use
 typedef struct {
     volatile timCCR_t *ccr;
     TIM_TypeDef       *tim;
+} timerChannel_t;
+
+typedef struct {
+    timerChannel_t channel;
     float pulseScale;
     float pulseOffset;
     bool forceOverflow;
@@ -181,6 +185,7 @@ void pwmWriteBeeper(bool onoffBeep);
 void pwmToggleBeeper(void);
 void beeperPwmInit(IO_t io, uint16_t frequency);
 #endif
+void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion);
 
 void pwmWriteMotor(uint8_t index, float value);
 void pwmShutdownPulsesForAllMotors(uint8_t motorCount);

src/main/drivers/resource.c

@@ -65,5 +65,6 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
     "SPI_PREINIT",
     "RX_BIND_PLUG",
     "ESCSERIAL",
+    "CAMERA_CONTROL",
 };
 

src/main/drivers/resource.h

@@ -65,6 +65,7 @@ typedef enum {
     OWNER_SPI_PREINIT,
     OWNER_RX_BIND_PLUG,
     OWNER_ESCSERIAL,
+    OWNER_CAMERA_CONTROL,
     OWNER_TOTAL_COUNT
 } resourceOwner_e;
 

src/main/drivers/rx_pwm.c

@@ -412,7 +412,7 @@ void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer)
 {
     pwmOutputPort_t *motors = pwmGetMotors();
     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS; motorIndex++) {
-        if (!motors[motorIndex].enabled || motors[motorIndex].tim != pwmTimer) {
+        if (!motors[motorIndex].enabled || motors[motorIndex].channel.tim != pwmTimer) {
             continue;
         }
 

src/main/fc/cli.c

@@ -77,6 +77,7 @@ extern uint8_t __config_end;
 #include "drivers/timer.h"
 #include "drivers/vcd.h"
 #include "drivers/light_led.h"
+#include "drivers/camera_control.h"
 
 #include "fc/settings.h"
 #include "fc/cli.h"
@@ -2864,6 +2865,9 @@ const cliResourceValue_t resourceTable[] = {
 #ifdef USE_ESCSERIAL
     { OWNER_ESCSERIAL,     PG_ESCSERIAL_CONFIG, offsetof(escSerialConfig_t, ioTag), 0 },
 #endif
+#ifdef CAMERA_CONTROL
+    { OWNER_CAMERA_CONTROL, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, ioTag), 0 },
+#endif
 };
 
 static ioTag_t *getIoTag(const cliResourceValue_t value, uint8_t index)

src/main/fc/fc_init.c

@@ -68,6 +68,7 @@
 #include "drivers/max7456.h"
 #include "drivers/vtx_rtc6705.h"
 #include "drivers/vtx_common.h"
+#include "drivers/camera_control.h"
 
 #include "fc/config.h"
 #include "fc/fc_init.h"
@@ -469,6 +470,10 @@ void init(void)
     rtc6705IOInit();
 #endif
 
+#ifdef CAMERA_CONTROL
+    cameraControlInit();
+#endif
+
 #if defined(SONAR_SOFTSERIAL2_EXCLUSIVE) && defined(SONAR) && defined(USE_SOFTSERIAL2)
     if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
         serialRemovePort(SERIAL_PORT_SOFTSERIAL2);

src/main/fc/fc_msp.c

@@ -53,6 +53,7 @@
 #include "drivers/vcd.h"
 #include "drivers/vtx_common.h"
 #include "drivers/transponder_ir.h"
+#include "drivers/camera_control.h"
 
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
@@ -1846,6 +1847,19 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         break;
 #endif
 
+#ifdef USE_CAMERA_CONTROL
+    case MSP_CAMERA_CONTROL:
+        {
+            if (ARMING_FLAG(ARMED)) {
+                return MSP_RESULT_ERROR;
+            }
+
+            const uint8_t key = sbufReadU8(src);
+            cameraControlKeyPress(key, 0);
+        }
+        break;
+#endif
+
 #ifdef USE_FLASHFS
     case MSP_DATAFLASH_ERASE:
         flashfsEraseCompletely();

src/main/fc/fc_tasks.c

@@ -41,6 +41,7 @@
 #include "drivers/stack_check.h"
 #include "drivers/vtx_common.h"
 #include "drivers/transponder_ir.h"
+#include "drivers/camera_control.h"
 
 #include "fc/config.h"
 #include "fc/fc_msp.h"
@@ -256,6 +257,17 @@ void osdSlaveTasksInit(void)
 #endif
 
 #ifndef USE_OSD_SLAVE
+
+#ifdef USE_CAMERA_CONTROL
+void taskCameraControl(uint32_t currentTime)
+{
+    if (ARMING_FLAG(ARMED))
+        return;
+
+    cameraControlProcess(currentTime);
+}
+#endif
+
 void fcTasksInit(void)
 {
     schedulerInit();
@@ -356,6 +368,9 @@ void fcTasksInit(void)
     setTaskEnabled(TASK_VTXCTRL, true);
 #endif
 #endif
+#ifdef USE_CAMERA_CONTROL
+    setTaskEnabled(TASK_CAMCTRL, true);
+#endif
 }
 #endif
 
@@ -607,5 +622,14 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .staticPriority = TASK_PRIORITY_MEDIUM,
     },
 #endif
+
+#ifdef USE_CAMERA_CONTROL
+    [TASK_CAMCTRL] = {
+        .taskName = "CAMCTRL",
+        .taskFunc = taskCameraControl,
+        .desiredPeriod = TASK_PERIOD_HZ(5),
+        .staticPriority = TASK_PRIORITY_IDLE
+    },
+#endif
 #endif
 };

src/main/fc/rc_controls.c

@@ -301,7 +301,6 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
         vtxDecrementChannel();
     }
 #endif
-
 }
 
 int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {

src/main/fc/settings.c

@@ -37,6 +37,7 @@
 #include "config/parameter_group_ids.h"
 
 #include "drivers/light_led.h"
+#include "drivers/camera_control.h"
 
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
@@ -228,6 +229,14 @@ static const char * const lookupTableOsdType[] = {
 };
 #endif
 
+#ifdef USE_CAMERA_CONTROL
+static const char * const lookupTableCameraControlMode[] = {
+    "HARDWARE_PWM",
+    "SOFTWARE_PWM",
+    "DAC"
+};
+#endif
+
 static const char * const lookupTableSuperExpoYaw[] = {
     "OFF", "ON", "ALWAYS"
 };
@@ -297,6 +306,9 @@ const lookupTableEntry_t lookupTables[] = {
 #ifdef OSD
     { lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) },
 #endif
+#ifdef USE_CAMERA_CONTROL
+    { lookupTableCameraControlMode, sizeof(lookupTableCameraControlMode) / sizeof(char *) },
+#endif
 };
 
 const clivalue_t valueTable[] = {
@@ -736,6 +748,13 @@ const clivalue_t valueTable[] = {
     { "dashboard_i2c_bus",           VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, I2CDEV_COUNT }, PG_DASHBOARD_CONFIG, offsetof(dashboardConfig_t, device) },
     { "dashboard_i2c_addr",          VAR_UINT8  | MASTER_VALUE, .config.minmax = { I2C_ADDR8_MIN, I2C_ADDR8_MAX }, PG_DASHBOARD_CONFIG, offsetof(dashboardConfig_t, address) },
 #endif
+
+// PG_CAMERA_CONTROL_CONFIG
+#ifdef USE_CAMERA_CONTROL
+    { "camera_control_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CAMERA_CONTROL_MODE }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, mode) },
+    { "camera_control_ref_voltage", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 200, 400 }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, refVoltage) },
+    { "camera_control_key_delay", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 100, 500 }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, keyDelayMs) },
+#endif
 };
 
 const uint16_t valueTableEntryCount = ARRAYLEN(valueTable);

src/main/fc/settings.h

@@ -59,6 +59,9 @@ typedef enum {
     TABLE_CRASH_RECOVERY,
 #ifdef OSD
     TABLE_OSD,
+#endif
+#ifdef USE_CAMERA_CONTROL
+    TABLE_CAMERA_CONTROL_MODE,
 #endif
     LOOKUP_TABLE_COUNT
 } lookupTableIndex_e;

src/main/msp/msp_protocol.h

@@ -212,6 +212,8 @@
 #define MSP_SENSOR_CONFIG               96
 #define MSP_SET_SENSOR_CONFIG           97
 
+#define MSP_CAMERA_CONTROL              98
+
 //
 // OSD specific
 //

src/main/scheduler/scheduler.h

@@ -110,6 +110,9 @@ typedef enum {
 #ifdef VTX_CONTROL
     TASK_VTXCTRL,
 #endif
+#ifdef USE_CAMERA_CONTROL
+    TASK_CAMCTRL,
+#endif
 
 #ifdef USE_RCSPLIT
     TASK_RCSPLIT,

src/main/target/SITL/target.h

@@ -111,6 +111,7 @@
 #undef VTX_CONTROL
 #undef VTX_SMARTAUDIO
 #undef VTX_TRAMP
+#undef USE_CAMERA_CONTROL
 
 #undef USE_I2C
 #undef USE_SPI

src/main/target/STM32F3DISCOVERY/target.h

@@ -31,6 +31,8 @@
 
 #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE
 
+#define CURRENT_TARGET_CPU_VOLTAGE 3.0
+
 #define LED0_PIN                PE8  // Blue LEDs - PE8/PE12
 #define LED0_INVERTED
 #define LED1_PIN                PE10 // Orange LEDs - PE10/PE14

src/main/target/common_fc_pre.h

@@ -124,6 +124,7 @@
 #define VTX_CONTROL
 #define VTX_SMARTAUDIO
 #define VTX_TRAMP
+#define USE_CAMERA_CONTROL
 
 #ifdef USE_SERIALRX_SPEKTRUM
 #define USE_SPEKTRUM_BIND