Merge pull request #5115 from jflyper/bfdev-camcontrol-drop-softpwm
CAMERA_CONTROL Drop software PWM mode
This commit is contained in:
commit
25b49a58a1
|
@ -65,6 +65,7 @@ COMMON_SRC = \
|
||||||
pg/beeper_dev.c \
|
pg/beeper_dev.c \
|
||||||
pg/bus_i2c.c \
|
pg/bus_i2c.c \
|
||||||
pg/bus_spi.c \
|
pg/bus_spi.c \
|
||||||
|
pg/camera_control.c \
|
||||||
pg/max7456.c \
|
pg/max7456.c \
|
||||||
pg/pg.c \
|
pg/pg.c \
|
||||||
pg/rx_pwm.c \
|
pg/rx_pwm.c \
|
||||||
|
|
|
@ -25,7 +25,7 @@
|
||||||
#include "nvic.h"
|
#include "nvic.h"
|
||||||
#include "pwm_output.h"
|
#include "pwm_output.h"
|
||||||
#include "time.h"
|
#include "time.h"
|
||||||
#include "pg/pg_ids.h"
|
#include "pg/camera_control.h"
|
||||||
|
|
||||||
#if defined(STM32F40_41xxx)
|
#if defined(STM32F40_41xxx)
|
||||||
#define CAMERA_CONTROL_TIMER_HZ MHZ_TO_HZ(84)
|
#define CAMERA_CONTROL_TIMER_HZ MHZ_TO_HZ(84)
|
||||||
|
@ -36,7 +36,6 @@
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#define CAMERA_CONTROL_PWM_RESOLUTION 128
|
#define CAMERA_CONTROL_PWM_RESOLUTION 128
|
||||||
#define CAMERA_CONTROL_SOFT_PWM_RESOLUTION 448
|
|
||||||
|
|
||||||
#ifdef CURRENT_TARGET_CPU_VOLTAGE
|
#ifdef CURRENT_TARGET_CPU_VOLTAGE
|
||||||
#define ADC_VOLTAGE CURRENT_TARGET_CPU_VOLTAGE
|
#define ADC_VOLTAGE CURRENT_TARGET_CPU_VOLTAGE
|
||||||
|
@ -44,32 +43,13 @@
|
||||||
#define ADC_VOLTAGE 3.3f
|
#define ADC_VOLTAGE 3.3f
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if !defined(STM32F411xE) && !defined(STM32F7)
|
|
||||||
#define CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
|
|
||||||
#include "build/atomic.h"
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#define CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
|
#define CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
|
||||||
#include "timer.h"
|
#include "timer.h"
|
||||||
|
|
||||||
#ifndef CAMERA_CONTROL_PIN
|
|
||||||
#define CAMERA_CONTROL_PIN NONE
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef USE_OSD
|
#ifdef USE_OSD
|
||||||
#include "io/osd.h"
|
#include "io/osd.h"
|
||||||
#endif
|
#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 {
|
static struct {
|
||||||
bool enabled;
|
bool enabled;
|
||||||
IO_t io;
|
IO_t io;
|
||||||
|
@ -79,22 +59,6 @@ static struct {
|
||||||
|
|
||||||
static uint32_t endTimeMillis;
|
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)
|
void cameraControlInit(void)
|
||||||
{
|
{
|
||||||
if (cameraControlConfig()->ioTag == IO_TAG_NONE)
|
if (cameraControlConfig()->ioTag == IO_TAG_NONE)
|
||||||
|
@ -122,27 +86,6 @@ void cameraControlInit(void)
|
||||||
cameraControlRuntime.period = CAMERA_CONTROL_PWM_RESOLUTION;
|
cameraControlRuntime.period = CAMERA_CONTROL_PWM_RESOLUTION;
|
||||||
*cameraControlRuntime.channel.ccr = cameraControlRuntime.period;
|
*cameraControlRuntime.channel.ccr = cameraControlRuntime.period;
|
||||||
cameraControlRuntime.enabled = true;
|
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
|
#endif
|
||||||
} else if (CAMERA_CONTROL_MODE_DAC == cameraControlConfig()->mode) {
|
} else if (CAMERA_CONTROL_MODE_DAC == cameraControlConfig()->mode) {
|
||||||
// @todo not yet implemented
|
// @todo not yet implemented
|
||||||
|
@ -154,8 +97,6 @@ void cameraControlProcess(uint32_t currentTimeUs)
|
||||||
if (endTimeMillis && currentTimeUs >= 1000 * endTimeMillis) {
|
if (endTimeMillis && currentTimeUs >= 1000 * endTimeMillis) {
|
||||||
if (CAMERA_CONTROL_MODE_HARDWARE_PWM == cameraControlConfig()->mode) {
|
if (CAMERA_CONTROL_MODE_HARDWARE_PWM == cameraControlConfig()->mode) {
|
||||||
*cameraControlRuntime.channel.ccr = cameraControlRuntime.period;
|
*cameraControlRuntime.channel.ccr = cameraControlRuntime.period;
|
||||||
} else if (CAMERA_CONTROL_MODE_SOFTWARE_PWM == cameraControlConfig()->mode) {
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
endTimeMillis = 0;
|
endTimeMillis = 0;
|
||||||
|
@ -170,7 +111,7 @@ static float calculateKeyPressVoltage(const cameraControlKey_e key)
|
||||||
return 1.0e-2f * cameraControlConfig()->refVoltage * buttonResistance / (100 * cameraControlConfig()->internalResistance + buttonResistance);
|
return 1.0e-2f * cameraControlConfig()->refVoltage * buttonResistance / (100 * cameraControlConfig()->internalResistance + buttonResistance);
|
||||||
}
|
}
|
||||||
|
|
||||||
#if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE) || defined(CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE)
|
#if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE)
|
||||||
static float calculatePWMDutyCycle(const cameraControlKey_e key)
|
static float calculatePWMDutyCycle(const cameraControlKey_e key)
|
||||||
{
|
{
|
||||||
const float voltage = calculateKeyPressVoltage(key);
|
const float voltage = calculateKeyPressVoltage(key);
|
||||||
|
@ -187,7 +128,7 @@ void cameraControlKeyPress(cameraControlKey_e key, uint32_t holdDurationMs)
|
||||||
if (key >= CAMERA_CONTROL_KEYS_COUNT)
|
if (key >= CAMERA_CONTROL_KEYS_COUNT)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
#if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE) || defined(CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE)
|
#if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE)
|
||||||
const float dutyCycle = calculatePWMDutyCycle(key);
|
const float dutyCycle = calculatePWMDutyCycle(key);
|
||||||
#else
|
#else
|
||||||
(void) holdDurationMs;
|
(void) holdDurationMs;
|
||||||
|
@ -202,46 +143,6 @@ void cameraControlKeyPress(cameraControlKey_e key, uint32_t holdDurationMs)
|
||||||
#ifdef CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
|
#ifdef CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
|
||||||
*cameraControlRuntime.channel.ccr = lrintf(dutyCycle * cameraControlRuntime.period);
|
*cameraControlRuntime.channel.ccr = lrintf(dutyCycle * cameraControlRuntime.period);
|
||||||
endTimeMillis = millis() + cameraControlConfig()->keyDelayMs + holdDurationMs;
|
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
|
#endif
|
||||||
} else if (CAMERA_CONTROL_MODE_DAC == cameraControlConfig()->mode) {
|
} else if (CAMERA_CONTROL_MODE_DAC == cameraControlConfig()->mode) {
|
||||||
// @todo not yet implemented
|
// @todo not yet implemented
|
||||||
|
|
|
@ -17,9 +17,6 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "io_types.h"
|
|
||||||
#include "pg/pg.h"
|
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
CAMERA_CONTROL_KEY_ENTER,
|
CAMERA_CONTROL_KEY_ENTER,
|
||||||
CAMERA_CONTROL_KEY_LEFT,
|
CAMERA_CONTROL_KEY_LEFT,
|
||||||
|
@ -31,25 +28,10 @@ typedef enum {
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
CAMERA_CONTROL_MODE_HARDWARE_PWM,
|
CAMERA_CONTROL_MODE_HARDWARE_PWM,
|
||||||
CAMERA_CONTROL_MODE_SOFTWARE_PWM,
|
|
||||||
CAMERA_CONTROL_MODE_DAC,
|
CAMERA_CONTROL_MODE_DAC,
|
||||||
CAMERA_CONTROL_MODES_COUNT
|
CAMERA_CONTROL_MODES_COUNT
|
||||||
} cameraControlMode_e;
|
} 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 cameraControlInit(void);
|
||||||
|
|
||||||
void cameraControlProcess(uint32_t currentTimeUs);
|
void cameraControlProcess(uint32_t currentTimeUs);
|
||||||
void cameraControlKeyPress(cameraControlKey_e key, uint32_t holdDurationMs);
|
void cameraControlKeyPress(cameraControlKey_e key, uint32_t holdDurationMs);
|
||||||
|
|
|
@ -118,6 +118,7 @@ extern uint8_t __config_end;
|
||||||
#include "pg/beeper_dev.h"
|
#include "pg/beeper_dev.h"
|
||||||
#include "pg/bus_i2c.h"
|
#include "pg/bus_i2c.h"
|
||||||
#include "pg/bus_spi.h"
|
#include "pg/bus_spi.h"
|
||||||
|
#include "pg/camera_control.h"
|
||||||
#include "pg/pg.h"
|
#include "pg/pg.h"
|
||||||
#include "pg/pg_ids.h"
|
#include "pg/pg_ids.h"
|
||||||
#include "pg/rx_pwm.h"
|
#include "pg/rx_pwm.h"
|
||||||
|
|
|
@ -67,6 +67,7 @@
|
||||||
#include "pg/max7456.h"
|
#include "pg/max7456.h"
|
||||||
#include "pg/pg.h"
|
#include "pg/pg.h"
|
||||||
#include "pg/pg_ids.h"
|
#include "pg/pg_ids.h"
|
||||||
|
#include "pg/camera_control.h"
|
||||||
#include "pg/rx_pwm.h"
|
#include "pg/rx_pwm.h"
|
||||||
#include "pg/sdcard.h"
|
#include "pg/sdcard.h"
|
||||||
#include "pg/vcd.h"
|
#include "pg/vcd.h"
|
||||||
|
@ -229,7 +230,6 @@ static const char * const lookupTableGyroLpf[] = {
|
||||||
#ifdef USE_CAMERA_CONTROL
|
#ifdef USE_CAMERA_CONTROL
|
||||||
static const char * const lookupTableCameraControlMode[] = {
|
static const char * const lookupTableCameraControlMode[] = {
|
||||||
"HARDWARE_PWM",
|
"HARDWARE_PWM",
|
||||||
"SOFTWARE_PWM",
|
|
||||||
"DAC"
|
"DAC"
|
||||||
};
|
};
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -0,0 +1,46 @@
|
||||||
|
/*
|
||||||
|
* 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 <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <platform.h>
|
||||||
|
|
||||||
|
#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
|
|
@ -0,0 +1,35 @@
|
||||||
|
/*
|
||||||
|
* 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 <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#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);
|
Loading…
Reference in New Issue