mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 11:29:58 +03:00
Code re-organisation: src/platform/xxx for the MCU type (#13955)
This commit is contained in:
parent
7158ff7081
commit
b21cfe3282
311 changed files with 645 additions and 917 deletions
30
Makefile
30
Makefile
|
@ -56,14 +56,13 @@ FORKNAME = betaflight
|
||||||
|
|
||||||
# Working directories
|
# Working directories
|
||||||
ROOT := $(patsubst %/,%,$(dir $(lastword $(MAKEFILE_LIST))))
|
ROOT := $(patsubst %/,%,$(dir $(lastword $(MAKEFILE_LIST))))
|
||||||
|
PLATFORM_DIR := $(ROOT)/src/platform
|
||||||
SRC_DIR := $(ROOT)/src/main
|
SRC_DIR := $(ROOT)/src/main
|
||||||
OBJECT_DIR := $(ROOT)/obj/main
|
OBJECT_DIR := $(ROOT)/obj/main
|
||||||
BIN_DIR := $(ROOT)/obj
|
BIN_DIR := $(ROOT)/obj
|
||||||
CMSIS_DIR := $(ROOT)/lib/main/CMSIS
|
CMSIS_DIR := $(ROOT)/lib/main/CMSIS
|
||||||
INCLUDE_DIRS := $(SRC_DIR) \
|
INCLUDE_DIRS := $(SRC_DIR)
|
||||||
$(ROOT)/src/main/target
|
|
||||||
|
|
||||||
LINKER_DIR := $(ROOT)/src/link
|
|
||||||
MAKE_SCRIPT_DIR := $(ROOT)/mk
|
MAKE_SCRIPT_DIR := $(ROOT)/mk
|
||||||
|
|
||||||
## V : Set verbosity level based on the V= parameter
|
## V : Set verbosity level based on the V= parameter
|
||||||
|
@ -89,7 +88,8 @@ MAKE_PARALLEL = $(if $(filter -j%, $(MAKEFLAGS)),$(EMPTY),-j$(DEFAULT_PARALLEL
|
||||||
include $(MAKE_SCRIPT_DIR)/checks.mk
|
include $(MAKE_SCRIPT_DIR)/checks.mk
|
||||||
|
|
||||||
# basic target list
|
# basic target list
|
||||||
BASE_TARGETS := $(sort $(notdir $(patsubst %/,%,$(dir $(wildcard $(ROOT)/src/main/target/*/target.mk)))))
|
PLATFORMS := $(sort $(notdir $(patsubst /%,%, $(wildcard $(PLATFORM_DIR)/*))))
|
||||||
|
BASE_TARGETS := $(sort $(notdir $(patsubst %/,%,$(dir $(wildcard $(PLATFORM_DIR)/*/target/*/target.mk)))))
|
||||||
|
|
||||||
# configure some directories that are relative to wherever ROOT_DIR is located
|
# configure some directories that are relative to wherever ROOT_DIR is located
|
||||||
TOOLS_DIR ?= $(ROOT)/tools
|
TOOLS_DIR ?= $(ROOT)/tools
|
||||||
|
@ -109,7 +109,7 @@ include $(MAKE_SCRIPT_DIR)/$(OSFAMILY).mk
|
||||||
include $(MAKE_SCRIPT_DIR)/tools.mk
|
include $(MAKE_SCRIPT_DIR)/tools.mk
|
||||||
|
|
||||||
# Search path for sources
|
# Search path for sources
|
||||||
VPATH := $(SRC_DIR):$(SRC_DIR)/startup
|
VPATH := $(SRC_DIR)
|
||||||
FATFS_DIR = $(ROOT)/lib/main/FatFS
|
FATFS_DIR = $(ROOT)/lib/main/FatFS
|
||||||
FATFS_SRC = $(notdir $(wildcard $(FATFS_DIR)/*.c))
|
FATFS_SRC = $(notdir $(wildcard $(FATFS_DIR)/*.c))
|
||||||
CSOURCES := $(shell find $(SRC_DIR) -name '*.c')
|
CSOURCES := $(shell find $(SRC_DIR) -name '*.c')
|
||||||
|
@ -132,9 +132,15 @@ endif
|
||||||
# default xtal value
|
# default xtal value
|
||||||
HSE_VALUE ?= 8000000
|
HSE_VALUE ?= 8000000
|
||||||
|
|
||||||
CI_EXCLUDED_TARGETS := $(sort $(notdir $(patsubst %/,%,$(dir $(wildcard $(ROOT)/src/main/target/*/.exclude)))))
|
CI_EXCLUDED_TARGETS := $(sort $(notdir $(patsubst %/,%,$(dir $(wildcard $(PLATFORM_DIR)/*/target/*/.exclude)))))
|
||||||
CI_TARGETS := $(filter-out $(CI_EXCLUDED_TARGETS), $(BASE_TARGETS)) $(filter CRAZYBEEF4SX1280 CRAZYBEEF4FR STM32F4DISCOVERY IFLIGHT_BLITZ_F722 NUCLEOF446 SPRACINGH7EXTREME SPRACINGH7RF, $(BASE_CONFIGS))
|
CI_TARGETS := $(filter-out $(CI_EXCLUDED_TARGETS), $(BASE_TARGETS)) $(filter STM32F4DISCOVERY CRAZYBEEF4SX1280 CRAZYBEEF4FR IFLIGHT_BLITZ_F722 NUCLEOF446 SPRACINGH7EXTREME SPRACINGH7RF, $(BASE_CONFIGS))
|
||||||
include $(ROOT)/src/main/target/$(TARGET)/target.mk
|
|
||||||
|
TARGET_PLATFORM := $(notdir $(patsubst %/,%,$(subst target/$(TARGET)/,, $(dir $(wildcard $(PLATFORM_DIR)/*/target/$(TARGET)/target.mk)))))
|
||||||
|
TARGET_PLATFORM_DIR := $(PLATFORM_DIR)/$(TARGET_PLATFORM)
|
||||||
|
LINKER_DIR := $(TARGET_PLATFORM_DIR)/link
|
||||||
|
VPATH := $(VPATH):$(TARGET_PLATFORM_DIR):$(TARGET_PLATFORM_DIR)/startup
|
||||||
|
|
||||||
|
include $(TARGET_PLATFORM_DIR)/target/$(TARGET)/target.mk
|
||||||
|
|
||||||
REVISION := norevision
|
REVISION := norevision
|
||||||
ifeq ($(shell git diff --shortstat),)
|
ifeq ($(shell git diff --shortstat),)
|
||||||
|
@ -173,7 +179,6 @@ OPTIMISE_SIZE := -Os
|
||||||
LTO_FLAGS := $(OPTIMISATION_BASE) $(OPTIMISE_SPEED)
|
LTO_FLAGS := $(OPTIMISATION_BASE) $(OPTIMISE_SPEED)
|
||||||
endif
|
endif
|
||||||
|
|
||||||
VPATH := $(VPATH):$(MAKE_SCRIPT_DIR)/mcu
|
|
||||||
VPATH := $(VPATH):$(MAKE_SCRIPT_DIR)
|
VPATH := $(VPATH):$(MAKE_SCRIPT_DIR)
|
||||||
|
|
||||||
# start specific includes
|
# start specific includes
|
||||||
|
@ -185,13 +190,13 @@ ifeq ($(TARGET_MCU_FAMILY),)
|
||||||
$(error No TARGET_MCU_FAMILY specified. Is the target.mk valid for $(TARGET)?)
|
$(error No TARGET_MCU_FAMILY specified. Is the target.mk valid for $(TARGET)?)
|
||||||
endif
|
endif
|
||||||
|
|
||||||
TARGET_FLAGS := -D$(TARGET) -D$(TARGET_MCU_FAMILY) $(TARGET_FLAGS)
|
TARGET_FLAGS := -D$(TARGET) -D$(TARGET_PLATFORM) -D$(TARGET_MCU_FAMILY) $(TARGET_FLAGS)
|
||||||
|
|
||||||
ifneq ($(CONFIG),)
|
ifneq ($(CONFIG),)
|
||||||
TARGET_FLAGS := $(TARGET_FLAGS) -DUSE_CONFIG
|
TARGET_FLAGS := $(TARGET_FLAGS) -DUSE_CONFIG
|
||||||
endif
|
endif
|
||||||
|
|
||||||
include $(MAKE_SCRIPT_DIR)/mcu/$(TARGET_MCU_FAMILY).mk
|
include $(TARGET_PLATFORM_DIR)/mk/$(TARGET_MCU_FAMILY).mk
|
||||||
|
|
||||||
# openocd specific includes
|
# openocd specific includes
|
||||||
include $(MAKE_SCRIPT_DIR)/openocd.mk
|
include $(MAKE_SCRIPT_DIR)/openocd.mk
|
||||||
|
@ -211,8 +216,7 @@ ifneq ($(HSE_VALUE),)
|
||||||
DEVICE_FLAGS := $(DEVICE_FLAGS) -DHSE_VALUE=$(HSE_VALUE)
|
DEVICE_FLAGS := $(DEVICE_FLAGS) -DHSE_VALUE=$(HSE_VALUE)
|
||||||
endif
|
endif
|
||||||
|
|
||||||
TARGET_DIR = $(ROOT)/src/main/target/$(TARGET)
|
TARGET_DIR = $(TARGET_PLATFORM_DIR)/target/$(TARGET)
|
||||||
TARGET_DIR_SRC = $(notdir $(wildcard $(TARGET_DIR)/*.c))
|
|
||||||
|
|
||||||
.DEFAULT_GOAL := hex
|
.DEFAULT_GOAL := hex
|
||||||
|
|
||||||
|
|
|
@ -48,7 +48,6 @@ COMMON_SRC = \
|
||||||
build/debug.c \
|
build/debug.c \
|
||||||
build/debug_pin.c \
|
build/debug_pin.c \
|
||||||
build/version.c \
|
build/version.c \
|
||||||
$(TARGET_DIR_SRC) \
|
|
||||||
main.c \
|
main.c \
|
||||||
$(PG_SRC) \
|
$(PG_SRC) \
|
||||||
common/bitarray.c \
|
common/bitarray.c \
|
||||||
|
@ -98,6 +97,7 @@ COMMON_SRC = \
|
||||||
drivers/bus_spi_config.c \
|
drivers/bus_spi_config.c \
|
||||||
drivers/bus_spi_pinconfig.c \
|
drivers/bus_spi_pinconfig.c \
|
||||||
drivers/buttons.c \
|
drivers/buttons.c \
|
||||||
|
drivers/camera_control.c \
|
||||||
drivers/display.c \
|
drivers/display.c \
|
||||||
drivers/display_canvas.c \
|
drivers/display_canvas.c \
|
||||||
drivers/dma_common.c \
|
drivers/dma_common.c \
|
||||||
|
|
|
@ -1,19 +1,20 @@
|
||||||
/*
|
/*
|
||||||
* This file is part of Cleanflight and Betaflight.
|
* This file is part of Betaflight.
|
||||||
*
|
*
|
||||||
* Cleanflight and Betaflight are free software. You can redistribute
|
* Betaflight is free software. You can redistribute this software
|
||||||
* this software and/or modify this software under the terms of the
|
* and/or modify this software under the terms of the GNU General
|
||||||
* GNU General Public License as published by the Free Software
|
* Public License as published by the Free Software Foundation,
|
||||||
* Foundation, either version 3 of the License, or (at your option)
|
* either version 3 of the License, or (at your option) any later
|
||||||
* any later version.
|
* version.
|
||||||
|
*
|
||||||
|
* Betaflight 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.
|
||||||
*
|
*
|
||||||
* 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.
|
* See the GNU General Public License for more details.
|
||||||
*
|
*
|
||||||
* You should have received a copy of the GNU General Public License
|
* You should have received a copy of the GNU General Public
|
||||||
* along with this software.
|
* License along with this software.
|
||||||
*
|
*
|
||||||
* If not, see <http://www.gnu.org/licenses/>.
|
* If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
@ -29,30 +30,17 @@
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
|
||||||
#include "drivers/camera_control_impl.h"
|
#include "drivers/camera_control_impl.h"
|
||||||
#include "drivers/rcc.h"
|
|
||||||
#include "drivers/io.h"
|
#include "drivers/io.h"
|
||||||
#include "drivers/nvic.h"
|
|
||||||
#include "drivers/pwm_output.h"
|
#include "drivers/pwm_output.h"
|
||||||
#include "drivers/time.h"
|
#include "drivers/time.h"
|
||||||
#include "pg/pg_ids.h"
|
#include "pg/pg_ids.h"
|
||||||
|
|
||||||
#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
|
||||||
#else
|
#else
|
||||||
#define ADC_VOLTAGE 3.3f
|
#define ADC_VOLTAGE 3.3f
|
||||||
#endif
|
#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
|
#ifdef USE_OSD
|
||||||
#include "osd/osd.h"
|
#include "osd/osd.h"
|
||||||
#endif
|
#endif
|
||||||
|
@ -85,7 +73,7 @@ static struct {
|
||||||
static uint32_t endTimeMillis;
|
static uint32_t endTimeMillis;
|
||||||
|
|
||||||
#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
|
#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
|
||||||
static void cameraControlHi(void)
|
void cameraControlHi(void)
|
||||||
{
|
{
|
||||||
if (cameraControlRuntime.inverted) {
|
if (cameraControlRuntime.inverted) {
|
||||||
IOLo(cameraControlRuntime.io);
|
IOLo(cameraControlRuntime.io);
|
||||||
|
@ -94,7 +82,7 @@ static void cameraControlHi(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void cameraControlLo(void)
|
void cameraControlLo(void)
|
||||||
{
|
{
|
||||||
if (cameraControlRuntime.inverted) {
|
if (cameraControlRuntime.inverted) {
|
||||||
IOHi(cameraControlRuntime.io);
|
IOHi(cameraControlRuntime.io);
|
||||||
|
@ -102,19 +90,6 @@ static void cameraControlLo(void)
|
||||||
IOLo(cameraControlRuntime.io);
|
IOLo(cameraControlRuntime.io);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void TIM6_DAC_IRQHandler(void)
|
|
||||||
{
|
|
||||||
cameraControlHi();
|
|
||||||
TIM6->SR = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
void TIM7_IRQHandler(void)
|
|
||||||
{
|
|
||||||
cameraControlLo();
|
|
||||||
|
|
||||||
TIM7->SR = 0;
|
|
||||||
}
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
void cameraControlInit(void)
|
void cameraControlInit(void)
|
||||||
|
@ -136,7 +111,7 @@ void cameraControlInit(void)
|
||||||
|
|
||||||
IOConfigGPIOAF(cameraControlRuntime.io, IOCFG_AF_PP, timerHardware->alternateFunction);
|
IOConfigGPIOAF(cameraControlRuntime.io, IOCFG_AF_PP, timerHardware->alternateFunction);
|
||||||
|
|
||||||
pwmOutConfig(&cameraControlRuntime.channel, timerHardware, timerClock(TIM6), CAMERA_CONTROL_PWM_RESOLUTION, 0, cameraControlRuntime.inverted);
|
cameraControlHardwarePwmInit(&cameraControlRuntime.channel, timerHardware, cameraControlRuntime.inverted);
|
||||||
|
|
||||||
cameraControlRuntime.period = CAMERA_CONTROL_PWM_RESOLUTION;
|
cameraControlRuntime.period = CAMERA_CONTROL_PWM_RESOLUTION;
|
||||||
*cameraControlRuntime.channel.ccr = cameraControlRuntime.period;
|
*cameraControlRuntime.channel.ccr = cameraControlRuntime.period;
|
||||||
|
@ -151,19 +126,7 @@ void cameraControlInit(void)
|
||||||
cameraControlRuntime.period = CAMERA_CONTROL_SOFT_PWM_RESOLUTION;
|
cameraControlRuntime.period = CAMERA_CONTROL_SOFT_PWM_RESOLUTION;
|
||||||
cameraControlRuntime.enabled = true;
|
cameraControlRuntime.enabled = true;
|
||||||
|
|
||||||
|
cameraControlSoftwarePwmInit();
|
||||||
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
|
||||||
|
@ -183,13 +146,14 @@ void cameraControlProcess(uint32_t currentTimeUs)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE) || defined(CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE)
|
||||||
|
|
||||||
static float calculateKeyPressVoltage(const cameraControlKey_e key)
|
static float calculateKeyPressVoltage(const cameraControlKey_e key)
|
||||||
{
|
{
|
||||||
const int buttonResistance = cameraControlConfig()->buttonResistanceValues[key] * 100;
|
const int buttonResistance = cameraControlConfig()->buttonResistanceValues[key] * 100;
|
||||||
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)
|
|
||||||
static float calculatePWMDutyCycle(const cameraControlKey_e key)
|
static float calculatePWMDutyCycle(const cameraControlKey_e key)
|
||||||
{
|
{
|
||||||
const float voltage = calculateKeyPressVoltage(key);
|
const float voltage = calculateKeyPressVoltage(key);
|
||||||
|
@ -231,21 +195,7 @@ void cameraControlKeyPress(cameraControlKey_e key, uint32_t holdDurationMs)
|
||||||
delay(cameraControlConfig()->keyDelayMs + holdDurationMs);
|
delay(cameraControlConfig()->keyDelayMs + holdDurationMs);
|
||||||
cameraControlHi();
|
cameraControlHi();
|
||||||
} else {
|
} else {
|
||||||
TIM6->CNT = hiTime;
|
cameraControlSoftwarePwmEnable(hiTime, cameraControlRuntime.period);
|
||||||
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;
|
const uint32_t endTime = millis() + cameraControlConfig()->keyDelayMs + holdDurationMs;
|
||||||
|
|
||||||
|
@ -253,10 +203,7 @@ void cameraControlKeyPress(cameraControlKey_e key, uint32_t holdDurationMs)
|
||||||
while (millis() < endTime);
|
while (millis() < endTime);
|
||||||
|
|
||||||
// Disable timers and interrupt generation
|
// Disable timers and interrupt generation
|
||||||
TIM6->CR1 &= ~TIM_CR1_CEN;
|
cameraControlSoftwarePwmDisable();
|
||||||
TIM7->CR1 &= ~TIM_CR1_CEN;
|
|
||||||
TIM6->DIER = 0;
|
|
||||||
TIM7->DIER = 0;
|
|
||||||
|
|
||||||
// Reset to idle state
|
// Reset to idle state
|
||||||
IOHi(cameraControlRuntime.io);
|
IOHi(cameraControlRuntime.io);
|
||||||
|
@ -267,4 +214,4 @@ void cameraControlKeyPress(cameraControlKey_e key, uint32_t holdDurationMs)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif // USE_CAMERA_CONTROL
|
|
@ -22,6 +22,10 @@
|
||||||
|
|
||||||
#include "io_types.h"
|
#include "io_types.h"
|
||||||
#include "pg/pg.h"
|
#include "pg/pg.h"
|
||||||
|
#include "drivers/pwm_output.h"
|
||||||
|
|
||||||
|
#define CAMERA_CONTROL_PWM_RESOLUTION 128
|
||||||
|
#define CAMERA_CONTROL_SOFT_PWM_RESOLUTION 448
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
CAMERA_CONTROL_KEY_ENTER,
|
CAMERA_CONTROL_KEY_ENTER,
|
||||||
|
@ -58,3 +62,11 @@ 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);
|
||||||
|
|
||||||
|
void cameraControlHi(void);
|
||||||
|
void cameraControlLo(void);
|
||||||
|
|
||||||
|
void cameraControlSoftwarePwmInit(void);
|
||||||
|
void cameraControlSoftwarePwmEnable(uint32_t hiTime, uint32_t period);
|
||||||
|
void cameraControlSoftwarePwmDisable(void);
|
||||||
|
void cameraControlHardwarePwmInit(timerChannel_t *channel, const timerHardware_t *timerHardware, uint8_t inverted);
|
||||||
|
|
|
@ -23,11 +23,11 @@
|
||||||
#include "drivers/resource.h"
|
#include "drivers/resource.h"
|
||||||
|
|
||||||
#if defined(USE_ATBSP_DRIVER)
|
#if defined(USE_ATBSP_DRIVER)
|
||||||
#include "drivers/mcu/at32/dma_atbsp.h"
|
#include "dma_atbsp.h"
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if defined(APM32F4)
|
#if defined(APM32F4)
|
||||||
#include "drivers/mcu/apm32/dma_apm32.h"
|
#include "dma_apm32.h"
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#define CACHE_LINE_SIZE 32
|
#define CACHE_LINE_SIZE 32
|
||||||
|
|
|
@ -1,268 +0,0 @@
|
||||||
/*
|
|
||||||
* 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 <http://www.gnu.org/licenses/>.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "platform.h"
|
|
||||||
|
|
||||||
#ifdef USE_CAMERA_CONTROL
|
|
||||||
|
|
||||||
#ifndef CAMERA_CONTROL_PIN
|
|
||||||
#define CAMERA_CONTROL_PIN NONE
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#include <math.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
|
|
||||||
#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 TMR6_DAC_IRQHandler(void)
|
|
||||||
{
|
|
||||||
cameraControlHi();
|
|
||||||
TMR6->STS = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
void TMR7_IRQHandler(void)
|
|
||||||
{
|
|
||||||
cameraControlLo();
|
|
||||||
|
|
||||||
TMR7->STS = 0;
|
|
||||||
}
|
|
||||||
#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;
|
|
||||||
|
|
||||||
DAL_NVIC_SetPriority(TMR6_DAC_IRQn, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER));
|
|
||||||
DAL_NVIC_EnableIRQ(TMR6_DAC_IRQn);
|
|
||||||
|
|
||||||
DAL_NVIC_SetPriority(TMR7_IRQn, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER));
|
|
||||||
DAL_NVIC_EnableIRQ(TMR7_IRQn);
|
|
||||||
|
|
||||||
__DAL_RCM_TMR6_CLK_ENABLE();
|
|
||||||
__DAL_RCM_TMR7_CLK_ENABLE();
|
|
||||||
DDL_TMR_SetPrescaler(TMR6, 0);
|
|
||||||
DDL_TMR_SetPrescaler(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 {
|
|
||||||
DDL_TMR_SetCounter(TMR6, hiTime);
|
|
||||||
DDL_TMR_SetAutoReload(TMR6, cameraControlRuntime.period);
|
|
||||||
|
|
||||||
DDL_TMR_SetCounter(TMR7, 0);
|
|
||||||
DDL_TMR_SetAutoReload(TMR7, cameraControlRuntime.period);
|
|
||||||
|
|
||||||
// Start two timers as simultaneously as possible
|
|
||||||
ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
|
|
||||||
DDL_TMR_EnableCounter(TMR6);
|
|
||||||
DDL_TMR_EnableCounter(TMR7);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Enable interrupt generation
|
|
||||||
DDL_TMR_EnableIT_UPDATE(TMR6);
|
|
||||||
DDL_TMR_EnableIT_UPDATE(TMR7);
|
|
||||||
|
|
||||||
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
|
|
||||||
DDL_TMR_DisableCounter(TMR6);
|
|
||||||
DDL_TMR_DisableCounter(TMR7);
|
|
||||||
|
|
||||||
TMR6->DIEN = 0;
|
|
||||||
TMR7->DIEN = 0;
|
|
||||||
|
|
||||||
// Reset to idle state
|
|
||||||
IOHi(cameraControlRuntime.io);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
} else if (CAMERA_CONTROL_MODE_DAC == cameraControlConfig()->mode) {
|
|
||||||
// @todo not yet implemented
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // USE_CAMERA_CONTROL
|
|
|
@ -1,269 +0,0 @@
|
||||||
/*
|
|
||||||
* 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 <http://www.gnu.org/licenses/>.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "platform.h"
|
|
||||||
|
|
||||||
#ifdef USE_CAMERA_CONTROL
|
|
||||||
|
|
||||||
#ifndef CAMERA_CONTROL_PIN
|
|
||||||
#define CAMERA_CONTROL_PIN NONE
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#include <math.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
|
|
||||||
#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
|
|
|
@ -1,3 +0,0 @@
|
||||||
TARGET_MCU := APM32F405xx
|
|
||||||
TARGET_MCU_FAMILY := APM32F4
|
|
||||||
|
|
99
src/platform/APM32/camera_control_apm32.c
Normal file
99
src/platform/APM32/camera_control_apm32.c
Normal file
|
@ -0,0 +1,99 @@
|
||||||
|
/*
|
||||||
|
* This file is part of Betaflight.
|
||||||
|
*
|
||||||
|
* Betaflight is 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.
|
||||||
|
*
|
||||||
|
* Betaflight 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 this software.
|
||||||
|
*
|
||||||
|
* If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "platform.h"
|
||||||
|
|
||||||
|
#ifdef USE_CAMERA_CONTROL
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
#include "drivers/camera_control_impl.h"
|
||||||
|
#include "drivers/nvic.h"
|
||||||
|
#include "drivers/pwm_output.h"
|
||||||
|
|
||||||
|
#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
|
||||||
|
#include "build/atomic.h"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
|
||||||
|
void cameraControlHardwarePwmInit(timerChannel_t *channel, const timerHardware_t *timerHardware, uint8_t inverted)
|
||||||
|
{
|
||||||
|
pwmOutConfig(channel, timerHardware, timerClock(TMR6), CAMERA_CONTROL_PWM_RESOLUTION, 0, inverted);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
|
||||||
|
void TMR6_DAC_IRQHandler(void)
|
||||||
|
{
|
||||||
|
cameraControlHi();
|
||||||
|
TMR6->STS = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void TMR7_IRQHandler(void)
|
||||||
|
{
|
||||||
|
cameraControlLo();
|
||||||
|
TMR7->STS = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void cameraControlSoftwarePwmInit(void)
|
||||||
|
{
|
||||||
|
DAL_NVIC_SetPriority(TMR6_DAC_IRQn, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER));
|
||||||
|
DAL_NVIC_EnableIRQ(TMR6_DAC_IRQn);
|
||||||
|
|
||||||
|
DAL_NVIC_SetPriority(TMR7_IRQn, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER));
|
||||||
|
DAL_NVIC_EnableIRQ(TMR7_IRQn);
|
||||||
|
|
||||||
|
__DAL_RCM_TMR6_CLK_ENABLE();
|
||||||
|
__DAL_RCM_TMR7_CLK_ENABLE();
|
||||||
|
DDL_TMR_SetPrescaler(TMR6, 0);
|
||||||
|
DDL_TMR_SetPrescaler(TMR7, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void cameraControlSoftwarePwmEnable(uint32_t hiTime, uint32_t period)
|
||||||
|
{
|
||||||
|
DDL_TMR_SetCounter(TMR6, hiTime);
|
||||||
|
DDL_TMR_SetAutoReload(TMR6, period);
|
||||||
|
|
||||||
|
DDL_TMR_SetCounter(TMR7, 0);
|
||||||
|
DDL_TMR_SetAutoReload(TMR7, period);
|
||||||
|
|
||||||
|
// Start two timers as simultaneously as possible
|
||||||
|
ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
|
||||||
|
DDL_TMR_EnableCounter(TMR6);
|
||||||
|
DDL_TMR_EnableCounter(TMR7);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Enable interrupt generation
|
||||||
|
DDL_TMR_EnableIT_UPDATE(TMR6);
|
||||||
|
DDL_TMR_EnableIT_UPDATE(TMR7);
|
||||||
|
}
|
||||||
|
|
||||||
|
void cameraControlSoftwarePwmDisable(void)
|
||||||
|
{
|
||||||
|
DDL_TMR_DisableCounter(TMR6);
|
||||||
|
DDL_TMR_DisableCounter(TMR7);
|
||||||
|
|
||||||
|
TMR6->DIEN = 0;
|
||||||
|
TMR7->DIEN = 0;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif // USE_CAMERA_CONTROL
|
|
@ -37,4 +37,4 @@ REGION_ALIAS("VECTAB", RAM)
|
||||||
|
|
||||||
REGION_ALIAS("MOVABLE_FLASH", FLASH1)
|
REGION_ALIAS("MOVABLE_FLASH", FLASH1)
|
||||||
|
|
||||||
INCLUDE "stm32_flash_f4_split.ld"
|
INCLUDE "../../STM32/link/stm32_flash_f4_split.ld"
|
|
@ -37,4 +37,4 @@ REGION_ALIAS("VECTAB", RAM)
|
||||||
|
|
||||||
REGION_ALIAS("MOVABLE_FLASH", FLASH1)
|
REGION_ALIAS("MOVABLE_FLASH", FLASH1)
|
||||||
|
|
||||||
INCLUDE "stm32_flash_f4_split.ld"
|
INCLUDE "../../STM32/link/stm32_flash_f4_split.ld"
|
|
@ -111,8 +111,8 @@ DEVICE_STDPERIPH_SRC := $(STDPERIPH_SRC) \
|
||||||
VPATH := $(VPATH):$(ROOT)/lib/main/APM32F4/Libraries/Device/Geehy/APM32F4xx
|
VPATH := $(VPATH):$(ROOT)/lib/main/APM32F4/Libraries/Device/Geehy/APM32F4xx
|
||||||
|
|
||||||
INCLUDE_DIRS := $(INCLUDE_DIRS) \
|
INCLUDE_DIRS := $(INCLUDE_DIRS) \
|
||||||
$(SRC_DIR)/startup/apm32 \
|
$(TARGET_PLATFORM_DIR)/startup \
|
||||||
$(SRC_DIR)/drivers/mcu/apm32
|
$(TARGET_PLATFORM_DIR)
|
||||||
|
|
||||||
CMSIS_SRC :=
|
CMSIS_SRC :=
|
||||||
INCLUDE_DIRS := $(INCLUDE_DIRS) \
|
INCLUDE_DIRS := $(INCLUDE_DIRS) \
|
||||||
|
@ -121,9 +121,9 @@ INCLUDE_DIRS := $(INCLUDE_DIRS) \
|
||||||
$(USBCDC_DIR)/Inc \
|
$(USBCDC_DIR)/Inc \
|
||||||
$(USBMSC_DIR)/Inc \
|
$(USBMSC_DIR)/Inc \
|
||||||
$(CMSIS_DIR)/Geehy/APM32F4xx/Include \
|
$(CMSIS_DIR)/Geehy/APM32F4xx/Include \
|
||||||
$(SRC_DIR)/drivers/mcu/apm32/usb/vcp \
|
$(TARGET_PLATFORM_DIR)/usb/vcp \
|
||||||
$(SRC_DIR)/drivers/mcu/apm32/usb/msc \
|
$(TARGET_PLATFORM_DIR)/usb/msc \
|
||||||
$(SRC_DIR)/drivers/mcu/apm32/usb \
|
$(TARGET_PLATFORM_DIR)/usb \
|
||||||
$(ROOT)/lib/main/CMSIS/Core/Include \
|
$(ROOT)/lib/main/CMSIS/Core/Include \
|
||||||
$(SRC_DIR)/msc
|
$(SRC_DIR)/msc
|
||||||
|
|
||||||
|
@ -135,59 +135,59 @@ DEVICE_FLAGS = -DUSE_DAL_DRIVER -DHSE_VALUE=$(HSE_VALUE) -DAPM32
|
||||||
ifeq ($(TARGET_MCU),APM32F405xx)
|
ifeq ($(TARGET_MCU),APM32F405xx)
|
||||||
DEVICE_FLAGS += -DAPM32F405xx
|
DEVICE_FLAGS += -DAPM32F405xx
|
||||||
LD_SCRIPT = $(LINKER_DIR)/apm32_flash_f405.ld
|
LD_SCRIPT = $(LINKER_DIR)/apm32_flash_f405.ld
|
||||||
STARTUP_SRC = apm32/startup_apm32f405xx.S
|
STARTUP_SRC = startup/startup_apm32f405xx.S
|
||||||
MCU_FLASH_SIZE := 1024
|
MCU_FLASH_SIZE := 1024
|
||||||
|
|
||||||
else ifeq ($(TARGET_MCU),APM32F407xx)
|
else ifeq ($(TARGET_MCU),APM32F407xx)
|
||||||
DEVICE_FLAGS += -DAPM32F407xx
|
DEVICE_FLAGS += -DAPM32F407xx
|
||||||
LD_SCRIPT = $(LINKER_DIR)/apm32_flash_f407.ld
|
LD_SCRIPT = $(LINKER_DIR)/apm32_flash_f407.ld
|
||||||
STARTUP_SRC = apm32/startup_apm32f407xx.S
|
STARTUP_SRC = startup/startup_apm32f407xx.S
|
||||||
MCU_FLASH_SIZE := 1024
|
MCU_FLASH_SIZE := 1024
|
||||||
else
|
else
|
||||||
$(error TARGET_MCU [$(TARGET_MCU] is not supported)
|
$(error TARGET_MCU [$(TARGET_MCU)] is not supported)
|
||||||
endif
|
endif
|
||||||
|
|
||||||
MCU_COMMON_SRC = \
|
MCU_COMMON_SRC = \
|
||||||
startup/apm32/system_apm32f4xx.c \
|
startup/system_apm32f4xx.c \
|
||||||
drivers/inverter.c \
|
drivers/inverter.c \
|
||||||
drivers/dshot_bitbang_decode.c \
|
drivers/dshot_bitbang_decode.c \
|
||||||
drivers/pwm_output_dshot_shared.c \
|
drivers/pwm_output_dshot_shared.c \
|
||||||
drivers/mcu/apm32/bus_spi_apm32.c \
|
bus_spi_apm32.c \
|
||||||
drivers/mcu/apm32/bus_i2c_apm32.c \
|
bus_i2c_apm32.c \
|
||||||
drivers/mcu/apm32/bus_i2c_apm32_init.c \
|
bus_i2c_apm32_init.c \
|
||||||
drivers/mcu/apm32/camera_control.c \
|
camera_control_apm32.c \
|
||||||
drivers/mcu/apm32/debug.c \
|
debug.c \
|
||||||
drivers/mcu/apm32/dma_reqmap_mcu.c \
|
dma_reqmap_mcu.c \
|
||||||
drivers/mcu/apm32/dshot_bitbang.c \
|
dshot_bitbang.c \
|
||||||
drivers/mcu/apm32/dshot_bitbang_ddl.c \
|
dshot_bitbang_ddl.c \
|
||||||
drivers/mcu/apm32/eint_apm32.c \
|
eint_apm32.c \
|
||||||
drivers/mcu/apm32/io_apm32.c \
|
io_apm32.c \
|
||||||
drivers/mcu/apm32/light_ws2811strip_apm32.c \
|
light_ws2811strip_apm32.c \
|
||||||
drivers/mcu/apm32/persistent_apm32.c \
|
persistent_apm32.c \
|
||||||
drivers/mcu/apm32/pwm_output_apm32.c \
|
pwm_output_apm32.c \
|
||||||
drivers/mcu/apm32/pwm_output_dshot_apm32.c \
|
pwm_output_dshot_apm32.c \
|
||||||
drivers/mcu/apm32/rcm_apm32.c \
|
rcm_apm32.c \
|
||||||
drivers/mcu/apm32/serial_uart_apm32.c \
|
serial_uart_apm32.c \
|
||||||
drivers/mcu/apm32/timer_apm32.c \
|
timer_apm32.c \
|
||||||
drivers/mcu/apm32/transponder_ir_io_apm32.c \
|
transponder_ir_io_apm32.c \
|
||||||
drivers/mcu/apm32/timer_apm32f4xx.c \
|
timer_apm32f4xx.c \
|
||||||
drivers/mcu/apm32/adc_apm32f4xx.c \
|
adc_apm32f4xx.c \
|
||||||
drivers/mcu/apm32/dma_apm32f4xx.c \
|
dma_apm32f4xx.c \
|
||||||
drivers/mcu/apm32/serial_uart_apm32f4xx.c \
|
serial_uart_apm32f4xx.c \
|
||||||
drivers/mcu/apm32/system_apm32f4xx.c
|
system_apm32f4xx.c
|
||||||
|
|
||||||
VCP_SRC = \
|
VCP_SRC = \
|
||||||
drivers/mcu/apm32/usb/vcp/usbd_cdc_descriptor.c \
|
usb/vcp/usbd_cdc_descriptor.c \
|
||||||
drivers/mcu/apm32/usb/usbd_board_apm32f4.c \
|
usb/usbd_board_apm32f4.c \
|
||||||
drivers/mcu/apm32/usb/vcp/usbd_cdc_vcp.c \
|
usb/vcp/usbd_cdc_vcp.c \
|
||||||
drivers/mcu/apm32/usb/vcp/serial_usb_vcp.c \
|
usb/vcp/serial_usb_vcp.c \
|
||||||
drivers/usb_io.c
|
drivers/usb_io.c
|
||||||
|
|
||||||
MSC_SRC = \
|
MSC_SRC = \
|
||||||
drivers/usb_msc_common.c \
|
drivers/usb_msc_common.c \
|
||||||
drivers/mcu/apm32/usb/msc/usb_msc_apm32f4xx.c \
|
usb/msc/usb_msc_apm32f4xx.c \
|
||||||
drivers/mcu/apm32/usb/msc/usbd_memory.c \
|
usb/msc/usbd_memory.c \
|
||||||
drivers/mcu/apm32/usb/msc/usbd_msc_descriptor.c \
|
usb/msc/usbd_msc_descriptor.c \
|
||||||
msc/usbd_storage.c \
|
msc/usbd_storage.c \
|
||||||
msc/usbd_storage_emfat.c \
|
msc/usbd_storage_emfat.c \
|
||||||
msc/emfat.c \
|
msc/emfat.c \
|
2
src/platform/APM32/target/APM32F405/target.mk
Normal file
2
src/platform/APM32/target/APM32F405/target.mk
Normal file
|
@ -0,0 +1,2 @@
|
||||||
|
TARGET_MCU := APM32F405xx
|
||||||
|
TARGET_MCU_FAMILY := APM32F4
|
97
src/platform/AT32/camera_control_at32.c
Normal file
97
src/platform/AT32/camera_control_at32.c
Normal file
|
@ -0,0 +1,97 @@
|
||||||
|
/*
|
||||||
|
* This file is part of Betaflight.
|
||||||
|
*
|
||||||
|
* Betaflight is 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.
|
||||||
|
*
|
||||||
|
* Betaflight 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 this software.
|
||||||
|
*
|
||||||
|
* If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "platform.h"
|
||||||
|
|
||||||
|
#ifdef USE_CAMERA_CONTROL
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
#include "drivers/camera_control_impl.h"
|
||||||
|
#include "drivers/nvic.h"
|
||||||
|
#include "drivers/pwm_output.h"
|
||||||
|
#include "drivers/rcc.h"
|
||||||
|
|
||||||
|
#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
|
||||||
|
#include "build/atomic.h"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
|
||||||
|
void cameraControlHardwarePwmInit(timerChannel_t *channel, const timerHardware_t *timerHardware, uint8_t inverted)
|
||||||
|
{
|
||||||
|
pwmOutConfig(channel, timerHardware, timerClock(TMR6), CAMERA_CONTROL_PWM_RESOLUTION, 0, inverted);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
void cameraControlSoftwarePwmInit(void)
|
||||||
|
{
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
void cameraControlSoftwarePwmEnable(uint32_t hiTime, uint32_t period)
|
||||||
|
{
|
||||||
|
tmr_counter_value_set(TMR6, hiTime);
|
||||||
|
tmr_period_value_set(TMR6, period);
|
||||||
|
|
||||||
|
tmr_counter_value_set(TMR7, 0);
|
||||||
|
tmr_period_value_set(TMR7, 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);
|
||||||
|
}
|
||||||
|
|
||||||
|
void cameraControlSoftwarePwmDisable(void)
|
||||||
|
{
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif // USE_CAMERA_CONTROL
|
|
@ -45,7 +45,7 @@ STDPERIPH_SRC = \
|
||||||
usbd_class/msc/msc_class.c \
|
usbd_class/msc/msc_class.c \
|
||||||
usbd_class/msc/msc_desc.c
|
usbd_class/msc/msc_desc.c
|
||||||
|
|
||||||
STARTUP_SRC = at32/startup_at32f435_437.s
|
STARTUP_SRC = startup/startup_at32f435_437.s
|
||||||
|
|
||||||
VPATH := $(VPATH):$(ROOT)/lib/main/AT32F43x/cmsis/cm4/core_support:$(STDPERIPH_DIR)/src:$(MIDDLEWARES_DIR):$(SRC_DIR)/startup/at32
|
VPATH := $(VPATH):$(ROOT)/lib/main/AT32F43x/cmsis/cm4/core_support:$(STDPERIPH_DIR)/src:$(MIDDLEWARES_DIR):$(SRC_DIR)/startup/at32
|
||||||
|
|
||||||
|
@ -61,9 +61,8 @@ VCP_INCLUDES = \
|
||||||
DEVICE_STDPERIPH_SRC = $(STDPERIPH_SRC)
|
DEVICE_STDPERIPH_SRC = $(STDPERIPH_SRC)
|
||||||
|
|
||||||
INCLUDE_DIRS := $(INCLUDE_DIRS) \
|
INCLUDE_DIRS := $(INCLUDE_DIRS) \
|
||||||
$(SRC_DIR)/startup/at32 \
|
$(TARGET_PLATFORM_DIR)/startup \
|
||||||
$(SRC_DIR)/drivers \
|
$(TARGET_PLATFORM_DIR) \
|
||||||
$(SRC_DIR)/drivers/mcu/at32 \
|
|
||||||
$(STDPERIPH_DIR)/inc \
|
$(STDPERIPH_DIR)/inc \
|
||||||
$(CMSIS_DIR)/cm4/core_support \
|
$(CMSIS_DIR)/cm4/core_support \
|
||||||
$(CMSIS_DIR)/cm4 \
|
$(CMSIS_DIR)/cm4 \
|
||||||
|
@ -81,32 +80,32 @@ ARCH_FLAGS = -std=c99 -mthumb -mcpu=cortex-m4 -march=armv7e-m -mfloat-abi=
|
||||||
DEVICE_FLAGS += -DUSE_ATBSP_DRIVER -DAT32F43x -DHSE_VALUE=$(HSE_VALUE) -DAT32 -DUSE_OTG_HOST_MODE
|
DEVICE_FLAGS += -DUSE_ATBSP_DRIVER -DAT32F43x -DHSE_VALUE=$(HSE_VALUE) -DAT32 -DUSE_OTG_HOST_MODE
|
||||||
|
|
||||||
MCU_COMMON_SRC = \
|
MCU_COMMON_SRC = \
|
||||||
startup/at32/at32f435_437_clock.c \
|
startup/at32f435_437_clock.c \
|
||||||
startup/at32/system_at32f435_437.c \
|
startup/system_at32f435_437.c \
|
||||||
drivers/mcu/at32/adc_at32f43x.c \
|
adc_at32f43x.c \
|
||||||
drivers/mcu/at32/bus_i2c_atbsp.c \
|
bus_i2c_atbsp.c \
|
||||||
drivers/mcu/at32/bus_i2c_atbsp_init.c \
|
bus_i2c_atbsp_init.c \
|
||||||
drivers/mcu/at32/bus_spi_at32bsp.c \
|
bus_spi_at32bsp.c \
|
||||||
drivers/mcu/at32/camera_control.c \
|
camera_control_at32.c \
|
||||||
drivers/mcu/at32/debug.c \
|
debug.c \
|
||||||
drivers/mcu/at32/dma_at32f43x.c \
|
dma_at32f43x.c \
|
||||||
drivers/mcu/at32/dma_reqmap_mcu.c \
|
dma_reqmap_mcu.c \
|
||||||
drivers/mcu/at32/dshot_bitbang.c \
|
dshot_bitbang.c \
|
||||||
drivers/mcu/at32/dshot_bitbang_stdperiph.c \
|
dshot_bitbang_stdperiph.c \
|
||||||
drivers/mcu/at32/exti_at32.c \
|
exti_at32.c \
|
||||||
drivers/mcu/at32/io_at32.c \
|
io_at32.c \
|
||||||
drivers/mcu/at32/light_ws2811strip_at32f43x.c \
|
light_ws2811strip_at32f43x.c \
|
||||||
drivers/mcu/at32/persistent_at32bsp.c \
|
persistent_at32bsp.c \
|
||||||
drivers/mcu/at32/pwm_output_at32bsp.c \
|
pwm_output_at32bsp.c \
|
||||||
drivers/mcu/at32/pwm_output_dshot.c \
|
pwm_output_dshot.c \
|
||||||
drivers/mcu/at32/rcc_at32.c \
|
rcc_at32.c \
|
||||||
drivers/mcu/at32/serial_uart_at32bsp.c \
|
serial_uart_at32bsp.c \
|
||||||
drivers/mcu/at32/serial_uart_at32f43x.c \
|
serial_uart_at32f43x.c \
|
||||||
drivers/mcu/at32/serial_usb_vcp_at32f4.c \
|
serial_usb_vcp_at32f4.c \
|
||||||
drivers/mcu/at32/system_at32f43x.c \
|
system_at32f43x.c \
|
||||||
drivers/mcu/at32/timer_at32bsp.c \
|
timer_at32bsp.c \
|
||||||
drivers/mcu/at32/timer_at32f43x.c \
|
timer_at32f43x.c \
|
||||||
drivers/mcu/at32/usb_msc_at32f43x.c \
|
usb_msc_at32f43x.c \
|
||||||
drivers/accgyro/accgyro_mpu.c \
|
drivers/accgyro/accgyro_mpu.c \
|
||||||
drivers/dshot_bitbang_decode.c \
|
drivers/dshot_bitbang_decode.c \
|
||||||
drivers/inverter.c \
|
drivers/inverter.c \
|
Some files were not shown because too many files have changed in this diff Show more
Loading…
Add table
Add a link
Reference in a new issue