FPV camera control

make/source.mk

@@ -34,6 +34,7 @@ COMMON_SRC = \
             drivers/bus_busdev_spi.c \
             drivers/bus_i2c_soft.c \
             drivers/bus_spi.c \
+            drivers/camera_control.c \
             drivers/display.c \
             drivers/exti.c \
             drivers/io.c \

src/main/config/parameter_group_ids.h

@@ -85,6 +85,8 @@
 // cleanflight v2 specific parameter group ids start at 256
 #define PG_VTX_SETTINGS_CONFIG 259
 
+#define PG_CAMERA_CONTROL_CONFIG 522
+
 // iNav specific parameter group ids start at 1000
 #define PG_INAV_START 1000
 #define PG_PITOTMETER_CONFIG 1000

src/main/drivers/camera_control.c

@@ -0,0 +1,176 @@
+/*
+ * 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
+
+#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"
+#include "pwm_mapping.h"
+
+
+#if defined(STM32F40_41xxx)
+#define CAMERA_CONTROL_TIMER_HZ   MHZ_TO_HZ(84)
+#elif defined(STM32F7)
+#define CAMERA_CONTROL_TIMER_HZ   MHZ_TO_HZ(216)
+#else
+#define CAMERA_CONTROL_TIMER_HZ   MHZ_TO_HZ(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) && !defined(STM32H7)
+#define CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
+#include "build/atomic.h"
+#endif
+
+#define CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
+#include "timer.h"
+
+#ifdef USE_OSD
+#include "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->inverted = 0;   // Output is inverted externally
+}
+
+static struct {
+    bool enabled;
+    IO_t io;
+    volatile timCCR_t *ccr;         // Shortcut for timer CCR register
+    TCH_t *channel;
+    uint32_t period;
+    uint8_t inverted;
+} cameraControlRuntime;
+
+static uint32_t endTimeMillis;
+
+void cameraControlInit(void)
+{
+    cameraControlRuntime.inverted = cameraControlConfig()->inverted;
+    cameraControlRuntime.io = IOGetByTag(IO_TAG(CAMERA_CONTROL_PIN));
+    IOInit(cameraControlRuntime.io, OWNER_CAMERA_CONTROL, RESOURCE_OUTPUT, 0);
+
+    const timerHardware_t *timerHardware = timerGetByTag(IO_TAG(CAMERA_CONTROL_PIN), TIM_USE_ANY);
+
+    if (!timerHardware) {
+       	return;
+    }
+
+    IOConfigGPIOAF(cameraControlRuntime.io, IOCFG_AF_PP, timerHardware->alternateFunction);
+
+    cameraControlRuntime.channel = timerGetTCH(timerHardware);
+
+    timerConfigBase(cameraControlRuntime.channel, CAMERA_CONTROL_PWM_RESOLUTION, CAMERA_CONTROL_TIMER_HZ);
+    timerPWMConfigChannel(cameraControlRuntime.channel, 0);
+    timerPWMStart(cameraControlRuntime.channel);
+
+    timerEnable(cameraControlRuntime.channel);
+
+    cameraControlRuntime.ccr = timerCCR(cameraControlRuntime.channel);
+
+    cameraControlRuntime.period = CAMERA_CONTROL_PWM_RESOLUTION;
+    *cameraControlRuntime.ccr = cameraControlRuntime.period;
+    cameraControlRuntime.enabled = true;
+}
+
+void cameraControlProcess(uint32_t currentTimeUs)
+{
+    if (endTimeMillis && currentTimeUs >= 1000 * endTimeMillis) {
+        if (CAMERA_CONTROL_MODE_HARDWARE_PWM == cameraControlConfig()->mode) {
+            *cameraControlRuntime.ccr = cameraControlRuntime.period;
+        } else if (CAMERA_CONTROL_MODE_SOFTWARE_PWM == cameraControlConfig()->mode) {
+
+        }
+
+        endTimeMillis = 0;
+    }
+}
+
+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 / (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.ccr = lrintf(dutyCycle * cameraControlRuntime.period);
+        endTimeMillis = millis() + cameraControlConfig()->keyDelayMs + holdDurationMs;
+#endif
+    } else if (CAMERA_CONTROL_MODE_SOFTWARE_PWM == cameraControlConfig()->mode) {
+    } else if (CAMERA_CONTROL_MODE_DAC == cameraControlConfig()->mode) {
+        // @todo not yet implemented
+    }
+}
+
+#endif

src/main/drivers/camera_control.h

@@ -0,0 +1,57 @@
+/*
+ * 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/>.
+ */
+
+#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 {
+    uint8_t mode;
+    // measured in 10 mV steps
+    uint16_t refVoltage;
+    uint16_t keyDelayMs;
+    // measured 100 Ohm steps
+    uint16_t internalResistance;
+    uint8_t inverted;
+} cameraControlConfig_t;
+
+PG_DECLARE(cameraControlConfig_t, cameraControlConfig);
+
+void cameraControlInit(void);
+
+void cameraControlProcess(uint32_t currentTimeUs);
+void cameraControlKeyPress(cameraControlKey_e key, uint32_t holdDurationMs);

src/main/drivers/resource.c

@@ -22,7 +22,7 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
     "RANGEFINDER", "SYSTEM", "SPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD",
     "BARO", "MPU", "INVERTER", "LED STRIP", "LED", "RECEIVER", "TRANSMITTER",
     "NRF24", "VTX", "SPI_PREINIT", "COMPASS", "TEMPERATURE", "1-WIRE", "AIRSPEED", "OLED DISPLAY",
-    "PINIO"
+    "PINIO", "CAMERA_CONTROL"
 };
 
 const char * const resourceNames[RESOURCE_TOTAL_COUNT] = {

src/main/drivers/resource.h

@@ -56,6 +56,7 @@ typedef enum {
     OWNER_AIRSPEED,
     OWNER_OLED_DISPLAY,
     OWNER_PINIO,
+    OWNER_CAMERA_CONTROL,
     OWNER_TOTAL_COUNT
 } resourceOwner_e;
 

src/main/drivers/timer.h

@@ -29,6 +29,8 @@
 
 typedef uint16_t captureCompare_t;        // 16 bit on both 103 and 303, just register access must be 32bit sometimes (use timCCR_t)
 
+#define MHZ_TO_HZ(x) ((x) * 1000000)
+
 #if defined(STM32F4)
 typedef uint32_t timCCR_t;
 typedef uint32_t timCCER_t;

src/main/fc/cli.c

@@ -53,6 +53,7 @@ extern uint8_t __config_end;
 #include "drivers/pwm_mapping.h"
 #include "drivers/buf_writer.h"
 #include "drivers/bus_i2c.h"
+#include "drivers/camera_control.h"
 #include "drivers/compass/compass.h"
 #include "drivers/io.h"
 #include "drivers/io_impl.h"

src/main/fc/fc_init.c

@@ -48,6 +48,7 @@
 #include "drivers/adc.h"
 #include "drivers/compass/compass.h"
 #include "drivers/bus.h"
+#include "drivers/camera_control.h"
 #include "drivers/dma.h"
 #include "drivers/exti.h"
 #include "drivers/flash_m25p16.h"
@@ -432,6 +433,10 @@ void init(void)
     pinioInit();
 #endif
 
+#ifdef USE_CAMERA_CONTROL
+    cameraControlInit();
+#endif
+
 #ifdef USE_PINIOBOX
     pinioBoxInit();
 #endif

src/main/fc/fc_msp.c

@@ -40,6 +40,7 @@
 
 #include "drivers/accgyro/accgyro.h"
 #include "drivers/bus_i2c.h"
+#include "drivers/camera_control.h"
 #include "drivers/compass/compass.h"
 #include "drivers/display.h"
 #include "drivers/osd.h"
@@ -2356,6 +2357,19 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         }
         break;
 
+#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

@@ -29,6 +29,7 @@
 #include "common/logic_condition.h"
 
 #include "drivers/accgyro/accgyro.h"
+#include "drivers/camera_control.h"
 #include "drivers/compass/compass.h"
 #include "drivers/sensor.h"
 #include "drivers/serial.h"
@@ -261,6 +262,17 @@ void taskUpdateOsd(timeUs_t currentTimeUs)
 }
 #endif
 
+#ifdef USE_CAMERA_CONTROL
+static void taskCameraControl(timeUs_t currentTimeUs)
+{
+    if (ARMING_FLAG(ARMED)) {
+        return;
+    }
+
+    cameraControlProcess(currentTimeUs);
+}
+#endif
+
 void fcTasksInit(void)
 {
     schedulerInit();
@@ -333,6 +345,9 @@ void fcTasksInit(void)
 #ifdef USE_UAV_INTERCONNECT
     setTaskEnabled(TASK_UAV_INTERCONNECT, uavInterconnectBusIsInitialized());
 #endif
+#ifdef USE_CAMERA_CONTROL
+    setTaskEnabled(TASK_CAMCTRL, true);
+#endif
 #ifdef USE_RCDEVICE
     setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled());
 #endif
@@ -536,6 +551,15 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
+#ifdef USE_CAMERA_CONTROL
+    [TASK_CAMCTRL] = {
+	.taskName = "CAMCTRL", 
+	.taskFunc = taskCameraControl, 
+	.desiredPeriod = TASK_PERIOD_HZ(5), 
+	.staticPriority = TASK_PRIORITY_IDLE,
+    },
+#endif
+
 #if defined(USE_VTX_CONTROL)
     [TASK_VTXCTRL] = {
         .taskName = "VTXCTRL",