From cc358dc4055f28c0ae309c51e90704f0684935c7 Mon Sep 17 00:00:00 2001 From: DieHertz Date: Thu, 18 May 2017 18:35:33 +0300 Subject: [PATCH 1/4] Implemented Camera Control using Hardware and Software PWM --- Makefile | 2 + src/main/config/parameter_group_ids.h | 3 +- src/main/drivers/camera_control.c | 238 ++++++++++++++++++++++ src/main/drivers/camera_control.h | 52 +++++ src/main/drivers/pwm_output.c | 36 ++-- src/main/drivers/pwm_output.h | 7 +- src/main/drivers/resource.c | 1 + src/main/drivers/resource.h | 1 + src/main/drivers/rx_pwm.c | 2 +- src/main/fc/cli.c | 4 + 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 | 1 - src/main/fc/settings.c | 19 ++ src/main/fc/settings.h | 3 + src/main/msp/msp_protocol.h | 2 + src/main/scheduler/scheduler.h | 3 + src/main/target/SITL/target.h | 1 + src/main/target/STM32F3DISCOVERY/target.h | 2 + src/main/target/common_fc_pre.h | 1 + 21 files changed, 399 insertions(+), 22 deletions(-) create mode 100644 src/main/drivers/camera_control.c create mode 100644 src/main/drivers/camera_control.h diff --git a/Makefile b/Makefile index f415d80fe..205dad6d3 100644 --- a/Makefile +++ b/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 \ diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 7180b7e58..d1bbc2184 100644 --- a/src/main/config/parameter_group_ids.h +++ b/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) diff --git a/src/main/drivers/camera_control.c b/src/main/drivers/camera_control.c new file mode 100644 index 000000000..7339e1f7c --- /dev/null +++ b/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 . + */ + +#include + +#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 diff --git a/src/main/drivers/camera_control.h b/src/main/drivers/camera_control.h new file mode 100644 index 000000000..674aa9905 --- /dev/null +++ b/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 . + */ + +#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); diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 9c05608fb..66b70901c 100644 --- a/src/main/drivers/pwm_output.c +++ b/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) { - *motors[index].ccr = 0; + if (motors[index].channel.ccr) { + *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) { - *servos[index].ccr = lrintf(value); + if (index < MAX_SUPPORTED_SERVOS && servos[index].channel.ccr) { + *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 diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 5c2b3cf7c..55a76ea3c 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -131,7 +131,11 @@ typedef void pwmCompleteWriteFunc(uint8_t motorCount); // function pointer use typedef struct { volatile timCCR_t *ccr; - TIM_TypeDef *tim; + 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); diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c index ef1c5d4bf..13953e318 100644 --- a/src/main/drivers/resource.c +++ b/src/main/drivers/resource.c @@ -65,5 +65,6 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = { "SPI_PREINIT", "RX_BIND_PLUG", "ESCSERIAL", + "CAMERA_CONTROL", }; diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index 9ed75318d..f9ed25c94 100644 --- a/src/main/drivers/resource.h +++ b/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; diff --git a/src/main/drivers/rx_pwm.c b/src/main/drivers/rx_pwm.c index bdd17a094..67376a978 100644 --- a/src/main/drivers/rx_pwm.c +++ b/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; } diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 520d26ba1..793740a13 100755 --- a/src/main/fc/cli.c +++ b/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) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 4314dc0cd..3d3897c7b 100644 --- a/src/main/fc/fc_init.c +++ b/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); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 95cefe691..87a0565cd 100644 --- a/src/main/fc/fc_msp.c +++ b/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(); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 5e1bb3b3b..9ddfc92d9 100644 --- a/src/main/fc/fc_tasks.c +++ b/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 }; diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 8344326ca..0fdef9c59 100644 --- a/src/main/fc/rc_controls.c +++ b/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) { diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index b2742f3e8..10f722684 100644 --- a/src/main/fc/settings.c +++ b/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); diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index a9f05e580..073b5fe8f 100644 --- a/src/main/fc/settings.h +++ b/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; diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 3574f99ea..ebaf1ab5b 100644 --- a/src/main/msp/msp_protocol.h +++ b/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 // diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index f943d950c..771a26ddb 100644 --- a/src/main/scheduler/scheduler.h +++ b/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, diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 68a5212b3..765c44e1a 100644 --- a/src/main/target/SITL/target.h +++ b/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 diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 24add1590..521f909f5 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/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 diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 5ce914c9e..80fc5d42d 100644 --- a/src/main/target/common_fc_pre.h +++ b/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 From 65476c506cc5c8aa9934fc5a07149a9a028d17b5 Mon Sep 17 00:00:00 2001 From: DieHertz Date: Thu, 18 May 2017 19:06:12 +0300 Subject: [PATCH 2/4] Added stick commands for camera control --- src/main/fc/rc_controls.c | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 0fdef9c59..5a7bec6ab 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -34,6 +34,8 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "drivers/camera_control.h" + #include "fc/config.h" #include "fc/fc_core.h" #include "fc/rc_controls.h" @@ -301,6 +303,22 @@ void processRcStickPositions(throttleStatus_e throttleStatus) vtxDecrementChannel(); } #endif + +#ifdef USE_CAMERA_CONTROL + if (rcSticks == THR_LO + YAW_CE + PIT_LO + ROL_CE) { + cameraControlKeyPress(CAMERA_CONTROL_KEY_ENTER, 0); + } else if (rcSticks == THR_CE + YAW_CE + PIT_CE + ROL_LO) { + cameraControlKeyPress(CAMERA_CONTROL_KEY_LEFT, 0); + } else if (rcSticks == THR_CE + YAW_CE + PIT_HI + ROL_CE) { + cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 0); + } else if (rcSticks == THR_CE + YAW_CE + PIT_CE + ROL_HI) { + cameraControlKeyPress(CAMERA_CONTROL_KEY_RIGHT, 0); + } else if (rcSticks == THR_CE + YAW_CE + PIT_LO + ROL_CE) { + cameraControlKeyPress(CAMERA_CONTROL_KEY_DOWN, 0); + } else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_CE) { + cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 2000); + } +#endif } int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) { From d1b331baea9de0487d1802a196113179c2851458 Mon Sep 17 00:00:00 2001 From: "brucesdad13@gmail.com" Date: Sun, 16 Jul 2017 23:46:48 -0400 Subject: [PATCH 3/4] * @mikeller asked if I would "[add] a warning message if targets are not built because of `SKIP_TARGETS`" to avoid confusion and frustration. I've put it at the beginning of the build process. - Charlie Stevenson --- Makefile | 1 + 1 file changed, 1 insertion(+) diff --git a/Makefile b/Makefile index 9988ce818..871b58143 100644 --- a/Makefile +++ b/Makefile @@ -1342,6 +1342,7 @@ targets-group-rest: $(GROUP_OTHER_TARGETS) $(VALID_TARGETS): + $(V0) @echo "The following targets, listed in 'SKIP_TARGETS', will not be built:\r\n\t$(SKIP_TARGETS)" $(V0) @echo "" && \ echo "Building $@" && \ time $(MAKE) binary hex TARGET=$@ && \ From 9e73acf73b7814262922f14acbee030bb95db311 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Mon, 17 Jul 2017 19:01:14 +0800 Subject: [PATCH 4/4] Inverted when the SKIP_TARGET warning is being shown. --- Makefile | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/Makefile b/Makefile index 871b58143..593867913 100644 --- a/Makefile +++ b/Makefile @@ -97,7 +97,6 @@ VALID_TARGETS = $(dir $(wildcard $(ROOT)/src/main/target/*/target.mk)) VALID_TARGETS := $(subst /,, $(subst ./src/main/target/,, $(VALID_TARGETS))) VALID_TARGETS := $(VALID_TARGETS) $(ALT_TARGETS) VALID_TARGETS := $(sort $(VALID_TARGETS)) -VALID_TARGETS := $(filter-out $(SKIP_TARGETS), $(VALID_TARGETS)) GROUP_1_TARGETS := \ AFROMINI \ @@ -1342,11 +1341,13 @@ targets-group-rest: $(GROUP_OTHER_TARGETS) $(VALID_TARGETS): - $(V0) @echo "The following targets, listed in 'SKIP_TARGETS', will not be built:\r\n\t$(SKIP_TARGETS)" - $(V0) @echo "" && \ - echo "Building $@" && \ - time $(MAKE) binary hex TARGET=$@ && \ - echo "Building $@ succeeded." + $(V0) $(if $(findstring $@,$(SKIP_TARGETS)), \ + @echo "" && \ + echo "Not building $@ since it is listed in SKIP_TARGETS.", \ + @echo "" && \ + echo "Building $@" && \ + time $(MAKE) binary hex TARGET=$@ && \ + echo "Building $@ succeeded.") CLEAN_TARGETS = $(addprefix clean_,$(VALID_TARGETS) ) TARGETS_CLEAN = $(addsuffix _clean,$(VALID_TARGETS) )