rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rebased

This commit is contained in:
jflyper 2017-07-18 17:45:55 +09:00
parent e436c24fb6 dcc600a78b
commit ec8592d779
21 changed files with 423 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Makefile
View File

@ -97,7 +97,6 @@ VALID_TARGETS = $(dir $(wildcard $(ROOT)/src/main/target/*/target.mk))
VALID_TARGETS := $(subst /,, $(subst ./src/main/target/,, $(VALID_TARGETS)))
VALID_TARGETS := $(VALID_TARGETS) $(ALT_TARGETS)
VALID_TARGETS := $(sort $(VALID_TARGETS))
VALID_TARGETS := $(filter-out $(SKIP_TARGETS), $(VALID_TARGETS))
GROUP_1_TARGETS := \
AFROMINI \
@ -793,6 +792,7 @@ FC_SRC = \
cms/cms_menu_osd.c \
common/colorconversion.c \
common/gps_conversion.c \
drivers/camera_control.c \
drivers/display_ug2864hsweg01.c \
drivers/light_ws2811strip.c \
drivers/serial_escserial.c \
@ -894,6 +894,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
drivers/bus_i2c_config.c \
drivers/bus_spi_config.c \
drivers/bus_spi_pinconfig.c \
drivers/camera_control.c \
drivers/serial_escserial.c \
drivers/serial_pinconfig.c \
drivers/serial_uart_init.c \
@ -1344,10 +1345,13 @@ targets-group-rest: $(GROUP_OTHER_TARGETS)
$(VALID_TARGETS):
$(V0) @echo "" && \
$(V0) $(if $(findstring $@,$(SKIP_TARGETS)), \
@echo "" && \
echo "Not building $@ since it is listed in SKIP_TARGETS.", \
@echo "" && \
echo "Building $@" && \
time $(MAKE) binary hex TARGET=$@ && \
echo "Building $@ succeeded."
echo "Building $@ succeeded.")
CLEAN_TARGETS = $(addprefix clean_,$(VALID_TARGETS) )
TARGETS_CLEAN = $(addsuffix _clean,$(VALID_TARGETS) )

3
src/main/config/parameter_group_ids.h
View File

@ -110,7 +110,8 @@
#define PG_DASHBOARD_CONFIG 519
#define PG_SPI_PIN_CONFIG 520
#define PG_ESCSERIAL_CONFIG 521
#define PG_BETAFLIGHT_END 521
#define PG_CAMERA_CONTROL_CONFIG 522
#define PG_BETAFLIGHT_END 522
// OSD configuration (subject to change)

238
src/main/drivers/camera_control.c Normal file
View File

@ -0,0 +1,238 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <platform.h>
#ifdef USE_CAMERA_CONTROL
#include "camera_control.h"
#include "io.h"
#include "math.h"
#include "nvic.h"
#include "pwm_output.h"
#include "time.h"
#include "config/parameter_group_ids.h"
#if defined(STM32F40_41xxx)
#define CAMERA_CONTROL_TIMER_MHZ 84
#elif defined(STM32F7)
#define CAMERA_CONTROL_TIMER_MHZ 108
#else
#define CAMERA_CONTROL_TIMER_MHZ 72
#endif
#define CAMERA_CONTROL_PWM_RESOLUTION 128
#define CAMERA_CONTROL_SOFT_PWM_RESOLUTION 448
#ifdef CURRENT_TARGET_CPU_VOLTAGE
#define ADC_VOLTAGE CURRENT_TARGET_CPU_VOLTAGE
#else
#define ADC_VOLTAGE 3.3f
#endif
#if !defined(STM32F411xE) && !defined(STM32F7)
#define CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
#include "build/atomic.h"
#endif
#define CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
#include "timer.h"
PG_REGISTER_WITH_RESET_TEMPLATE(cameraControlConfig_t, cameraControlConfig, PG_CAMERA_CONTROL_CONFIG, 0);
PG_RESET_TEMPLATE(cameraControlConfig_t, cameraControlConfig,
.mode = CAMERA_CONTROL_MODE_HARDWARE_PWM,
.refVoltage = 330,
.keyDelayMs = 150,
.ioTag = IO_TAG_NONE
);
static struct {
bool enabled;
IO_t io;
timerChannel_t channel;
uint32_t period;
} cameraControlRuntime;
static uint32_t endTimeMillis;
#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
void TIM6_DAC_IRQHandler()
{
IOHi(cameraControlRuntime.io);
TIM6->SR = 0;
}
void TIM7_IRQHandler()
{
IOLo(cameraControlRuntime.io);
TIM7->SR = 0;
}
#endif
void cameraControlInit()
{
if (cameraControlConfig()->ioTag == IO_TAG_NONE)
return;
cameraControlRuntime.io = IOGetByTag(cameraControlConfig()->ioTag);
IOInit(cameraControlRuntime.io, OWNER_CAMERA_CONTROL, 0);
if (CAMERA_CONTROL_MODE_HARDWARE_PWM == cameraControlConfig()->mode) {
#ifdef CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
const timerHardware_t *timerHardware = timerGetByTag(cameraControlConfig()->ioTag, TIM_USE_ANY);
if (!timerHardware) {
return;
}
#ifdef USE_HAL_DRIVER
IOConfigGPIOAF(cameraControlRuntime.io, IOCFG_AF_PP, timerHardware->alternateFunction);
#else
IOConfigGPIO(cameraControlRuntime.io, IOCFG_AF_PP);
#endif
pwmOutConfig(&cameraControlRuntime.channel, timerHardware, CAMERA_CONTROL_TIMER_MHZ * 1000000, CAMERA_CONTROL_PWM_RESOLUTION, 0, 0);
*cameraControlRuntime.channel.ccr = cameraControlRuntime.period;
cameraControlRuntime.enabled = true;
#endif
} else if (CAMERA_CONTROL_MODE_SOFTWARE_PWM == cameraControlConfig()->mode) {
#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
IOConfigGPIO(cameraControlRuntime.io, IOCFG_OUT_PP);
IOHi(cameraControlRuntime.io);
cameraControlRuntime.period = CAMERA_CONTROL_SOFT_PWM_RESOLUTION;
cameraControlRuntime.enabled = true;
NVIC_InitTypeDef nvicTIM6 = {
TIM6_DAC_IRQn, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER), ENABLE
};
NVIC_Init(&nvicTIM6);
NVIC_InitTypeDef nvicTIM7 = {
TIM7_IRQn, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER), ENABLE
};
NVIC_Init(&nvicTIM7);
RCC->APB1ENR |= RCC_APB1Periph_TIM6 | RCC_APB1Periph_TIM7;
TIM6->PSC = 0;
TIM7->PSC = 0;
#endif
} else if (CAMERA_CONTROL_MODE_DAC == cameraControlConfig()->mode) {
// @todo not yet implemented
}
}
void cameraControlProcess(uint32_t currentTimeUs)
{
if (endTimeMillis && currentTimeUs >= 1000 * endTimeMillis) {
if (CAMERA_CONTROL_MODE_HARDWARE_PWM == cameraControlConfig()->mode) {
*cameraControlRuntime.channel.ccr = cameraControlRuntime.period;
} else if (CAMERA_CONTROL_MODE_SOFTWARE_PWM == cameraControlConfig()->mode) {
}
endTimeMillis = 0;
}
}
static const int cameraPullUpResistance = 47000;
static const int buttonResistanceValues[] = { 45000, 27000, 15000, 6810, 0 };
static float calculateKeyPressVoltage(const cameraControlKey_e key)
{
const int buttonResistance = buttonResistanceValues[key];
return 1.0e-2f * cameraControlConfig()->refVoltage * buttonResistance / (cameraPullUpResistance + buttonResistance);
}
#if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE) || defined(CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE)
static float calculatePWMDutyCycle(const cameraControlKey_e key)
{
const float voltage = calculateKeyPressVoltage(key);
return voltage / ADC_VOLTAGE;
}
#endif
void cameraControlKeyPress(cameraControlKey_e key, uint32_t holdDurationMs)
{
if (!cameraControlRuntime.enabled)
return;
if (key >= CAMERA_CONTROL_KEYS_COUNT)
return;
#if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE) || defined(CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE)
const float dutyCycle = calculatePWMDutyCycle(key);
#else
(void) holdDurationMs;
#endif
if (CAMERA_CONTROL_MODE_HARDWARE_PWM == cameraControlConfig()->mode) {
#ifdef CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
*cameraControlRuntime.channel.ccr = lrintf(dutyCycle * cameraControlRuntime.period);
endTimeMillis = millis() + cameraControlConfig()->keyDelayMs + holdDurationMs;
#endif
} else if (CAMERA_CONTROL_MODE_SOFTWARE_PWM == cameraControlConfig()->mode) {
#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
const uint32_t hiTime = lrintf(dutyCycle * cameraControlRuntime.period);
if (0 == hiTime) {
IOLo(cameraControlRuntime.io);
delay(cameraControlConfig()->keyDelayMs + holdDurationMs);
IOHi(cameraControlRuntime.io);
} else {
TIM6->CNT = hiTime;
TIM6->ARR = cameraControlRuntime.period;
TIM7->CNT = 0;
TIM7->ARR = cameraControlRuntime.period;
// Start two timers as simultaneously as possible
ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
TIM6->CR1 = TIM_CR1_CEN;
TIM7->CR1 = TIM_CR1_CEN;
}
// Enable interrupt generation
TIM6->DIER = TIM_IT_Update;
TIM7->DIER = TIM_IT_Update;
const uint32_t endTime = millis() + cameraControlConfig()->keyDelayMs + holdDurationMs;
// Wait to give the camera a chance at registering the key press
while (millis() < endTime);
// Disable timers and interrupt generation
TIM6->CR1 &= ~TIM_CR1_CEN;
TIM7->CR1 &= ~TIM_CR1_CEN;
TIM6->DIER = 0;
TIM7->DIER = 0;
// Reset to idle state
IOHi(cameraControlRuntime.io);
}
#endif
} else if (CAMERA_CONTROL_MODE_DAC == cameraControlConfig()->mode) {
// @todo not yet implemented
}
}
#endif

52
src/main/drivers/camera_control.h Normal file
View File

@ -0,0 +1,52 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "io_types.h"
#include "config/parameter_group.h"
typedef enum {
CAMERA_CONTROL_KEY_ENTER,
CAMERA_CONTROL_KEY_LEFT,
CAMERA_CONTROL_KEY_UP,
CAMERA_CONTROL_KEY_RIGHT,
CAMERA_CONTROL_KEY_DOWN,
CAMERA_CONTROL_KEYS_COUNT
} cameraControlKey_e;
typedef enum {
CAMERA_CONTROL_MODE_HARDWARE_PWM,
CAMERA_CONTROL_MODE_SOFTWARE_PWM,
CAMERA_CONTROL_MODE_DAC,
CAMERA_CONTROL_MODES_COUNT
} cameraControlMode_e;
typedef struct cameraControlConfig_s {
cameraControlMode_e mode;
uint16_t refVoltage;
uint16_t keyDelayMs;
ioTag_t ioTag;
} cameraControlConfig_t;
PG_DECLARE(cameraControlConfig_t, cameraControlConfig);
void cameraControlInit();
void cameraControlProcess(uint32_t currentTimeUs);
void cameraControlKeyPress(cameraControlKey_e key, uint32_t holdDurationMs);

36
src/main/drivers/pwm_output.c
View File

@ -96,7 +96,7 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8
#endif
}
static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion)
void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion)
{
#if defined(USE_HAL_DRIVER)
TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim);
@ -121,11 +121,11 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard
TIM_Cmd(timerHardware->tim, ENABLE);
#endif
port->ccr = timerChCCR(timerHardware);
channel->ccr = timerChCCR(timerHardware);
port->tim = timerHardware->tim;
channel->tim = timerHardware->tim;
*port->ccr = 0;
*channel->ccr = 0;
}
static void pwmWriteUnused(uint8_t index, float value)
@ -137,7 +137,7 @@ static void pwmWriteUnused(uint8_t index, float value)
static void pwmWriteStandard(uint8_t index, float value)
{
/* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */
*motors[index].ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset);
*motors[index].channel.ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset);
}
#ifdef USE_DSHOT
@ -176,8 +176,8 @@ void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
{
for (int index = 0; index < motorCount; index++) {
// Set the compare register to 0, which stops the output pulsing if the timer overflows
if (motors[index].ccr) {
*motors[index].ccr = 0;
if (motors[index].channel.ccr) {
*motors[index].channel.ccr = 0;
}
}
}
@ -208,11 +208,11 @@ static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
{
for (int index = 0; index < motorCount; index++) {
if (motors[index].forceOverflow) {
timerForceOverflow(motors[index].tim);
timerForceOverflow(motors[index].channel.tim);
}
// Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again.
// This compare register will be set to the output value on the next main loop.
*motors[index].ccr = 0;
*motors[index].channel.ccr = 0;
}
}
@ -328,11 +328,11 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
motors[motorIndex].pulseScale = ((motorConfig->motorPwmProtocol == PWM_TYPE_BRUSHED) ? period : (sLen * hz)) / 1000.0f;
motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000);
pwmOutConfig(&motors[motorIndex], timerHardware, hz, period, idlePulse, motorConfig->motorPwmInversion);
pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorPwmInversion);
bool timerAlreadyUsed = false;
for (int i = 0; i < motorIndex; i++) {
if (motors[i].tim == motors[motorIndex].tim) {
if (motors[i].channel.tim == motors[motorIndex].channel.tim) {
timerAlreadyUsed = true;
break;
}
@ -425,8 +425,8 @@ uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, const uint16_t value)
#ifdef USE_SERVOS
void pwmWriteServo(uint8_t index, float value)
{
if (index < MAX_SUPPORTED_SERVOS && servos[index].ccr) {
*servos[index].ccr = lrintf(value);
if (index < MAX_SUPPORTED_SERVOS && servos[index].channel.ccr) {
*servos[index].channel.ccr = lrintf(value);
}
}
@ -454,7 +454,7 @@ void servoDevInit(const servoDevConfig_t *servoConfig)
/* flag failure and disable ability to arm */
break;
}
pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
pwmOutConfig(&servos[servoIndex].channel, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
servos[servoIndex].enabled = true;
}
}
@ -467,10 +467,10 @@ void pwmWriteBeeper(bool onoffBeep)
if (!beeperPwm.io)
return;
if (onoffBeep == true) {
*beeperPwm.ccr = (PWM_TIMER_1MHZ / freqBeep) / 2;
*beeperPwm.channel.ccr = (PWM_TIMER_1MHZ / freqBeep) / 2;
beeperPwm.enabled = true;
} else {
*beeperPwm.ccr = 0;
*beeperPwm.channel.ccr = 0;
beeperPwm.enabled = false;
}
}
@ -492,9 +492,9 @@ void beeperPwmInit(const ioTag_t tag, uint16_t frequency)
IOConfigGPIO(beeperPwm.io, IOCFG_AF_PP);
#endif
freqBeep = frequency;
pwmOutConfig(&beeperPwm, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / freqBeep, (PWM_TIMER_1MHZ / freqBeep) / 2, 0);
pwmOutConfig(&beeperPwm.channel, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / freqBeep, (PWM_TIMER_1MHZ / freqBeep) / 2, 0);
}
*beeperPwm.ccr = 0;
*beeperPwm.channel.ccr = 0;
beeperPwm.enabled = false;
}
#endif

5
src/main/drivers/pwm_output.h
View File

@ -132,6 +132,10 @@ typedef void pwmCompleteWriteFunc(uint8_t motorCount); // function pointer use
typedef struct {
volatile timCCR_t *ccr;
TIM_TypeDef *tim;
} timerChannel_t;
typedef struct {
timerChannel_t channel;
float pulseScale;
float pulseOffset;
bool forceOverflow;
@ -181,6 +185,7 @@ void pwmWriteBeeper(bool onoffBeep);
void pwmToggleBeeper(void);
void beeperPwmInit(const ioTag_t tag, uint16_t frequency);
#endif
void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion);
void pwmWriteMotor(uint8_t index, float value);
void pwmShutdownPulsesForAllMotors(uint8_t motorCount);

1
src/main/drivers/resource.c
View File

@ -65,5 +65,6 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
"SPI_PREINIT",
"RX_BIND_PLUG",
"ESCSERIAL",
"CAMERA_CONTROL",
};

1
src/main/drivers/resource.h
View File

@ -65,6 +65,7 @@ typedef enum {
OWNER_SPI_PREINIT,
OWNER_RX_BIND_PLUG,
OWNER_ESCSERIAL,
OWNER_CAMERA_CONTROL,
OWNER_TOTAL_COUNT
} resourceOwner_e;

2
src/main/drivers/rx_pwm.c
View File

@ -412,7 +412,7 @@ void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer)
{
pwmOutputPort_t *motors = pwmGetMotors();
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS; motorIndex++) {
if (!motors[motorIndex].enabled || motors[motorIndex].tim != pwmTimer) {
if (!motors[motorIndex].enabled || motors[motorIndex].channel.tim != pwmTimer) {
continue;
}

4
src/main/fc/cli.c
View File

@ -77,6 +77,7 @@ extern uint8_t __config_end;
#include "drivers/timer.h"
#include "drivers/vcd.h"
#include "drivers/light_led.h"
#include "drivers/camera_control.h"
#include "fc/settings.h"
#include "fc/cli.h"
@ -3031,6 +3032,9 @@ const cliResourceValue_t resourceTable[] = {
#ifdef USE_ESCSERIAL
{ OWNER_ESCSERIAL, PG_ESCSERIAL_CONFIG, offsetof(escSerialConfig_t, ioTag), 0 },
#endif
#ifdef CAMERA_CONTROL
{ OWNER_CAMERA_CONTROL, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, ioTag), 0 },
#endif
#ifdef BARO
{ OWNER_BARO_CS, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_spi_csn), 0 },
#endif

5
src/main/fc/fc_init.c
View File

@ -68,6 +68,7 @@
#include "drivers/max7456.h"
#include "drivers/vtx_rtc6705.h"
#include "drivers/vtx_common.h"
#include "drivers/camera_control.h"
#include "fc/config.h"
#include "fc/fc_init.h"
@ -469,6 +470,10 @@ void init(void)
rtc6705IOInit();
#endif
#ifdef CAMERA_CONTROL
cameraControlInit();
#endif
#if defined(SONAR_SOFTSERIAL2_EXCLUSIVE) && defined(SONAR) && defined(USE_SOFTSERIAL2)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);

14
src/main/fc/fc_msp.c
View File

@ -53,6 +53,7 @@
#include "drivers/vcd.h"
#include "drivers/vtx_common.h"
#include "drivers/transponder_ir.h"
#include "drivers/camera_control.h"
#include "fc/config.h"
#include "fc/controlrate_profile.h"
@ -1856,6 +1857,19 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
#endif
#ifdef USE_CAMERA_CONTROL
case MSP_CAMERA_CONTROL:
{
if (ARMING_FLAG(ARMED)) {
return MSP_RESULT_ERROR;
}
const uint8_t key = sbufReadU8(src);
cameraControlKeyPress(key, 0);
}
break;
#endif
#ifdef USE_FLASHFS
case MSP_DATAFLASH_ERASE:
flashfsEraseCompletely();

24
src/main/fc/fc_tasks.c
View File

@ -41,6 +41,7 @@
#include "drivers/stack_check.h"
#include "drivers/vtx_common.h"
#include "drivers/transponder_ir.h"
#include "drivers/camera_control.h"
#include "fc/config.h"
#include "fc/fc_msp.h"
@ -256,6 +257,17 @@ void osdSlaveTasksInit(void)
#endif
#ifndef USE_OSD_SLAVE
#ifdef USE_CAMERA_CONTROL
void taskCameraControl(uint32_t currentTime)
{
if (ARMING_FLAG(ARMED))
return;
cameraControlProcess(currentTime);
}
#endif
void fcTasksInit(void)
{
schedulerInit();
@ -356,6 +368,9 @@ void fcTasksInit(void)
setTaskEnabled(TASK_VTXCTRL, true);
#endif
#endif
#ifdef USE_CAMERA_CONTROL
setTaskEnabled(TASK_CAMCTRL, true);
#endif
}
#endif
@ -607,5 +622,14 @@ cfTask_t cfTasks[TASK_COUNT] = {
.staticPriority = TASK_PRIORITY_MEDIUM,
},
#endif
#ifdef USE_CAMERA_CONTROL
[TASK_CAMCTRL] = {
.taskName = "CAMCTRL",
.taskFunc = taskCameraControl,
.desiredPeriod = TASK_PERIOD_HZ(5),
.staticPriority = TASK_PRIORITY_IDLE
},
#endif
#endif
};

17
src/main/fc/rc_controls.c
View File

@ -34,6 +34,8 @@
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/camera_control.h"
#include "fc/config.h"
#include "fc/fc_core.h"
#include "fc/rc_controls.h"
@ -302,6 +304,21 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
}
#endif
#ifdef USE_CAMERA_CONTROL
if (rcSticks == THR_LO + YAW_CE + PIT_LO + ROL_CE) {
cameraControlKeyPress(CAMERA_CONTROL_KEY_ENTER, 0);
} else if (rcSticks == THR_CE + YAW_CE + PIT_CE + ROL_LO) {
cameraControlKeyPress(CAMERA_CONTROL_KEY_LEFT, 0);
} else if (rcSticks == THR_CE + YAW_CE + PIT_HI + ROL_CE) {
cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 0);
} else if (rcSticks == THR_CE + YAW_CE + PIT_CE + ROL_HI) {
cameraControlKeyPress(CAMERA_CONTROL_KEY_RIGHT, 0);
} else if (rcSticks == THR_CE + YAW_CE + PIT_LO + ROL_CE) {
cameraControlKeyPress(CAMERA_CONTROL_KEY_DOWN, 0);
} else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_CE) {
cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 2000);
}
#endif
}
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {

19
src/main/fc/settings.c
View File

@ -37,6 +37,7 @@
#include "config/parameter_group_ids.h"
#include "drivers/light_led.h"
#include "drivers/camera_control.h"
#include "fc/config.h"
#include "fc/controlrate_profile.h"
@ -203,6 +204,14 @@ static const char * const lookupTableOsdType[] = {
};
#endif
#ifdef USE_CAMERA_CONTROL
static const char * const lookupTableCameraControlMode[] = {
"HARDWARE_PWM",
"SOFTWARE_PWM",
"DAC"
};
#endif
static const char * const lookupTableSuperExpoYaw[] = {
"OFF", "ON", "ALWAYS"
};
@ -275,6 +284,9 @@ const lookupTableEntry_t lookupTables[] = {
{ lookupTableCrashRecovery, sizeof(lookupTableCrashRecovery) / sizeof(char *) },
#ifdef OSD
{ lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) },
#endif
#ifdef USE_CAMERA_CONTROL
{ lookupTableCameraControlMode, sizeof(lookupTableCameraControlMode) / sizeof(char *) },
#endif
{ lookupTableBusType, sizeof(lookupTableBusType) / sizeof(char *) },
};
@ -721,6 +733,13 @@ const clivalue_t valueTable[] = {
{ "dashboard_i2c_bus", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, I2CDEV_COUNT }, PG_DASHBOARD_CONFIG, offsetof(dashboardConfig_t, device) },
{ "dashboard_i2c_addr", VAR_UINT8 | MASTER_VALUE, .config.minmax = { I2C_ADDR7_MIN, I2C_ADDR7_MAX }, PG_DASHBOARD_CONFIG, offsetof(dashboardConfig_t, address) },
#endif
// PG_CAMERA_CONTROL_CONFIG
#ifdef USE_CAMERA_CONTROL
{ "camera_control_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CAMERA_CONTROL_MODE }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, mode) },
{ "camera_control_ref_voltage", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 200, 400 }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, refVoltage) },
{ "camera_control_key_delay", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 100, 500 }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, keyDelayMs) },
#endif
};
const uint16_t valueTableEntryCount = ARRAYLEN(valueTable);

3
src/main/fc/settings.h
View File

@ -59,6 +59,9 @@ typedef enum {
TABLE_CRASH_RECOVERY,
#ifdef OSD
TABLE_OSD,
#endif
#ifdef USE_CAMERA_CONTROL
TABLE_CAMERA_CONTROL_MODE,
#endif
TABLE_BUS_TYPE,
LOOKUP_TABLE_COUNT

2
src/main/msp/msp_protocol.h
View File

@ -212,6 +212,8 @@
#define MSP_SENSOR_CONFIG 96
#define MSP_SET_SENSOR_CONFIG 97
#define MSP_CAMERA_CONTROL 98
//
// OSD specific
//

3
src/main/scheduler/scheduler.h
View File

@ -110,6 +110,9 @@ typedef enum {
#ifdef VTX_CONTROL
TASK_VTXCTRL,
#endif
#ifdef USE_CAMERA_CONTROL
TASK_CAMCTRL,
#endif
#ifdef USE_RCSPLIT
TASK_RCSPLIT,

1
src/main/target/SITL/target.h
View File

@ -111,6 +111,7 @@
#undef VTX_CONTROL
#undef VTX_SMARTAUDIO
#undef VTX_TRAMP
#undef USE_CAMERA_CONTROL
#undef USE_I2C
#undef USE_SPI

2
src/main/target/STM32F3DISCOVERY/target.h
View File

@ -31,6 +31,8 @@
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE
#define CURRENT_TARGET_CPU_VOLTAGE 3.0
#define LED0_PIN PE8 // Blue LEDs - PE8/PE12
#define LED0_INVERTED
#define LED1_PIN PE10 // Orange LEDs - PE10/PE14

1
src/main/target/common_fc_pre.h
View File

@ -124,6 +124,7 @@
#define VTX_CONTROL
#define VTX_SMARTAUDIO
#define VTX_TRAMP
#define USE_CAMERA_CONTROL
#ifdef USE_SERIALRX_SPEKTRUM
#define USE_SPEKTRUM_BIND