From 8c58d0ce6ffd9af83a8c5231793b34351f2e12bb Mon Sep 17 00:00:00 2001 From: alex-martynov Date: Wed, 24 Jul 2019 01:58:31 +0300 Subject: [PATCH] FPV camera control FPV camera virtual joystick from Betaflight https://www.youtube.com/watch?v=gxg5Y6f4EU4 --- make/source.mk | 1 + src/main/config/parameter_group_ids.h | 2 + src/main/drivers/camera_control.c | 176 ++++++++++++++++++++++++++ src/main/drivers/camera_control.h | 57 +++++++++ src/main/drivers/resource.c | 2 +- src/main/drivers/resource.h | 1 + src/main/drivers/timer.h | 2 + src/main/fc/cli.c | 1 + src/main/fc/fc_init.c | 5 + src/main/fc/fc_msp.c | 14 ++ src/main/fc/fc_tasks.c | 24 ++++ src/main/fc/rc_controls.c | 29 +++++ src/main/fc/settings.yaml | 31 +++++ src/main/msp/msp_protocol.h | 3 +- src/main/scheduler/scheduler.h | 3 + src/main/target/OMNIBUSF4/target.h | 5 + 16 files changed, 354 insertions(+), 2 deletions(-) create mode 100644 src/main/drivers/camera_control.c create mode 100644 src/main/drivers/camera_control.h diff --git a/make/source.mk b/make/source.mk index a130f5e27e6..5e4cc0b8fd7 100644 --- a/make/source.mk +++ b/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 \ diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 7f817d45111..1226f5d9f5e 100644 --- a/src/main/config/parameter_group_ids.h +++ b/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 diff --git a/src/main/drivers/camera_control.c b/src/main/drivers/camera_control.c new file mode 100644 index 00000000000..453e21a468a --- /dev/null +++ b/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 . + */ + +#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 diff --git a/src/main/drivers/camera_control.h b/src/main/drivers/camera_control.h new file mode 100644 index 00000000000..0d5ff020cab --- /dev/null +++ b/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 . + */ + +#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); diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c index 1169f152900..86b81278bf6 100644 --- a/src/main/drivers/resource.c +++ b/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] = { diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index fbbbbdba27f..d0f5f4d0186 100644 --- a/src/main/drivers/resource.h +++ b/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; diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index f16a4eceb7e..fbc28606a2b 100644 --- a/src/main/drivers/timer.h +++ b/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; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index b031fd57350..f5c3b4a57c2 100755 --- a/src/main/fc/cli.c +++ b/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" diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 85276b2d121..0e18794618a 100644 --- a/src/main/fc/fc_init.c +++ b/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 diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index d72fab4d0aa..8660383da34 100644 --- a/src/main/fc/fc_msp.c +++ b/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(); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index a71bf2692ff..488e5dee142 100755 --- a/src/main/fc/fc_tasks.c +++ b/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", diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 6cc3e129d8b..eb729f81520 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -35,6 +35,7 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "drivers/camera_control.h" #include "drivers/time.h" #include "fc/config.h" @@ -65,6 +66,11 @@ #define MIN_RC_TICK_INTERVAL_MS 20 #define DEFAULT_RC_SWITCH_DISARM_DELAY_MS 250 // Wait at least 250ms before disarming via switch +#define repeatAfter(t) { \ + rcDelayCommand -= (t); \ +/* doNotRepeat = false;*/ \ +} + stickPositions_e rcStickPositions; FASTRAM int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW @@ -110,6 +116,8 @@ throttleStatus_e calculateThrottleStatus(void) return THROTTLE_HIGH; } +#define STICK_DELAY_MS 50 + rollPitchStatus_e calculateRollPitchCenterStatus(void) { if (((rxGetChannelValue(PITCH) < (PWM_RANGE_MIDDLE + AIRMODE_DEADBAND)) && (rxGetChannelValue(PITCH) > (PWM_RANGE_MIDDLE -AIRMODE_DEADBAND))) @@ -309,6 +317,27 @@ void processRcStickPositions(throttleStatus_e throttleStatus) rcDelayCommand = 10; return; } + +#ifdef USE_CAMERA_CONTROL + if (rcSticks == THR_CE + YAW_HI + PIT_CE + ROL_CE) { + cameraControlKeyPress(CAMERA_CONTROL_KEY_ENTER, 0); + repeatAfter(3 * STICK_DELAY_MS); + } else if (rcSticks == THR_CE + YAW_CE + PIT_CE + ROL_LO) { + cameraControlKeyPress(CAMERA_CONTROL_KEY_LEFT, 0); + repeatAfter(3 * STICK_DELAY_MS); + } else if (rcSticks == THR_CE + YAW_CE + PIT_HI + ROL_CE) { + cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 0); + repeatAfter(3 * STICK_DELAY_MS); + } else if (rcSticks == THR_CE + YAW_CE + PIT_CE + ROL_HI) { + cameraControlKeyPress(CAMERA_CONTROL_KEY_RIGHT, 0); + repeatAfter(3 * STICK_DELAY_MS); + } else if (rcSticks == THR_CE + YAW_CE + PIT_LO + ROL_CE) { + cameraControlKeyPress(CAMERA_CONTROL_KEY_DOWN, 0); + repeatAfter(3 * STICK_DELAY_MS); + } else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_CE) { + cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 2000); + } +#endif } int32_t getRcStickDeflection(int32_t axis) { diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 7d856dac02d..2d151b7c0cc 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -125,6 +125,9 @@ tables: - name: nav_extra_arming_safety values: ["OFF", "ON", "ALLOW_BYPASS"] enum: navExtraArmingSafety_e + - name: camera_control_mode + values: ["HARDWARE_PWM", "SOFTWARE_PWM", "DAC"] + enum: cameraControlMode_e groups: - name: PG_GYRO_CONFIG @@ -1691,6 +1694,34 @@ groups: - name: ledstrip_visual_beeper type: bool + - name: PG_CAMERA_CONTROL_CONFIG + type: cameraControlConfig_t + headers: ["drivers/camera_control.h"] + condition: USE_CAMERA_CONTROL + members: + - name: camera_control_mode + field: mode + type: uint8_t + table: camera_control_mode + - name: camera_control_ref_voltage + field: refVoltage + type: uint16_t + min: 200 + max: 400 + - name: camera_control_key_delay + field: keyDelayMs + type: uint16_t + min: 100 + max: 500 + - name: camera_control_internal_resistance + field: internalResistance + type: uint16_t + min: 10 + max: 1000 + - name: camera_control_inverted + field: inverted + type: uint8_t + - name: PG_OSD_CONFIG type: osdConfig_t headers: ["io/osd.h", "drivers/osd.h"] diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index d3032690183..f0634812fc5 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -231,7 +231,8 @@ #define MSP_SENSOR_CONFIG 96 #define MSP_SET_SENSOR_CONFIG 97 -#define MSP_SPECIAL_PARAMETERS 98 // Temporary betaflight parameters before cleanup and keep CF compatibility +#define MSP_CAMERA_CONTROL 98 + #define MSP_SET_SPECIAL_PARAMETERS 99 // Temporary betaflight parameters before cleanup and keep CF compatibility // diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index cf42a7ea5fe..fa6046c1415 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -104,6 +104,9 @@ typedef enum { #ifdef USE_UAV_INTERCONNECT TASK_UAV_INTERCONNECT, #endif +#ifdef USE_CAMERA_CONTROL + TASK_CAMCTRL, +#endif #ifdef USE_RCDEVICE TASK_RCDEVICE, #endif diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 47988469aba..023a455804a 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -265,6 +265,11 @@ # define WS2811_PIN PA1 #endif +#define USE_CAMERA_CONTROL +#define CAMERA_CONTROL_PIN PA8 + + + #define DEFAULT_RX_TYPE RX_TYPE_PPM #define DISABLE_RX_PWM_FEATURE #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_VBAT | FEATURE_OSD)