From cc358dc4055f28c0ae309c51e90704f0684935c7 Mon Sep 17 00:00:00 2001 From: DieHertz Date: Thu, 18 May 2017 18:35:33 +0300 Subject: [PATCH 01/10] Implemented Camera Control using Hardware and Software PWM --- Makefile | 2 + src/main/config/parameter_group_ids.h | 3 +- src/main/drivers/camera_control.c | 238 ++++++++++++++++++++++ src/main/drivers/camera_control.h | 52 +++++ src/main/drivers/pwm_output.c | 36 ++-- src/main/drivers/pwm_output.h | 7 +- src/main/drivers/resource.c | 1 + src/main/drivers/resource.h | 1 + src/main/drivers/rx_pwm.c | 2 +- src/main/fc/cli.c | 4 + src/main/fc/fc_init.c | 5 + src/main/fc/fc_msp.c | 14 ++ src/main/fc/fc_tasks.c | 24 +++ src/main/fc/rc_controls.c | 1 - src/main/fc/settings.c | 19 ++ src/main/fc/settings.h | 3 + src/main/msp/msp_protocol.h | 2 + src/main/scheduler/scheduler.h | 3 + src/main/target/SITL/target.h | 1 + src/main/target/STM32F3DISCOVERY/target.h | 2 + src/main/target/common_fc_pre.h | 1 + 21 files changed, 399 insertions(+), 22 deletions(-) create mode 100644 src/main/drivers/camera_control.c create mode 100644 src/main/drivers/camera_control.h diff --git a/Makefile b/Makefile index f415d80fec..205dad6d3b 100644 --- a/Makefile +++ b/Makefile @@ -787,6 +787,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 \ @@ -887,6 +888,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 \ diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 7180b7e58e..d1bbc2184f 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -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) diff --git a/src/main/drivers/camera_control.c b/src/main/drivers/camera_control.c new file mode 100644 index 0000000000..7339e1f7cb --- /dev/null +++ b/src/main/drivers/camera_control.c @@ -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 . + */ + +#include + +#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 diff --git a/src/main/drivers/camera_control.h b/src/main/drivers/camera_control.h new file mode 100644 index 0000000000..674aa9905e --- /dev/null +++ b/src/main/drivers/camera_control.h @@ -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 . + */ + +#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); diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 9c05608fb7..66b70901c6 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -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; } } @@ -327,11 +327,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; } @@ -424,8 +424,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); } } @@ -453,7 +453,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; } } @@ -466,10 +466,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(IO_t io, 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 diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 5c2b3cf7ce..55a76ea3c8 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -131,7 +131,11 @@ typedef void pwmCompleteWriteFunc(uint8_t motorCount); // function pointer use typedef struct { volatile timCCR_t *ccr; - TIM_TypeDef *tim; + 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(IO_t io, 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); diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c index ef1c5d4bf8..13953e3189 100644 --- a/src/main/drivers/resource.c +++ b/src/main/drivers/resource.c @@ -65,5 +65,6 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = { "SPI_PREINIT", "RX_BIND_PLUG", "ESCSERIAL", + "CAMERA_CONTROL", }; diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index 9ed75318d5..f9ed25c94d 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -65,6 +65,7 @@ typedef enum { OWNER_SPI_PREINIT, OWNER_RX_BIND_PLUG, OWNER_ESCSERIAL, + OWNER_CAMERA_CONTROL, OWNER_TOTAL_COUNT } resourceOwner_e; diff --git a/src/main/drivers/rx_pwm.c b/src/main/drivers/rx_pwm.c index bdd17a094d..67376a978c 100644 --- a/src/main/drivers/rx_pwm.c +++ b/src/main/drivers/rx_pwm.c @@ -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; } diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 520d26ba1d..793740a13c 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -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" @@ -2864,6 +2865,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 }; static ioTag_t *getIoTag(const cliResourceValue_t value, uint8_t index) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 4314dc0cd1..3d3897c7b7 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -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); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 95cefe691b..87a0565cdf 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -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" @@ -1846,6 +1847,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(); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 5e1bb3b3b7..9ddfc92d9b 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -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 }; diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 8344326caa..0fdef9c593 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -301,7 +301,6 @@ void processRcStickPositions(throttleStatus_e throttleStatus) vtxDecrementChannel(); } #endif - } int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) { diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index b2742f3e89..10f7226848 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -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" @@ -228,6 +229,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" }; @@ -297,6 +306,9 @@ const lookupTableEntry_t lookupTables[] = { #ifdef OSD { lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) }, #endif +#ifdef USE_CAMERA_CONTROL + { lookupTableCameraControlMode, sizeof(lookupTableCameraControlMode) / sizeof(char *) }, +#endif }; const clivalue_t valueTable[] = { @@ -736,6 +748,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_ADDR8_MIN, I2C_ADDR8_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); diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index a9f05e5803..073b5fe8ff 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -59,6 +59,9 @@ typedef enum { TABLE_CRASH_RECOVERY, #ifdef OSD TABLE_OSD, +#endif +#ifdef USE_CAMERA_CONTROL + TABLE_CAMERA_CONTROL_MODE, #endif LOOKUP_TABLE_COUNT } lookupTableIndex_e; diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 3574f99ea8..ebaf1ab5bd 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -212,6 +212,8 @@ #define MSP_SENSOR_CONFIG 96 #define MSP_SET_SENSOR_CONFIG 97 +#define MSP_CAMERA_CONTROL 98 + // // OSD specific // diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index f943d950cb..771a26ddbc 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -110,6 +110,9 @@ typedef enum { #ifdef VTX_CONTROL TASK_VTXCTRL, #endif +#ifdef USE_CAMERA_CONTROL + TASK_CAMCTRL, +#endif #ifdef USE_RCSPLIT TASK_RCSPLIT, diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 68a5212b32..765c44e1a8 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -111,6 +111,7 @@ #undef VTX_CONTROL #undef VTX_SMARTAUDIO #undef VTX_TRAMP +#undef USE_CAMERA_CONTROL #undef USE_I2C #undef USE_SPI diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 24add1590b..521f909f58 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -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 diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 5ce914c9ea..80fc5d42d2 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -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 From 65476c506cc5c8aa9934fc5a07149a9a028d17b5 Mon Sep 17 00:00:00 2001 From: DieHertz Date: Thu, 18 May 2017 19:06:12 +0300 Subject: [PATCH 02/10] Added stick commands for camera control --- src/main/fc/rc_controls.c | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 0fdef9c593..5a7bec6ab1 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -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" @@ -301,6 +303,22 @@ void processRcStickPositions(throttleStatus_e throttleStatus) vtxDecrementChannel(); } #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) { From d1b331baea9de0487d1802a196113179c2851458 Mon Sep 17 00:00:00 2001 From: "brucesdad13@gmail.com" Date: Sun, 16 Jul 2017 23:46:48 -0400 Subject: [PATCH 03/10] * @mikeller asked if I would "[add] a warning message if targets are not built because of `SKIP_TARGETS`" to avoid confusion and frustration. I've put it at the beginning of the build process. - Charlie Stevenson --- Makefile | 1 + 1 file changed, 1 insertion(+) diff --git a/Makefile b/Makefile index 9988ce818f..871b581438 100644 --- a/Makefile +++ b/Makefile @@ -1342,6 +1342,7 @@ targets-group-rest: $(GROUP_OTHER_TARGETS) $(VALID_TARGETS): + $(V0) @echo "The following targets, listed in 'SKIP_TARGETS', will not be built:\r\n\t$(SKIP_TARGETS)" $(V0) @echo "" && \ echo "Building $@" && \ time $(MAKE) binary hex TARGET=$@ && \ From 9e73acf73b7814262922f14acbee030bb95db311 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Mon, 17 Jul 2017 19:01:14 +0800 Subject: [PATCH 04/10] Inverted when the SKIP_TARGET warning is being shown. --- Makefile | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/Makefile b/Makefile index 871b581438..5938679135 100644 --- a/Makefile +++ b/Makefile @@ -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 \ @@ -1342,11 +1341,13 @@ targets-group-rest: $(GROUP_OTHER_TARGETS) $(VALID_TARGETS): - $(V0) @echo "The following targets, listed in 'SKIP_TARGETS', will not be built:\r\n\t$(SKIP_TARGETS)" - $(V0) @echo "" && \ - echo "Building $@" && \ - time $(MAKE) binary hex TARGET=$@ && \ - echo "Building $@ succeeded." + $(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.") CLEAN_TARGETS = $(addprefix clean_,$(VALID_TARGETS) ) TARGETS_CLEAN = $(addsuffix _clean,$(VALID_TARGETS) ) From 9ea8789869caa89c0f60ff748fd4e8370a9739d6 Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 18 Jul 2017 15:08:51 +0900 Subject: [PATCH 05/10] For DYSF4PRO, use PC3 for RSSI ADC input The RSSI pad has direct connection to PC3. --- src/main/target/OMNIBUSF4/target.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 0b871810c5..07ac741147 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -199,7 +199,11 @@ #define USE_ADC #define CURRENT_METER_ADC_PIN PC1 // Direct from CRNT pad (part of onboard sensor for Pro) #define VBAT_ADC_PIN PC2 // 11:1 (10K + 1K) divider +#ifdef DYSF4PRO +#define RSSI_ADC_PIN PC3 // Direct from RSSI pad +#else #define RSSI_ADC_PIN PA0 // Direct from RSSI pad +#endif #define TRANSPONDER From 2636ffddb9c15fb05d6b5226243739cf1f14a304 Mon Sep 17 00:00:00 2001 From: Andrey Mironov Date: Tue, 18 Jul 2017 17:55:08 +0300 Subject: [PATCH 06/10] Renamed occurrences of CAMERA_CONTROL to USE_CAMERA_CONTROL --- src/main/fc/cli.c | 2 +- src/main/fc/fc_init.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 622602e19c..25d0c19544 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3032,7 +3032,7 @@ const cliResourceValue_t resourceTable[] = { #ifdef USE_ESCSERIAL { OWNER_ESCSERIAL, PG_ESCSERIAL_CONFIG, offsetof(escSerialConfig_t, ioTag), 0 }, #endif -#ifdef CAMERA_CONTROL +#ifdef USE_CAMERA_CONTROL { OWNER_CAMERA_CONTROL, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, ioTag), 0 }, #endif }; diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 3d3897c7b7..dab0336721 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -470,7 +470,7 @@ void init(void) rtc6705IOInit(); #endif -#ifdef CAMERA_CONTROL +#ifdef USE_CAMERA_CONTROL cameraControlInit(); #endif From 84b828a93c1c8eed14b281a4a32688c934b9f341 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 19 Jul 2017 00:50:05 +0900 Subject: [PATCH 07/10] Target configuration improvements 1. Add timer entries for UART1 pins so that unused side, more specifically TX side when Serial RX is used, can be used as a timer for other uses such as software serial. 2. Delete AVOID_UART1_FOR_PWM_PPM so that - OMNIBUSF4/F4SD users will have an option to use UART1 with SBUS/PPM jumper mod, and - DYSF4PRO users to provide UART1 even when PPM is used. 3. Tidy on S6_OUT (consolidation) --- src/main/target/OMNIBUSF4/target.c | 6 +++--- src/main/target/OMNIBUSF4/target.h | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index e194ed6f37..27bbf82d88 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -46,11 +46,11 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { #if defined(OMNIBUSF4SD) DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S5_OUT - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S6_OUT DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // LED strip for F4 V2 / F4-Pro-0X and later (RCD_CS for F4) #else DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // S5_OUT - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S6_OUT #endif + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S6_OUT + DEF_TIM(TIM1, CH2, PA9, TIM_USE_NONE, TIMER_OUTPUT_NONE, 0), // UART1_TX + DEF_TIM(TIM1, CH3, PA10, TIM_USE_NONE, TIMER_OUTPUT_NONE, 0), // UART1_RX }; - diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 07ac741147..6cf9fecfb2 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -210,7 +210,7 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define DEFAULT_FEATURES (FEATURE_OSD) -#define AVOID_UART1_FOR_PWM_PPM + #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13))) @@ -219,8 +219,8 @@ #define TARGET_IO_PORTD BIT(2) #ifdef OMNIBUSF4SD -#define USABLE_TIMER_CHANNEL_COUNT 13 +#define USABLE_TIMER_CHANNEL_COUNT 15 #else -#define USABLE_TIMER_CHANNEL_COUNT 12 +#define USABLE_TIMER_CHANNEL_COUNT 14 #endif #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) From 9b189865d1d192a15f201b880218331dcd13abb7 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Sat, 15 Jul 2017 08:25:39 +0200 Subject: [PATCH 08/10] MSP changes for the new Power & Battery tab in configurator --- src/main/fc/fc_msp.c | 93 +++++++++++++++++++++++++++++--------------- 1 file changed, 61 insertions(+), 32 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 666319f9c3..bb836d1cf4 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -706,7 +706,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce case MSP_VOLTAGE_METERS: // write out id and voltage meter values, once for each meter we support - for (int i = 0; i < supportedVoltageMeterCount; i++) { + for (int i = 0; i < supportedVoltageMeterCount - (12-getMotorCount()); i++) { voltageMeter_t meter; uint8_t id = (uint8_t)voltageMeterIds[i]; @@ -719,7 +719,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce case MSP_CURRENT_METERS: // write out id and current meter values, once for each meter we support - for (int i = 0; i < supportedCurrentMeterCount; i++) { + for (int i = 0; i < supportedCurrentMeterCount - (12-getMotorCount()); i++) { currentMeter_t meter; uint8_t id = (uint8_t)currentMeterIds[i]; @@ -2131,46 +2131,75 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #endif case MSP_SET_VOLTAGE_METER_CONFIG: { - int id = sbufReadU8(src); + int8_t config_count = sbufReadU8(src); - // - // find and configure an ADC voltage sensor - // - int voltageSensorADCIndex; - for (voltageSensorADCIndex = 0; voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC; voltageSensorADCIndex++) { - if (id == voltageMeterADCtoIDMap[voltageSensorADCIndex]) { - break; + while (config_count > 0) { + + int8_t subframe_length = sbufReadU8(src); + if (subframe_length > 4) { + for (int8_t j = 0; j < subframe_length; j++) { + sbufReadU8(src); + } + } else { + int8_t id = sbufReadU8(src); + + // + // find and configure an ADC voltage sensor + // + int8_t voltageSensorADCIndex; + for (voltageSensorADCIndex = 0; voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC; voltageSensorADCIndex++) { + if (id == voltageMeterADCtoIDMap[voltageSensorADCIndex]) { + break; + } + } + + if (voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC) { + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatscale = sbufReadU8(src); + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivval = sbufReadU8(src); + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivmultiplier = sbufReadU8(src); + } else { + // if we had any other types of voltage sensor to configure, this is where we'd do it. + sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); + } } - } - - if (voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC) { - voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatscale = sbufReadU8(src); - voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivval = sbufReadU8(src); - voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivmultiplier = sbufReadU8(src); - } else { - // if we had any other types of voltage sensor to configure, this is where we'd do it. - return -1; + config_count--; } break; } case MSP_SET_CURRENT_METER_CONFIG: { - int id = sbufReadU8(src); + int8_t config_count = sbufReadU8(src); - switch (id) { - case CURRENT_METER_ID_BATTERY_1: - currentSensorADCConfigMutable()->scale = sbufReadU16(src); - currentSensorADCConfigMutable()->offset = sbufReadU16(src); - break; + while (config_count > 0) { + + int8_t subframe_length = sbufReadU8(src); + if (subframe_length > 5) { + for (int8_t j = 0; j < subframe_length; j++) { + sbufReadU8(src); + } + } else { + int id = sbufReadU8(src); + + switch (id) { + case CURRENT_METER_ID_BATTERY_1: + currentSensorADCConfigMutable()->scale = sbufReadU16(src); + currentSensorADCConfigMutable()->offset = sbufReadU16(src); + break; #ifdef USE_VIRTUAL_CURRENT_METER - case CURRENT_METER_ID_VIRTUAL_1: - currentSensorVirtualConfigMutable()->scale = sbufReadU16(src); - currentSensorVirtualConfigMutable()->offset = sbufReadU16(src); - break; + case CURRENT_METER_ID_VIRTUAL_1: + currentSensorVirtualConfigMutable()->scale = sbufReadU16(src); + currentSensorVirtualConfigMutable()->offset = sbufReadU16(src); + break; #endif - - default: - return -1; + default: + sbufReadU16(src); + sbufReadU16(src); + break; + } + } + config_count--; } break; From f61bb3aac8759b6120585017f5a9e8b3955bb274 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Sun, 16 Jul 2017 21:17:21 +0200 Subject: [PATCH 09/10] Replaced magic number with define VOLTAGE_METER_ID_ESC_COUNT --- src/main/fc/fc_msp.c | 4 ++-- src/main/sensors/voltage.h | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index bb836d1cf4..5b2c0facb1 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -706,7 +706,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce case MSP_VOLTAGE_METERS: // write out id and voltage meter values, once for each meter we support - for (int i = 0; i < supportedVoltageMeterCount - (12-getMotorCount()); i++) { + for (int i = 0; i < supportedVoltageMeterCount - (VOLTAGE_METER_ID_ESC_COUNT - getMotorCount()); i++) { voltageMeter_t meter; uint8_t id = (uint8_t)voltageMeterIds[i]; @@ -719,7 +719,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce case MSP_CURRENT_METERS: // write out id and current meter values, once for each meter we support - for (int i = 0; i < supportedCurrentMeterCount - (12-getMotorCount()); i++) { + for (int i = 0; i < supportedCurrentMeterCount - (VOLTAGE_METER_ID_ESC_COUNT - getMotorCount()); i++) { currentMeter_t meter; uint8_t id = (uint8_t)currentMeterIds[i]; diff --git a/src/main/sensors/voltage.h b/src/main/sensors/voltage.h index 4011e67141..0c8d3d6823 100644 --- a/src/main/sensors/voltage.h +++ b/src/main/sensors/voltage.h @@ -60,6 +60,8 @@ typedef enum { #define MAX_VOLTAGE_SENSOR_ADC 1 // VBAT - some boards have external, 12V, 9V and 5V meters. #endif +#define VOLTAGE_METER_ID_ESC_COUNT 12 + typedef enum { VOLTAGE_SENSOR_ADC_VBAT = 0, VOLTAGE_SENSOR_ADC_12V = 1, From 3c917794bffdeaf2cb2151ee2c077479a928bb09 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Tue, 18 Jul 2017 22:20:52 +0200 Subject: [PATCH 10/10] Changes according review comments & fixed linker error SPRACINGF3OSD --- src/main/fc/fc_msp.c | 112 +++++++++++++++++++------------------------ 1 file changed, 48 insertions(+), 64 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 5b2c0facb1..7799082fb7 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -704,9 +704,14 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce break; } - case MSP_VOLTAGE_METERS: + case MSP_VOLTAGE_METERS: { // write out id and voltage meter values, once for each meter we support - for (int i = 0; i < supportedVoltageMeterCount - (VOLTAGE_METER_ID_ESC_COUNT - getMotorCount()); i++) { + uint8_t count = supportedVoltageMeterCount; +#ifndef USE_OSD_SLAVE + count = supportedVoltageMeterCount - (VOLTAGE_METER_ID_ESC_COUNT - getMotorCount()); +#endif + + for (int i = 0; i < count; i++) { voltageMeter_t meter; uint8_t id = (uint8_t)voltageMeterIds[i]; @@ -716,10 +721,15 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce sbufWriteU8(dst, (uint8_t)constrain(meter.filtered, 0, 255)); } break; + } - case MSP_CURRENT_METERS: + case MSP_CURRENT_METERS: { // write out id and current meter values, once for each meter we support - for (int i = 0; i < supportedCurrentMeterCount - (VOLTAGE_METER_ID_ESC_COUNT - getMotorCount()); i++) { + uint8_t count = supportedVoltageMeterCount; +#ifndef USE_OSD_SLAVE + count = supportedVoltageMeterCount - (VOLTAGE_METER_ID_ESC_COUNT - getMotorCount()); +#endif + for (int i = 0; i < count; i++) { currentMeter_t meter; uint8_t id = (uint8_t)currentMeterIds[i]; @@ -730,6 +740,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce sbufWriteU16(dst, (uint16_t)constrain(meter.amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps (mA). Negative range is truncated to zero } break; + } case MSP_VOLTAGE_METER_CONFIG: // by using a sensor type and a sub-frame length it's possible to configure any type of voltage meter, @@ -2131,77 +2142,50 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #endif case MSP_SET_VOLTAGE_METER_CONFIG: { - int8_t config_count = sbufReadU8(src); + int8_t id = sbufReadU8(src); - while (config_count > 0) { - - int8_t subframe_length = sbufReadU8(src); - if (subframe_length > 4) { - for (int8_t j = 0; j < subframe_length; j++) { - sbufReadU8(src); - } - } else { - int8_t id = sbufReadU8(src); - - // - // find and configure an ADC voltage sensor - // - int8_t voltageSensorADCIndex; - for (voltageSensorADCIndex = 0; voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC; voltageSensorADCIndex++) { - if (id == voltageMeterADCtoIDMap[voltageSensorADCIndex]) { - break; - } - } - - if (voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC) { - voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatscale = sbufReadU8(src); - voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivval = sbufReadU8(src); - voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivmultiplier = sbufReadU8(src); - } else { - // if we had any other types of voltage sensor to configure, this is where we'd do it. - sbufReadU8(src); - sbufReadU8(src); - sbufReadU8(src); - } + // + // find and configure an ADC voltage sensor + // + int8_t voltageSensorADCIndex; + for (voltageSensorADCIndex = 0; voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC; voltageSensorADCIndex++) { + if (id == voltageMeterADCtoIDMap[voltageSensorADCIndex]) { + break; } - config_count--; + } + + if (voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC) { + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatscale = sbufReadU8(src); + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivval = sbufReadU8(src); + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivmultiplier = sbufReadU8(src); + } else { + // if we had any other types of voltage sensor to configure, this is where we'd do it. + sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); } break; } case MSP_SET_CURRENT_METER_CONFIG: { - int8_t config_count = sbufReadU8(src); + int id = sbufReadU8(src); - while (config_count > 0) { - - int8_t subframe_length = sbufReadU8(src); - if (subframe_length > 5) { - for (int8_t j = 0; j < subframe_length; j++) { - sbufReadU8(src); - } - } else { - int id = sbufReadU8(src); - - switch (id) { - case CURRENT_METER_ID_BATTERY_1: - currentSensorADCConfigMutable()->scale = sbufReadU16(src); - currentSensorADCConfigMutable()->offset = sbufReadU16(src); - break; + switch (id) { + case CURRENT_METER_ID_BATTERY_1: + currentSensorADCConfigMutable()->scale = sbufReadU16(src); + currentSensorADCConfigMutable()->offset = sbufReadU16(src); + break; #ifdef USE_VIRTUAL_CURRENT_METER - case CURRENT_METER_ID_VIRTUAL_1: - currentSensorVirtualConfigMutable()->scale = sbufReadU16(src); - currentSensorVirtualConfigMutable()->offset = sbufReadU16(src); - break; + case CURRENT_METER_ID_VIRTUAL_1: + currentSensorVirtualConfigMutable()->scale = sbufReadU16(src); + currentSensorVirtualConfigMutable()->offset = sbufReadU16(src); + break; #endif - default: - sbufReadU16(src); - sbufReadU16(src); - break; - } - } - config_count--; + default: + sbufReadU16(src); + sbufReadU16(src); + break; } - break; }