From 214946bc3f3df312bc96aad72fe3684320ec0c15 Mon Sep 17 00:00:00 2001 From: Eike Ahmels Date: Mon, 10 Jul 2023 07:54:20 +0200 Subject: [PATCH] AT32F435 camera control (#12913) * [AT32] camera control * remove comments * change to mcu specific implementation * fix includes --- mk/mcu/STM32F4.mk | 1 + mk/mcu/STM32F7.mk | 1 + mk/mcu/STM32G4.mk | 1 + mk/mcu/STM32H7.mk | 1 + mk/source.mk | 1 - src/main/cli/cli.c | 2 +- src/main/cli/settings.c | 2 +- src/main/drivers/at32/camera_control.c | 269 ++++++++++++++++++ ...camera_control.h => camera_control_impl.h} | 0 src/main/drivers/{ => stm32}/camera_control.c | 15 +- src/main/fc/init.c | 2 +- src/main/fc/rc_controls.c | 2 +- src/main/fc/tasks.c | 2 +- src/main/msp/msp.c | 2 +- src/main/target/AT32F435G/target.h | 1 - src/main/target/AT32F435M/target.h | 1 - 16 files changed, 287 insertions(+), 16 deletions(-) create mode 100644 src/main/drivers/at32/camera_control.c rename src/main/drivers/{camera_control.h => camera_control_impl.h} (100%) rename src/main/drivers/{ => stm32}/camera_control.c (97%) diff --git a/mk/mcu/STM32F4.mk b/mk/mcu/STM32F4.mk index e95af2e40f..b273f9f3d6 100644 --- a/mk/mcu/STM32F4.mk +++ b/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) diff --git a/mk/mcu/STM32F7.mk b/mk/mcu/STM32F7.mk index 804303b773..dc7f7031b2 100644 --- a/mk/mcu/STM32F7.mk +++ b/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 = \ diff --git a/mk/mcu/STM32G4.mk b/mk/mcu/STM32G4.mk index 72f5724fb6..e824504020 100644 --- a/mk/mcu/STM32G4.mk +++ b/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 = \ diff --git a/mk/mcu/STM32H7.mk b/mk/mcu/STM32H7.mk index ac3f32d786..8ed04e2635 100644 --- a/mk/mcu/STM32H7.mk +++ b/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 = \ diff --git a/mk/source.mk b/mk/source.mk index 0b5117d5ef..3193f786e5 100644 --- a/mk/source.mk +++ b/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 \ diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 4264c537a0..083a3ac5ab 100644 --- a/src/main/cli/cli.c +++ b/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" diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 5fe42f9f6d..353241c8ac 100644 --- a/src/main/cli/settings.c +++ b/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" diff --git a/src/main/drivers/at32/camera_control.c b/src/main/drivers/at32/camera_control.c new file mode 100644 index 0000000000..3930e41eaa --- /dev/null +++ b/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 . + */ + +#include "platform.h" + +#ifdef USE_CAMERA_CONTROL + +#ifndef CAMERA_CONTROL_PIN +#define CAMERA_CONTROL_PIN NONE +#endif + +#include + +#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 diff --git a/src/main/drivers/camera_control.h b/src/main/drivers/camera_control_impl.h similarity index 100% rename from src/main/drivers/camera_control.h rename to src/main/drivers/camera_control_impl.h diff --git a/src/main/drivers/camera_control.c b/src/main/drivers/stm32/camera_control.c similarity index 97% rename from src/main/drivers/camera_control.c rename to src/main/drivers/stm32/camera_control.c index c5601e7f60..2e92243ed2 100644 --- a/src/main/drivers/camera_control.c +++ b/src/main/drivers/stm32/camera_control.c @@ -28,11 +28,12 @@ #include -#include "camera_control.h" -#include "io.h" -#include "nvic.h" -#include "pwm_output.h" -#include "time.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 @@ -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 }; diff --git a/src/main/fc/init.c b/src/main/fc/init.c index 88a3470abe..14e0917e7e 100644 --- a/src/main/fc/init.c +++ b/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" diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 842efa02cf..d4c222dec7 100644 --- a/src/main/fc/rc_controls.c +++ b/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" diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 5c24e93009..bc3b554183 100644 --- a/src/main/fc/tasks.c +++ b/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" diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 187f339cdf..a10d5e6cc5 100644 --- a/src/main/msp/msp.c +++ b/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" diff --git a/src/main/target/AT32F435G/target.h b/src/main/target/AT32F435G/target.h index 6571e726ab..b93914d561 100644 --- a/src/main/target/AT32F435G/target.h +++ b/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 diff --git a/src/main/target/AT32F435M/target.h b/src/main/target/AT32F435M/target.h index cf8c1580a1..b83922c5ad 100644 --- a/src/main/target/AT32F435M/target.h +++ b/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