diff --git a/make/source.mk b/make/source.mk index 649c35742b..9bb812bca1 100644 --- a/make/source.mk +++ b/make/source.mk @@ -65,7 +65,6 @@ COMMON_SRC = \ pg/beeper_dev.c \ pg/bus_i2c.c \ pg/bus_spi.c \ - pg/camera_control.c \ pg/dashboard.c \ pg/max7456.c \ pg/pg.c \ diff --git a/src/main/drivers/camera_control.c b/src/main/drivers/camera_control.c index 5307ecb912..7c6ec70388 100644 --- a/src/main/drivers/camera_control.c +++ b/src/main/drivers/camera_control.c @@ -25,7 +25,7 @@ #include "nvic.h" #include "pwm_output.h" #include "time.h" -#include "pg/camera_control.h" +#include "pg/pg_ids.h" #if defined(STM32F40_41xxx) #define CAMERA_CONTROL_TIMER_HZ MHZ_TO_HZ(84) @@ -36,6 +36,7 @@ #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 @@ -43,13 +44,32 @@ #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" +#ifndef CAMERA_CONTROL_PIN +#define CAMERA_CONTROL_PIN NONE +#endif + #ifdef USE_OSD #include "io/osd.h" #endif +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 = 180, + .internalResistance = 470, + .ioTag = IO_TAG(CAMERA_CONTROL_PIN) +); + static struct { bool enabled; IO_t io; @@ -59,6 +79,22 @@ static struct { static uint32_t endTimeMillis; +#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE +void TIM6_DAC_IRQHandler(void) +{ + IOHi(cameraControlRuntime.io); + + TIM6->SR = 0; +} + +void TIM7_IRQHandler(void) +{ + IOLo(cameraControlRuntime.io); + + TIM7->SR = 0; +} +#endif + void cameraControlInit(void) { if (cameraControlConfig()->ioTag == IO_TAG_NONE) @@ -86,6 +122,27 @@ void cameraControlInit(void) 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); + 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 @@ -97,6 +154,8 @@ 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; @@ -111,7 +170,7 @@ static float calculateKeyPressVoltage(const cameraControlKey_e key) return 1.0e-2f * cameraControlConfig()->refVoltage * buttonResistance / (100 * cameraControlConfig()->internalResistance + buttonResistance); } -#if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE) +#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); @@ -128,7 +187,7 @@ void cameraControlKeyPress(cameraControlKey_e key, uint32_t holdDurationMs) if (key >= CAMERA_CONTROL_KEYS_COUNT) return; -#if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE) +#if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE) || defined(CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE) const float dutyCycle = calculatePWMDutyCycle(key); #else (void) holdDurationMs; @@ -143,6 +202,46 @@ void cameraControlKeyPress(cameraControlKey_e key, uint32_t holdDurationMs) #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 diff --git a/src/main/drivers/camera_control.h b/src/main/drivers/camera_control.h index e4c1420580..b825900b8a 100644 --- a/src/main/drivers/camera_control.h +++ b/src/main/drivers/camera_control.h @@ -17,6 +17,9 @@ #pragma once +#include "io_types.h" +#include "pg/pg.h" + typedef enum { CAMERA_CONTROL_KEY_ENTER, CAMERA_CONTROL_KEY_LEFT, @@ -28,10 +31,25 @@ typedef enum { 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; + // measured in 10 mV steps + uint16_t refVoltage; + uint16_t keyDelayMs; + // measured 100 Ohm steps + uint16_t internalResistance; + + ioTag_t ioTag; +} 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/interface/cli.c b/src/main/interface/cli.c index 033aff5ddb..4b55e60121 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -118,7 +118,6 @@ extern uint8_t __config_end; #include "pg/beeper_dev.h" #include "pg/bus_i2c.h" #include "pg/bus_spi.h" -#include "pg/camera_control.h" #include "pg/pg.h" #include "pg/pg_ids.h" #include "pg/rx_pwm.h" diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 605712ebe4..9c61d17f1b 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -68,7 +68,6 @@ #include "pg/max7456.h" #include "pg/pg.h" #include "pg/pg_ids.h" -#include "pg/camera_control.h" #include "pg/rx_pwm.h" #include "pg/sdcard.h" #include "pg/vcd.h" @@ -231,6 +230,7 @@ static const char * const lookupTableGyroLpf[] = { #ifdef USE_CAMERA_CONTROL static const char * const lookupTableCameraControlMode[] = { "HARDWARE_PWM", + "SOFTWARE_PWM", "DAC" }; #endif diff --git a/src/main/pg/camera_control.c b/src/main/pg/camera_control.c deleted file mode 100644 index 0241f653e6..0000000000 --- a/src/main/pg/camera_control.c +++ /dev/null @@ -1,46 +0,0 @@ -/* - * 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 "pg/pg_ids.h" -#include "pg/camera_control.h" -#include "drivers/camera_control.h" -#include "drivers/io.h" - -//#include "math.h" -//#include "nvic.h" -//#include "pwm_output.h" -//#include "time.h" - -#ifndef CAMERA_CONTROL_PIN -#define CAMERA_CONTROL_PIN NONE -#endif - -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 = 180, - .internalResistance = 470, - .ioTag = IO_TAG(CAMERA_CONTROL_PIN) -); - -#endif diff --git a/src/main/pg/camera_control.h b/src/main/pg/camera_control.h deleted file mode 100644 index 2bbd9bb436..0000000000 --- a/src/main/pg/camera_control.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - * 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 "drivers/io_types.h" -#include "drivers/camera_control.h" -#include "pg/pg.h" - -typedef struct cameraControlConfig_s { - cameraControlMode_e mode; - // measured in 10 mV steps - uint16_t refVoltage; - uint16_t keyDelayMs; - // measured 100 Ohm steps - uint16_t internalResistance; - - ioTag_t ioTag; -} cameraControlConfig_t; - -PG_DECLARE(cameraControlConfig_t, cameraControlConfig);