AT32F435 camera control (#12913)

* [AT32] camera control

* remove comments

* change to mcu specific implementation

* fix includes

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)

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 = \

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 = \

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 = \

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 \

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"

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"

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 <http://www.gnu.org/licenses/>.
+ */
+
+#include "platform.h"
+
+#ifdef USE_CAMERA_CONTROL
+
+#ifndef CAMERA_CONTROL_PIN
+#define CAMERA_CONTROL_PIN NONE
+#endif
+
+#include <math.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
+#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

src/main/drivers/stm32/camera_control.c

@@ -28,11 +28,12 @@
 
 #include <math.h>
 
-#include "camera_control.h"
+#include "drivers/camera_control_impl.h"
-#include "io.h"
+#include "drivers/rcc.h"
-#include "nvic.h"
+#include "drivers/io.h"
-#include "pwm_output.h"
+#include "drivers/nvic.h"
-#include "time.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
         };

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"

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"

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"

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"

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

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