From 2a12b809a0d937c5b9cf1803c7210952138ae7bb Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Wed, 14 Jun 2017 12:30:37 +0200 Subject: [PATCH 01/18] Unit tests with clang warnings slips by unnoticed, added -Werror compiler option. --- src/test/Makefile | 1 + 1 file changed, 1 insertion(+) diff --git a/src/test/Makefile b/src/test/Makefile index 7528b6f90d..f887e6bd50 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -192,6 +192,7 @@ COMMON_FLAGS = \ -g \ -Wall \ -Wextra \ + -Werror \ -ggdb3 \ -pthread \ -O0 \ From c7f62e47a6a306846c1ec15174707ed379b5d4f9 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 16 Jun 2017 18:39:16 +0900 Subject: [PATCH 02/18] Automatic spektrum bind pin determination --- src/main/drivers/resource.c | 3 +- src/main/drivers/resource.h | 1 + src/main/drivers/serial.h | 3 + src/main/drivers/serial_uart_hal.c | 2 + src/main/drivers/serial_uart_stm32f30x.c | 6 +- src/main/fc/cli.c | 4 + src/main/fc/config.h | 1 - src/main/fc/fc_init.c | 2 +- src/main/fc/settings.c | 2 +- src/main/rx/msp.c | 1 + src/main/rx/nrf24_cx10.c | 1 + src/main/rx/nrf24_h8_3d.c | 1 + src/main/rx/nrf24_inav.c | 1 + src/main/rx/nrf24_syma.c | 1 + src/main/rx/nrf24_v202.c | 1 + src/main/rx/rx.c | 10 +++ src/main/rx/rx.h | 2 + src/main/rx/spektrum.c | 89 +++++++++++++++-------- src/main/target/FRSKYF3/config.c | 1 + src/main/target/FRSKYF4/config.c | 1 + src/main/target/OMNIBUS/target.h | 7 +- src/test/unit/flight_failsafe_unittest.cc | 1 + src/test/unit/ledstrip_unittest.cc | 1 + src/test/unit/rx_ranges_unittest.cc | 1 + src/test/unit/rx_rx_unittest.cc | 1 + 25 files changed, 102 insertions(+), 42 deletions(-) diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c index 06ede3fdf9..0bdf92ae76 100644 --- a/src/main/drivers/resource.c +++ b/src/main/drivers/resource.c @@ -61,6 +61,7 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = { "LED_STRIP", "TRANSPONDER", "VTX", - "COMPASS_CS" + "COMPASS_CS", + "RX_BIND_PLUG", }; diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index f91aa682e7..aae4598151 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -62,6 +62,7 @@ typedef enum { OWNER_TRANSPONDER, OWNER_VTX, OWNER_COMPASS_CS, + OWNER_RX_BIND_PLUG, OWNER_TOTAL_COUNT } resourceOwner_e; diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h index d31d6638ef..d09e37d0ff 100644 --- a/src/main/drivers/serial.h +++ b/src/main/drivers/serial.h @@ -18,6 +18,7 @@ #pragma once #include "drivers/io.h" +#include "config/parameter_group.h" typedef enum portMode_t { MODE_RX = 1 << 0, @@ -86,6 +87,8 @@ typedef struct serialPinConfig_s { ioTag_t ioTagInverter[SERIAL_PORT_MAX_INDEX]; } serialPinConfig_t; +PG_DECLARE(serialPinConfig_t, serialPinConfig); + struct serialPortVTable { void (*serialWrite)(serialPort_t *instance, uint8_t ch); diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c index a21ab96868..fa213eeab0 100644 --- a/src/main/drivers/serial_uart_hal.c +++ b/src/main/drivers/serial_uart_hal.c @@ -58,6 +58,8 @@ static void usartConfigurePinInversion(uartPort_t *uartPort) { } } +// XXX uartReconfigure does not handle resource management properly. + void uartReconfigure(uartPort_t *uartPort) { /*RCC_PeriphCLKInitTypeDef RCC_PeriphClkInit; diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index 8dd877c940..881d634286 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -184,7 +184,7 @@ void serialUARTInitIO(IO_t txIO, IO_t rxIO, portMode_t mode, portOptions_t optio ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP)) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP ); - IOInit(txIO, OWNER_SERIAL_TX, index); + IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(index)); IOConfigGPIOAF(txIO, ioCfg, af); if (!(options & SERIAL_INVERTED)) @@ -192,12 +192,12 @@ void serialUARTInitIO(IO_t txIO, IO_t rxIO, portMode_t mode, portOptions_t optio } else { ioConfig_t ioCfg = IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, (options & SERIAL_INVERTED) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP); if ((mode & MODE_TX) && txIO) { - IOInit(txIO, OWNER_SERIAL_TX, index); + IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(index)); IOConfigGPIOAF(txIO, ioCfg, af); } if ((mode & MODE_RX) && rxIO) { - IOInit(rxIO, OWNER_SERIAL_RX, index); + IOInit(rxIO, OWNER_SERIAL_RX, RESOURCE_INDEX(index)); IOConfigGPIOAF(rxIO, ioCfg, af); } } diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index b2adcfbbee..16ecfc6579 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2755,6 +2755,10 @@ const cliResourceValue_t resourceTable[] = { { OWNER_I2C_SCL, PG_I2C_CONFIG, offsetof(i2cConfig_t, ioTagScl[0]), I2CDEV_COUNT }, { OWNER_I2C_SDA, PG_I2C_CONFIG, offsetof(i2cConfig_t, ioTagSda[0]), I2CDEV_COUNT }, #endif +#ifdef USE_SPEKTRUM_BIND + { OWNER_RX_BIND, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_bind_pin_override_ioTag), 0 }, + { OWNER_RX_BIND_PLUG, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_bind_plug_ioTag), 0 }, +#endif }; static ioTag_t *getIoTag(const cliResourceValue_t value, uint8_t index) diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 4ced00ee29..a1b96a08a0 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -91,7 +91,6 @@ PG_DECLARE(ppmConfig_t, ppmConfig); PG_DECLARE(pwmConfig_t, pwmConfig); PG_DECLARE(vcdProfile_t, vcdProfile); PG_DECLARE(sdcardConfig_t, sdcardConfig); -PG_DECLARE(serialPinConfig_t, serialPinConfig); struct pidProfile_s; extern struct pidProfile_s *currentPidProfile; diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 53a72e11e2..c37e3687d2 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -257,7 +257,7 @@ void init(void) } #endif -#ifdef SPEKTRUM_BIND_PIN +#ifdef USE_SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (rxConfig()->serialrx_provider) { case SERIALRX_SPEKTRUM1024: diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 6907ab8058..dbb091f20e 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -354,7 +354,7 @@ const clivalue_t valueTable[] = { { "serialrx_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_SERIAL_RX }, PG_RX_CONFIG, offsetof(rxConfig_t, serialrx_provider) }, { "sbus_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, sbus_inversion) }, #endif -#ifdef SPEKTRUM_BIND_PIN +#ifdef USE_SPEKTRUM_BIND { "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX}, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind) }, { "spektrum_sat_bind_autoreset",VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 1 }, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind_autoreset) }, #endif diff --git a/src/main/rx/msp.c b/src/main/rx/msp.c index 5841182e2c..4a009fa153 100644 --- a/src/main/rx/msp.c +++ b/src/main/rx/msp.c @@ -24,6 +24,7 @@ #include "common/utils.h" +#include "drivers/io.h" #include "rx/rx.h" #include "rx/msp.h" diff --git a/src/main/rx/nrf24_cx10.c b/src/main/rx/nrf24_cx10.c index 280d8398d1..318f02853e 100644 --- a/src/main/rx/nrf24_cx10.c +++ b/src/main/rx/nrf24_cx10.c @@ -28,6 +28,7 @@ #include "build/build_config.h" +#include "drivers/io.h" #include "drivers/rx_nrf24l01.h" #include "drivers/rx_xn297.h" #include "drivers/time.h" diff --git a/src/main/rx/nrf24_h8_3d.c b/src/main/rx/nrf24_h8_3d.c index 855ee5eb3f..54a9938f24 100644 --- a/src/main/rx/nrf24_h8_3d.c +++ b/src/main/rx/nrf24_h8_3d.c @@ -30,6 +30,7 @@ #include "common/utils.h" +#include "drivers/io.h" #include "drivers/rx_nrf24l01.h" #include "drivers/rx_xn297.h" #include "drivers/time.h" diff --git a/src/main/rx/nrf24_inav.c b/src/main/rx/nrf24_inav.c index 766d0b4eba..c3ba65326d 100644 --- a/src/main/rx/nrf24_inav.c +++ b/src/main/rx/nrf24_inav.c @@ -28,6 +28,7 @@ #include "common/utils.h" +#include "drivers/io.h" #include "drivers/rx_nrf24l01.h" #include "drivers/time.h" diff --git a/src/main/rx/nrf24_syma.c b/src/main/rx/nrf24_syma.c index b6eb3a16f2..2d0cdc3fdf 100644 --- a/src/main/rx/nrf24_syma.c +++ b/src/main/rx/nrf24_syma.c @@ -28,6 +28,7 @@ #include "build/build_config.h" +#include "drivers/io.h" #include "drivers/rx_nrf24l01.h" #include "drivers/time.h" diff --git a/src/main/rx/nrf24_v202.c b/src/main/rx/nrf24_v202.c index 0342b7b524..9de7865e83 100644 --- a/src/main/rx/nrf24_v202.c +++ b/src/main/rx/nrf24_v202.c @@ -30,6 +30,7 @@ #include "common/utils.h" +#include "drivers/io.h" #include "drivers/rx_nrf24l01.h" #include "drivers/time.h" diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index c8deb8010c..a93a6f5f39 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -107,6 +107,14 @@ static uint8_t rcSampleIndex = 0; #define RX_MAX_USEC 2115 #define RX_MID_USEC 1500 +#ifndef SPEKTRUM_BIND_PIN +#define SPEKTRUM_BIND_PIN NONE +#endif + +#ifndef BINDPLUG_PIN +#define BINDPLUG_PIN NONE +#endif + PG_REGISTER_WITH_RESET_FN(rxConfig_t, rxConfig, PG_RX_CONFIG, 0); void pgResetFn_rxConfig(rxConfig_t *rxConfig) { @@ -115,6 +123,8 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig) .serialrx_provider = SERIALRX_PROVIDER, .rx_spi_protocol = RX_SPI_DEFAULT_PROTOCOL, .sbus_inversion = 1, + .spektrum_bind_pin_override_ioTag = IO_TAG(SPEKTRUM_BIND_PIN), + .spektrum_bind_plug_ioTag = IO_TAG(BINDPLUG_PIN), .spektrum_sat_bind = 0, .spektrum_sat_bind_autoreset = 1, .midrc = RX_MID_USEC, diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index e08f5f0720..48772e0d16 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -122,6 +122,8 @@ typedef struct rxConfig_s { uint8_t rx_spi_protocol; // type of nrf24 protocol (0 = v202 250kbps). Must be enabled by FEATURE_RX_NRF24 first. uint32_t rx_spi_id; uint8_t rx_spi_rf_channel_count; + ioTag_t spektrum_bind_pin_override_ioTag; + ioTag_t spektrum_bind_plug_ioTag; uint8_t spektrum_sat_bind; // number of bind pulses for Spektrum satellite receivers uint8_t spektrum_sat_bind_autoreset; // whenever we will reset (exit) binding mode after hard reboot uint8_t rssi_channel; diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 0a9adb88ff..105612e35e 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -77,13 +77,6 @@ static volatile uint8_t spekFrame[SPEK_FRAME_SIZE]; static rxRuntimeConfig_t *rxRuntimeConfigPtr; static serialPort_t *serialPort; -#ifdef SPEKTRUM_BIND_PIN -static IO_t BindPin = DEFIO_IO(NONE); -#ifdef BINDPLUG_PIN -static IO_t BindPlug = DEFIO_IO(NONE); -#endif -#endif - static uint8_t telemetryBuf[SRXL_FRAME_SIZE_MAX]; static uint8_t telemetryBufLen = 0; @@ -180,19 +173,22 @@ static uint16_t spektrumReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint return data; } -#ifdef SPEKTRUM_BIND_PIN +#ifdef USE_SPEKTRUM_BIND bool spekShouldBind(uint8_t spektrum_sat_bind) { -#ifdef BINDPLUG_PIN - BindPlug = IOGetByTag(IO_TAG(BINDPLUG_PIN)); - IOInit(BindPlug, OWNER_RX_BIND, 0); - IOConfigGPIO(BindPlug, IOCFG_IPU); +#ifdef USE_SPEKTRUM_BIND_PLUG + IO_t BindPlug = IOGetByTag(rxConfig()->spektrum_bind_plug_ioTag); - // Check status of bind plug and exit if not active - delayMicroseconds(10); // allow configuration to settle - if (IORead(BindPlug)) { - return false; + if (BindPlug) { + IOInit(BindPlug, OWNER_RX_BIND, 0); + IOConfigGPIO(BindPlug, IOCFG_IPU); + + // Check status of bind plug and exit if not active + delayMicroseconds(10); // allow configuration to settle + if (IORead(BindPlug)) { + return false; + } } #endif @@ -202,6 +198,7 @@ bool spekShouldBind(uint8_t spektrum_sat_bind) spektrum_sat_bind > SPEKTRUM_SAT_BIND_MAX ); } + /* spektrumBind function ported from Baseflight. It's used to bind satellite receiver to TX. * Function must be called immediately after startup so that we don't miss satellite bind window. * Known parameters. Tested with DSMX satellite and DX8 radio. Framerate (11ms or 22ms) must be selected from TX. @@ -210,52 +207,80 @@ bool spekShouldBind(uint8_t spektrum_sat_bind) */ void spektrumBind(rxConfig_t *rxConfig) { - int i; if (!spekShouldBind(rxConfig->spektrum_sat_bind)) { return; } + ioTag_t bindPin; + + if (rxConfig->spektrum_bind_pin_override_ioTag) { + bindPin = rxConfig->spektrum_bind_pin_override_ioTag; + } else { + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + if (!portConfig) { + return; + } + + int index = SERIAL_PORT_IDENTIFIER_TO_INDEX(portConfig->identifier); + ioTag_t txPin = serialPinConfig()->ioTagTx[index]; + ioTag_t rxPin = serialPinConfig()->ioTagRx[index]; + + switch (rxConfig->serialrx_provider) { + case SERIALRX_SRXL: +#ifdef TELEMETRY + if (feature(FEATURE_TELEMETRY) && !telemetryCheckRxPortShared(portConfig)) { + bindPin = txPin; + } + break; +#endif + default: + bindPin = rxPin; + } + + if (!bindPin) { + return; + } + } + + IO_t bindIO = IOGetByTag(bindPin); + + IOInit(bindIO, OWNER_RX_BIND, 0); + IOConfigGPIO(bindIO, IOCFG_OUT_PP); + LED1_ON; - BindPin = IOGetByTag(IO_TAG(SPEKTRUM_BIND_PIN)); - IOInit(BindPin, OWNER_RX_BIND, 0); - IOConfigGPIO(BindPin, IOCFG_OUT_PP); - // RX line, set high - IOWrite(BindPin, true); + IOWrite(bindIO, true); // Bind window is around 20-140ms after powerup delay(60); LED1_OFF; - for (i = 0; i < rxConfig->spektrum_sat_bind; i++) { - + for (int i = 0; i < rxConfig->spektrum_sat_bind; i++) { LED0_OFF; LED2_OFF; // RX line, drive low for 120us - IOWrite(BindPin, false); + IOWrite(bindIO, false); delayMicroseconds(120); LED0_ON; LED2_ON; // RX line, drive high for 120us - IOWrite(BindPin, true); + IOWrite(bindIO, true); delayMicroseconds(120); } -#ifndef BINDPLUG_PIN // If we came here as a result of hard reset (power up, with spektrum_sat_bind set), then reset it back to zero and write config // Don't reset if hardware bind plug is present // Reset only when autoreset is enabled - if (rxConfig->spektrum_sat_bind_autoreset == 1 && !isMPUSoftReset()) { + + if (!rxConfig->spektrum_bind_plug_ioTag && rxConfig->spektrum_sat_bind_autoreset == 1 && !isMPUSoftReset()) { rxConfig->spektrum_sat_bind = 0; saveConfigAndNotify(); } -#endif - } -#endif // SPEKTRUM_BIND_PIN +#endif // USE_SPEKTRUM_BIND bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { @@ -276,7 +301,7 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig switch (rxConfig->serialrx_provider) { case SERIALRX_SRXL: #ifdef TELEMETRY - srxlEnabled = (feature(FEATURE_TELEMETRY) && !portShared && rxConfig->serialrx_provider == SERIALRX_SRXL); + srxlEnabled = (feature(FEATURE_TELEMETRY) && !portShared); #endif case SERIALRX_SPEKTRUM2048: // 11 bit frames diff --git a/src/main/target/FRSKYF3/config.c b/src/main/target/FRSKYF3/config.c index 1ba41e1a5c..d39e3c48af 100644 --- a/src/main/target/FRSKYF3/config.c +++ b/src/main/target/FRSKYF3/config.c @@ -19,6 +19,7 @@ #include #ifdef TARGET_CONFIG +#include "drivers/io.h" #include "rx/rx.h" #include "io/serial.h" diff --git a/src/main/target/FRSKYF4/config.c b/src/main/target/FRSKYF4/config.c index 1ba41e1a5c..d39e3c48af 100644 --- a/src/main/target/FRSKYF4/config.c +++ b/src/main/target/FRSKYF4/config.c @@ -19,6 +19,7 @@ #include #ifdef TARGET_CONFIG +#include "drivers/io.h" #include "rx/rx.h" #include "io/serial.h" diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index cb712f652b..b2ef6253f5 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -169,9 +169,10 @@ //#define AVOID_UART3_FOR_PWM_PPM // Disable this for using UART3 -#define SPEKTRUM_BIND_PIN UART3_RX_PIN - -#define BINDPLUG_PIN PB0 +#define USE_SPEKTRUM_BIND +#define USE_SPEKTRUM_BIND_PLUG +//#define SPEKTRUM_BIND_PIN NONE // Place holder +//#define BINDPLUG_PIN NONE // Place holder #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index 5976c96b61..f2ed5371da 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/src/test/unit/flight_failsafe_unittest.cc @@ -35,6 +35,7 @@ extern "C" { #include "io/beeper.h" #include "fc/rc_controls.h" + #include "drivers/io.h" #include "rx/rx.h" #include "flight/failsafe.h" } diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index 62ffa90f43..ed63fe149e 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -31,6 +31,7 @@ extern "C" { #include "sensors/battery.h" + #include "drivers/io.h" #include "rx/rx.h" #include "fc/config.h" diff --git a/src/test/unit/rx_ranges_unittest.cc b/src/test/unit/rx_ranges_unittest.cc index 59a4ecf8d4..d1ea8489e8 100644 --- a/src/test/unit/rx_ranges_unittest.cc +++ b/src/test/unit/rx_ranges_unittest.cc @@ -23,6 +23,7 @@ extern "C" { #include "platform.h" + #include "drivers/io.h" #include "common/maths.h" #include "config/parameter_group_ids.h" #include "fc/rc_controls.h" diff --git a/src/test/unit/rx_rx_unittest.cc b/src/test/unit/rx_rx_unittest.cc index cbaa95b2d9..d3b58c615f 100644 --- a/src/test/unit/rx_rx_unittest.cc +++ b/src/test/unit/rx_rx_unittest.cc @@ -23,6 +23,7 @@ extern "C" { #include "platform.h" + #include "drivers/io.h" #include "rx/rx.h" #include "fc/rc_controls.h" #include "common/maths.h" From 3d2fe5e8de060208a328025a7972b9f38ac2bf46 Mon Sep 17 00:00:00 2001 From: jflyper Date: Sat, 17 Jun 2017 11:12:50 +0900 Subject: [PATCH 03/18] Enable spektrum bind for SRXL --- src/main/fc/fc_init.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index c37e3687d2..382ca39234 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -262,6 +262,7 @@ void init(void) switch (rxConfig()->serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: + case SERIALRX_SRXL: // Spektrum satellite binding if enabled on startup. // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. // The rest of Spektrum initialization will happen later - via spektrumInit() From 7155a3e6a59236d8e26bbdb535b2fc8c47046c99 Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 19 Jun 2017 00:24:14 +0900 Subject: [PATCH 04/18] Comment tidy --- src/main/rx/spektrum.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 105612e35e..27c2578505 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -211,6 +211,7 @@ void spektrumBind(rxConfig_t *rxConfig) return; } + // Determine a pin to use ioTag_t bindPin; if (rxConfig->spektrum_bind_pin_override_ioTag) { @@ -225,6 +226,7 @@ void spektrumBind(rxConfig_t *rxConfig) ioTag_t txPin = serialPinConfig()->ioTagTx[index]; ioTag_t rxPin = serialPinConfig()->ioTagRx[index]; + // Take care half-duplex case switch (rxConfig->serialrx_provider) { case SERIALRX_SRXL: #ifdef TELEMETRY From 1fe839331e110c7b11a758ab41062a8e03ba633a Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 19 Jun 2017 19:34:32 +0900 Subject: [PATCH 05/18] Enable USE_SPEKTRUM_BIND{,_PIN} for relevant targets FLASH_SIZE > 128 and USE_SERIALRX_SPEKTRUM. --- src/main/target/common_fc_pre.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 9db23d8599..47a6c182e6 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -122,6 +122,11 @@ #define VTX_CONTROL #define VTX_SMARTAUDIO #define VTX_TRAMP + +#ifdef USE_SERIALRX_SPEKTRUM +#define USE_SPEKTRUM_BIND +#define USE_SPEKTRUM_BINDPIN +#endif #endif #if (FLASH_SIZE > 256) From 4451ae5fe95d151ce9bf83b9ec3e294a7b20c914 Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 19 Jun 2017 20:46:49 +0900 Subject: [PATCH 06/18] Exclude SITL case --- src/main/fc/fc_init.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index c90fdd309f..45dc3c6fcc 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -259,7 +259,7 @@ void init(void) } #endif -#ifdef USE_SPEKTRUM_BIND +#if defined(USE_SPEKTRUM_BIND) && !defined(SITL) if (feature(FEATURE_RX_SERIAL)) { switch (rxConfig()->serialrx_provider) { case SERIALRX_SPEKTRUM1024: From e2caf8831fb1493fac28b1fa1a2ed78ad36a9238 Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 19 Jun 2017 22:36:31 +0900 Subject: [PATCH 07/18] Target cleanup --- src/main/target/AIR32/target.h | 2 -- src/main/target/AIRHEROF3/target.h | 2 -- src/main/target/ALIENFLIGHTF1/target.h | 4 ++-- src/main/target/ALIENFLIGHTF3/target.h | 2 -- src/main/target/ALIENFLIGHTF4/target.h | 2 -- src/main/target/ALIENFLIGHTNGF7/target.h | 2 -- src/main/target/BETAFLIGHTF3/target.h | 2 -- src/main/target/BLUEJAYF4/target.h | 2 -- src/main/target/CC3D/target.h | 2 -- src/main/target/CJMCU/target.h | 2 +- src/main/target/CLRACINGF4/target.h | 2 -- src/main/target/CLRACINGF7/target.h | 1 - src/main/target/DOGE/target.h | 2 -- src/main/target/F4BY/target.h | 2 -- src/main/target/FF_FORTINIF4/target.h | 2 -- src/main/target/FF_PIKOBLX/target.h | 6 ------ src/main/target/FF_PIKOF4/target.h | 2 -- src/main/target/FRSKYF3/target.h | 2 -- src/main/target/FRSKYF4/target.h | 2 -- src/main/target/FURYF3/target.h | 2 -- src/main/target/FURYF4/target.h | 2 -- src/main/target/FURYF7/target.h | 6 +++++- src/main/target/IMPULSERCF3/target.h | 2 -- src/main/target/IRCFUSIONF3/target.h | 2 -- src/main/target/ISHAPEDF3/target.h | 2 -- src/main/target/KISSFC/target.h | 2 -- src/main/target/KIWIF4/target.h | 2 -- src/main/target/LUX_RACE/target.h | 2 -- src/main/target/MICROSCISKY/target.h | 2 +- src/main/target/MOTOLAB/target.h | 2 -- src/main/target/MULTIFLITEPICO/target.h | 2 -- src/main/target/NAZE/target.h | 2 +- src/main/target/NERO/target.h | 2 -- src/main/target/OMNIBUS/target.h | 5 ----- src/main/target/OMNIBUSF4/target.h | 2 -- src/main/target/RCEXPLORERF3/target.h | 2 -- src/main/target/REVO/target.h | 2 -- src/main/target/REVONANO/target.h | 2 -- src/main/target/RG_SSD_F3/target.h | 2 -- src/main/target/SINGULARITY/target.h | 2 -- src/main/target/SIRINFPV/target.h | 2 -- src/main/target/SPARKY/target.h | 2 -- src/main/target/SPRACINGF3/target.h | 2 -- src/main/target/SPRACINGF3EVO/target.h | 2 -- src/main/target/SPRACINGF3MINI/target.h | 2 -- src/main/target/SPRACINGF3NEO/target.h | 2 -- src/main/target/SPRACINGF4EVO/target.h | 2 -- src/main/target/SPRACINGF4NEO/target.h | 2 -- src/main/target/STM32F3DISCOVERY/target.h | 2 -- src/main/target/VRRACE/target.h | 4 ---- src/main/target/X_RACERSPI/target.h | 2 -- src/main/target/common_fc_pre.h | 2 +- 52 files changed, 11 insertions(+), 107 deletions(-) diff --git a/src/main/target/AIR32/target.h b/src/main/target/AIR32/target.h index 33fb04fb8c..317f1a940e 100644 --- a/src/main/target/AIR32/target.h +++ b/src/main/target/AIR32/target.h @@ -96,8 +96,6 @@ //#define CURRENT_METER_ADC_PIN PA5 #define RSSI_ADC_PIN PB2 -#define SPEKTRUM_BIND_PIN UART2_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package diff --git a/src/main/target/AIRHEROF3/target.h b/src/main/target/AIRHEROF3/target.h index f91f87b3ac..3872e070b9 100755 --- a/src/main/target/AIRHEROF3/target.h +++ b/src/main/target/AIRHEROF3/target.h @@ -88,8 +88,6 @@ #define SERIALRX_UART SERIAL_PORT_USART2 #define RX_CHANNELS_TAER -#define SPEKTRUM_BIND_PIN UART3_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h index 596b4f47a7..53a411a08b 100644 --- a/src/main/target/ALIENFLIGHTF1/target.h +++ b/src/main/target/ALIENFLIGHTF1/target.h @@ -62,8 +62,8 @@ #define RSSI_ADC_PIN PA1 #define EXTERNAL1_ADC_PIN PA5 -#define SPEKTRUM_BIND_PIN PA3 - +#define USE_SPEKTRUM_BIND +#define USE_SPEKTRUM_BIND_PLUG #define BINDPLUG_PIN PB5 #define DEFAULT_FEATURES FEATURE_MOTOR_STOP diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index f0ce648bc9..41bf661728 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -114,8 +114,6 @@ // LED strip configuration. #define LED_STRIP -#define SPEKTRUM_BIND_PIN UART2_RX_PIN - #define BINDPLUG_PIN PB12 #define DEFAULT_FEATURES FEATURE_MOTOR_STOP diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index bb7b3329d0..d149a32e7c 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -162,8 +162,6 @@ // LED strip configuration. #define LED_STRIP -#define SPEKTRUM_BIND_PIN UART2_RX_PIN - #define BINDPLUG_PIN PB2 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/ALIENFLIGHTNGF7/target.h b/src/main/target/ALIENFLIGHTNGF7/target.h index 7e07c38670..685d997757 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.h +++ b/src/main/target/ALIENFLIGHTNGF7/target.h @@ -171,8 +171,6 @@ // LED strip configuration. #define LED_STRIP -#define SPEKTRUM_BIND_PIN UART2_RX_PIN - #define BINDPLUG_PIN PB2 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index cc88854492..fb617e44f1 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -127,8 +127,6 @@ #define SBUS_TELEMETRY_UART SERIAL_PORT_USART1 #define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD) -#define SPEKTRUM_BIND_PIN UART2_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index a9ec2522bd..e6d7e7b1f3 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -152,8 +152,6 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define SPEKTRUM_BIND_PIN UART3_RX_PIN - #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index d03e0aa676..23e3af7c4c 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -93,8 +93,6 @@ #define VBAT_ADC_PIN PA0 #define RSSI_ADC_PIN PB0 -#define SPEKTRUM_BIND_PIN UART3_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE //#define SONAR diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index 84d573b88c..e75e2a7b9b 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -107,7 +107,7 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define USE_RX_MSP -#define SPEKTRUM_BIND_PIN PA3 // UART2, PA3 +#define USE_SPEKTRUM_BIND #endif //USE_RX_NRF24 diff --git a/src/main/target/CLRACINGF4/target.h b/src/main/target/CLRACINGF4/target.h index 8ddfe2e402..c76ca7bc59 100644 --- a/src/main/target/CLRACINGF4/target.h +++ b/src/main/target/CLRACINGF4/target.h @@ -122,8 +122,6 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define DEFAULT_FEATURES ( FEATURE_OSD ) #define CURRENT_METER_SCALE_DEFAULT 250 -#define SPEKTRUM_BIND_PIN UART6_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13))) diff --git a/src/main/target/CLRACINGF7/target.h b/src/main/target/CLRACINGF7/target.h index 62ab3fe003..12da28684e 100644 --- a/src/main/target/CLRACINGF7/target.h +++ b/src/main/target/CLRACINGF7/target.h @@ -120,7 +120,6 @@ // LED strip configuration. #define LED_STRIP -#define SPEKTRUM_BIND_PIN UART6_RX_PIN #define BINDPLUG_PIN PB2 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index a59e827abc..519f61c14e 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -130,8 +130,6 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define SPEKTRUM_BIND_PIN UART3_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE // !!TODO - check the TARGET_IO_PORTs are correct diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 91280a0fa5..2e6f518e9f 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -146,8 +146,6 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define SPEKTRUM_BIND_PIN UART6_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/FF_FORTINIF4/target.h b/src/main/target/FF_FORTINIF4/target.h index 5e5069d0c7..d754e40d60 100644 --- a/src/main/target/FF_FORTINIF4/target.h +++ b/src/main/target/FF_FORTINIF4/target.h @@ -103,8 +103,6 @@ #define SERIAL_PORT_COUNT 5 //SPECKTRUM BIND -#define SPEKTRUM_BIND_PIN UART3_RX_PIN - #define CMS #define USE_MSP_DISPLAYPORT /*---------------------------------*/ diff --git a/src/main/target/FF_PIKOBLX/target.h b/src/main/target/FF_PIKOBLX/target.h index 2b7d9ed722..b42237047e 100644 --- a/src/main/target/FF_PIKOBLX/target.h +++ b/src/main/target/FF_PIKOBLX/target.h @@ -94,12 +94,6 @@ #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART3 -#if defined(FF_RADIANCE) -#define SPEKTRUM_BIND_PIN UART2_RX_PIN -#else -#define SPEKTRUM_BIND_PIN UART3_RX_PIN -#endif - #define TRANSPONDER #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/FF_PIKOF4/target.h b/src/main/target/FF_PIKOF4/target.h index ab0cb499cc..4d34b84f11 100644 --- a/src/main/target/FF_PIKOF4/target.h +++ b/src/main/target/FF_PIKOF4/target.h @@ -98,8 +98,6 @@ #define UART6_TX_PIN PC6 #define SERIAL_PORT_COUNT 4 -//SPECKTRUM BIND -#define SPEKTRUM_BIND_PIN UART3_RX_PIN #define CMS #define USE_MSP_DISPLAYPORT diff --git a/src/main/target/FRSKYF3/target.h b/src/main/target/FRSKYF3/target.h index 550666518b..9e73b25663 100644 --- a/src/main/target/FRSKYF3/target.h +++ b/src/main/target/FRSKYF3/target.h @@ -138,8 +138,6 @@ #define TELEMETRY_UART SERIAL_PORT_USART3 #define SERIALRX_UART SERIAL_PORT_USART2 -#define SPEKTRUM_BIND_PIN UART3_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/FRSKYF4/target.h b/src/main/target/FRSKYF4/target.h index 26b83a916e..82674200df 100644 --- a/src/main/target/FRSKYF4/target.h +++ b/src/main/target/FRSKYF4/target.h @@ -123,8 +123,6 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define DEFAULT_FEATURES (FEATURE_OSD) #define AVOID_UART1_FOR_PWM_PPM -#define SPEKTRUM_BIND_PIN UART1_RX_PIN - #define SERIALRX_PROVIDER SERIALRX_SBUS #define TELEMETRY_UART SERIAL_PORT_USART6 #define SERIALRX_UART SERIAL_PORT_USART1 diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 5a9d893841..5785dde8d6 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -169,8 +169,6 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define SPEKTRUM_BIND_PIN UART3_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define USE_ESC_SENSOR diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 182b662e30..69d47398cd 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -179,8 +179,6 @@ #define SERIALRX_UART SERIAL_PORT_USART1 #define SERIALRX_PROVIDER SERIALRX_SBUS -#define SPEKTRUM_BIND_PIN UART3_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/FURYF7/target.h b/src/main/target/FURYF7/target.h index 335d1513a1..96fa4bb4ac 100644 --- a/src/main/target/FURYF7/target.h +++ b/src/main/target/FURYF7/target.h @@ -138,7 +138,11 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS -#define SPEKTRUM_BIND_PIN PB11 +// XXX To target maintainer: +// USE_SPEKTRUM_BIND is enabled for this target, and it will use +// RX (or TX if half-duplex) pin of the UART the receiver is connected. +// If PB11 is critical for this target, please resurrect this line. +//#define SPEKTRUM_BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/IMPULSERCF3/target.h b/src/main/target/IMPULSERCF3/target.h index f63687d568..5efa651d02 100644 --- a/src/main/target/IMPULSERCF3/target.h +++ b/src/main/target/IMPULSERCF3/target.h @@ -91,8 +91,6 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_UART SERIAL_PORT_USART2 -#define SPEKTRUM_BIND_PIN UART2_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index 41a478ffa5..18454b71b0 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -81,8 +81,6 @@ #undef LED_STRIP -#define SPEKTRUM_BIND_PIN UART3_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE /* diff --git a/src/main/target/ISHAPEDF3/target.h b/src/main/target/ISHAPEDF3/target.h index 2367ef2151..3d3040bdcd 100644 --- a/src/main/target/ISHAPEDF3/target.h +++ b/src/main/target/ISHAPEDF3/target.h @@ -105,8 +105,6 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define SPEKTRUM_BIND_PIN UART3_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 46e7b3c445..0509f46404 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -101,8 +101,6 @@ #define AVOID_UART2_FOR_PWM_PPM -#define SPEKTRUM_BIND_PIN UART2_RX_PIN - #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff diff --git a/src/main/target/KIWIF4/target.h b/src/main/target/KIWIF4/target.h index 0f72485724..ce21303499 100644 --- a/src/main/target/KIWIF4/target.h +++ b/src/main/target/KIWIF4/target.h @@ -178,8 +178,6 @@ #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART1 -#define SPEKTRUM_BIND_PIN UART3_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index fea3621eea..d8b8d4e127 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -157,8 +157,6 @@ #define DEFAULT_FEATURES (FEATURE_TELEMETRY) #endif -#define SPEKTRUM_BIND_PIN UART1_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define USE_ESCSERIAL diff --git a/src/main/target/MICROSCISKY/target.h b/src/main/target/MICROSCISKY/target.h index 7c2431aa44..ab0db600f1 100644 --- a/src/main/target/MICROSCISKY/target.h +++ b/src/main/target/MICROSCISKY/target.h @@ -70,7 +70,7 @@ #define USE_I2C_DEVICE_2 #define I2C_DEVICE (I2CDEV_2) -#define SPEKTRUM_BIND_PIN PA3 +#define USE_SPEKTRUM_BIND #define BRUSHED_MOTORS #define DEFAULT_FEATURES FEATURE_MOTOR_STOP diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index f9e0d681ff..c9c60b6f55 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -98,8 +98,6 @@ #define USE_ESC_SENSOR -#define SPEKTRUM_BIND_PIN UART2_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package diff --git a/src/main/target/MULTIFLITEPICO/target.h b/src/main/target/MULTIFLITEPICO/target.h index 0e249af074..38caeb8f0e 100755 --- a/src/main/target/MULTIFLITEPICO/target.h +++ b/src/main/target/MULTIFLITEPICO/target.h @@ -123,8 +123,6 @@ #define SERIALRX_UART SERIAL_PORT_USART3 #define RX_CHANNELS_TAER -#define SPEKTRUM_BIND_PIN UART3_RX_PIN - #define BINDPLUG_PIN PA13 #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 1de6785bf4..4c24d3f375 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -146,7 +146,7 @@ #define RSSI_ADC_PIN PA1 #define EXTERNAL1_ADC_PIN PA5 -#define SPEKTRUM_BIND_PIN PA3 +#define USE_SPEKTRUM_BIND_PIN #define DEFAULT_RX_FEATURE FEATURE_RX_PPM diff --git a/src/main/target/NERO/target.h b/src/main/target/NERO/target.h index 3106be009f..225f3c03c4 100644 --- a/src/main/target/NERO/target.h +++ b/src/main/target/NERO/target.h @@ -128,8 +128,6 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define SPEKTRUM_BIND_PIN UART3_RX_PIN - #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 50f3e3a753..d929657e6c 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -169,11 +169,6 @@ //#define AVOID_UART3_FOR_PWM_PPM // Disable this for using UART3 -#define USE_SPEKTRUM_BIND -#define USE_SPEKTRUM_BIND_PLUG -//#define SPEKTRUM_BIND_PIN NONE // Place holder -//#define BINDPLUG_PIN NONE // Place holder - #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 3775074832..4be0de9714 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -189,8 +189,6 @@ #define DEFAULT_FEATURES (FEATURE_OSD) #define AVOID_UART1_FOR_PWM_PPM -#define SPEKTRUM_BIND_PIN UART1_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13))) diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h index 93f1e37309..2aa351b0d2 100644 --- a/src/main/target/RCEXPLORERF3/target.h +++ b/src/main/target/RCEXPLORERF3/target.h @@ -109,8 +109,6 @@ #define NAV_GPS_GLITCH_DETECTION #define NAV_MAX_WAYPOINTS 60 -#define SPEKTRUM_BIND_PIN UART2_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 0c3926c7e5..c9ddb3d1a6 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -258,8 +258,6 @@ #define DEFAULT_FEATURES FEATURE_TELEMETRY #endif -#define SPEKTRUM_BIND_PIN UART3_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index 67c178b5b4..178d782f66 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -84,8 +84,6 @@ #define VBAT_ADC_PIN PA6 #define RSSI_ADC_PIN PA5 -#define SPEKTRUM_BIND_PIN UART2_RX_PIN - #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff diff --git a/src/main/target/RG_SSD_F3/target.h b/src/main/target/RG_SSD_F3/target.h index b83c875a10..62b2830b13 100644 --- a/src/main/target/RG_SSD_F3/target.h +++ b/src/main/target/RG_SSD_F3/target.h @@ -144,8 +144,6 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define SPEKTRUM_BIND_PIN UART3_RX_PIN - // IO - stm32f303rc in 64pin package #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 34fbef33fa..4d2a62ea7f 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -92,8 +92,6 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_UART SERIAL_PORT_USART2 -#define SPEKTRUM_BIND_PIN UART2_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 52b68f9cbc..287612fb08 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -148,8 +148,6 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define SPEKTRUM_BIND_PIN UART3_RX_PIN - #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define DEFAULT_FEATURES (FEATURE_RX_SERIAL | FEATURE_OSD) diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index ef2fb481f0..8730eb9260 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -90,8 +90,6 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define SPEKTRUM_BIND_PIN UART2_RX_PIN - //#define SONAR //#define SONAR_ECHO_PIN PB1 //#define SONAR_TRIGGER_PIN PA2 diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index baf48775ea..b2c5f4875c 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -189,8 +189,6 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_RSSI_ADC | FEATURE_TELEMETRY) -#define SPEKTRUM_BIND_PIN UART3_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 80da46617b..19c19b478c 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -172,8 +172,6 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_RSSI_ADC | FEATURE_TELEMETRY) -#define SPEKTRUM_BIND_PIN UART3_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index eefad26999..cc6af1328c 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -186,8 +186,6 @@ #define BINDPLUG_PIN BUTTON_B_PIN #endif -#define SPEKTRUM_BIND_PIN UART2_RX_PIN - #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h index de917c8595..3986fcc095 100755 --- a/src/main/target/SPRACINGF3NEO/target.h +++ b/src/main/target/SPRACINGF3NEO/target.h @@ -184,8 +184,6 @@ #define BUTTONS // Physically located on the optional OSD/VTX board. #define BUTTON_A_PIN PD2 -#define SPEKTRUM_BIND_PIN UART2_RX_PIN - // FIXME While it's possible to use the button on the OSD/VTX board for binding enabling it here will break binding unless you have the OSD/VTX connected. //#define BINDPLUG_PIN BUTTON_A_PIN diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index 256e1a31c9..0065aa4d36 100644 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/src/main/target/SPRACINGF4EVO/target.h @@ -176,8 +176,6 @@ #define TELEMETRY_UART SERIAL_PORT_UART5 #define SERIALRX_PROVIDER SERIALRX_SBUS -#define SPEKTRUM_BIND_PIN UART2_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/SPRACINGF4NEO/target.h b/src/main/target/SPRACINGF4NEO/target.h index 04b18fde14..01960602e2 100644 --- a/src/main/target/SPRACINGF4NEO/target.h +++ b/src/main/target/SPRACINGF4NEO/target.h @@ -214,8 +214,6 @@ #define BUTTON_A_PIN PB8 #endif -#define SPEKTRUM_BIND_PIN UART2_RX_PIN - // FIXME While it's possible to use the button on the OSD/VTX board for binding enabling it here will break binding unless you have the OSD/VTX connected. //#define BINDPLUG_PIN BUTTON_A_PIN diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 460e8c1d94..a5fefed4d1 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -175,8 +175,6 @@ #define USE_ESC_SENSOR -#define SPEKTRUM_BIND_PIN PA3 // USART2, PA3 - #define SONAR #define SONAR_TRIGGER_PIN PB0 #define SONAR_ECHO_PIN PB1 diff --git a/src/main/target/VRRACE/target.h b/src/main/target/VRRACE/target.h index 814cd2acbe..20a96924ad 100644 --- a/src/main/target/VRRACE/target.h +++ b/src/main/target/VRRACE/target.h @@ -165,10 +165,6 @@ //#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -/* -#define SPEKTRUM_BIND_PIN PB11 -*/ - #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 5984c6accf..8762fac962 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -115,8 +115,6 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define SPEKTRUM_BIND_PIN UART3_RX_PIN - #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 47a6c182e6..3f86f13b51 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -125,7 +125,7 @@ #ifdef USE_SERIALRX_SPEKTRUM #define USE_SPEKTRUM_BIND -#define USE_SPEKTRUM_BINDPIN +#define USE_SPEKTRUM_BIND_PLUG #endif #endif From 74f34d463db71828475ee27a1628656038d13a58 Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 20 Jun 2017 09:45:46 +0900 Subject: [PATCH 08/18] Use cached value instead of RCC->CSR --- src/main/drivers/system_stm32f10x.c | 2 +- src/main/drivers/system_stm32f30x.c | 2 +- src/main/drivers/system_stm32f4xx.c | 4 ++-- src/main/drivers/system_stm32f7xx.c | 4 ++-- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/drivers/system_stm32f10x.c b/src/main/drivers/system_stm32f10x.c index b6f2d423e0..536023fd27 100644 --- a/src/main/drivers/system_stm32f10x.c +++ b/src/main/drivers/system_stm32f10x.c @@ -83,7 +83,7 @@ void systemInit(void) // Turn on clocks for stuff we use RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE); - // cache RCC->CSR value to use it in isMPUSoftreset() and others + // cache RCC->CSR value to use it in isMPUSoftReset() and others cachedRccCsrValue = RCC->CSR; RCC_ClearFlag(); diff --git a/src/main/drivers/system_stm32f30x.c b/src/main/drivers/system_stm32f30x.c index a00abdfba0..f3a349706a 100644 --- a/src/main/drivers/system_stm32f30x.c +++ b/src/main/drivers/system_stm32f30x.c @@ -90,7 +90,7 @@ void systemInit(void) // Configure NVIC preempt/priority groups NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING); - // cache RCC->CSR value to use it in isMPUSoftreset() and others + // cache RCC->CSR value to use it in isMPUSoftReset() and others cachedRccCsrValue = RCC->CSR; RCC_ClearFlag(); diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index de32333124..5b7e155640 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -152,7 +152,7 @@ void enableGPIOPowerUsageAndNoiseReductions(void) bool isMPUSoftReset(void) { - if (RCC->CSR & RCC_CSR_SFTRSTF) + if (cachedRccCsrValue & RCC_CSR_SFTRSTF) return true; else return false; @@ -167,7 +167,7 @@ void systemInit(void) // Configure NVIC preempt/priority groups NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING); - // cache RCC->CSR value to use it in isMPUSoftreset() and others + // cache RCC->CSR value to use it in isMPUSoftReset() and others cachedRccCsrValue = RCC->CSR; /* Accounts for OP Bootloader, set the Vector Table base address as specified in .ld file */ diff --git a/src/main/drivers/system_stm32f7xx.c b/src/main/drivers/system_stm32f7xx.c index db4f9085ea..bfe8039dde 100644 --- a/src/main/drivers/system_stm32f7xx.c +++ b/src/main/drivers/system_stm32f7xx.c @@ -149,7 +149,7 @@ void enableGPIOPowerUsageAndNoiseReductions(void) bool isMPUSoftReset(void) { - if (RCC->CSR & RCC_CSR_SFTRSTF) + if (cachedRccCsrValue & RCC_CSR_SFTRSTF) return true; else return false; @@ -164,7 +164,7 @@ void systemInit(void) // Configure NVIC preempt/priority groups HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITY_GROUPING); - // cache RCC->CSR value to use it in isMPUSoftreset() and others + // cache RCC->CSR value to use it in isMPUSoftReset() and others cachedRccCsrValue = RCC->CSR; /* Accounts for OP Bootloader, set the Vector Table base address as specified in .ld file */ From 42fa95c2d01c86b846b46b948a295c6a358063a8 Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 20 Jun 2017 10:08:46 +0900 Subject: [PATCH 09/18] Reset bind pin when override is in effect. --- src/main/rx/spektrum.c | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 27c2578505..f73e2f2a84 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -273,6 +273,16 @@ void spektrumBind(rxConfig_t *rxConfig) } + + // Release the bind pin to avoid interference with an actual rx pin, + // when rxConfig->spektrum_bind_pin_override_ioTag is used. + // This happens when the bind pin is connected in parallel to the rx pin. + + if (rxConfig->spektrum_bind_pin_override_ioTag) { + delay(50); // Keep it high for 50msec + IOConfigGPIO(bindIO, IOCFG_IN_FLOATING); + } + // If we came here as a result of hard reset (power up, with spektrum_sat_bind set), then reset it back to zero and write config // Don't reset if hardware bind plug is present // Reset only when autoreset is enabled From 10605e00b9df04121bcd3fa54ffa3d7f178cf74a Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 23 Jun 2017 19:14:07 +0900 Subject: [PATCH 10/18] Resolve confict --- src/main/drivers/resource.c | 1 + src/main/drivers/resource.h | 1 + 2 files changed, 2 insertions(+) diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c index 0bdf92ae76..13cdf530b1 100644 --- a/src/main/drivers/resource.c +++ b/src/main/drivers/resource.c @@ -62,6 +62,7 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = { "TRANSPONDER", "VTX", "COMPASS_CS", + "SPI_PREINIT", "RX_BIND_PLUG", }; diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index aae4598151..f79990ea89 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -62,6 +62,7 @@ typedef enum { OWNER_TRANSPONDER, OWNER_VTX, OWNER_COMPASS_CS, + OWNER_SPI_PREINIT, OWNER_RX_BIND_PLUG, OWNER_TOTAL_COUNT } resourceOwner_e; From 81d00db1b10a96cc483c05810bf8e5cf50480f7b Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 24 Jun 2017 13:26:43 +1000 Subject: [PATCH 11/18] Increase frequency of led strip for higher resolution - tested as working off NERO motor pin 5 --- src/main/drivers/light_ws2811strip.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index db51d00edf..64967bc3b3 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -28,7 +28,7 @@ // number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes) #define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE + WS2811_DELAY_BUFFER_LENGTH) -#define WS2811_TIMER_MHZ 24 +#define WS2811_TIMER_MHZ 48 #define WS2811_CARRIER_HZ 800000 void ws2811LedStripInit(ioTag_t ioTag); From 3a84e81827fe32e29f0c2d8105e3e4f365868bd4 Mon Sep 17 00:00:00 2001 From: Dan Nixon Date: Thu, 8 Jun 2017 17:31:34 +0100 Subject: [PATCH 12/18] Add BB log no for SD card --- src/main/blackbox/blackbox_io.c | 8 ++++++++ src/main/blackbox/blackbox_io.h | 1 + src/main/fc/settings.c | 1 + src/main/io/osd.c | 28 +++++++++++++++++----------- src/main/io/osd.h | 1 + 5 files changed, 28 insertions(+), 11 deletions(-) diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 41ee6a909e..8e86e962e1 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -511,6 +511,14 @@ bool isBlackboxDeviceFull(void) } } +unsigned int blackboxGetLogNumber() +{ +#ifdef USE_SDCARD + return blackboxSDCard.largestLogFileNumber; +#endif + return 0; +} + /** * Call once every loop iteration in order to maintain the global blackboxHeaderBudget with the number of bytes we can * transmit this iteration. diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h index 5c8db43e75..c59b0df2c1 100644 --- a/src/main/blackbox/blackbox_io.h +++ b/src/main/blackbox/blackbox_io.h @@ -52,6 +52,7 @@ bool blackboxDeviceBeginLog(void); bool blackboxDeviceEndLog(bool retainLog); bool isBlackboxDeviceFull(void); +unsigned int blackboxGetLogNumber(); void blackboxReplenishHeaderBudget(); blackboxBufferReserveStatus_e blackboxDeviceReserveBufferSpace(int32_t bytes); diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index f316e5c92b..43518f8fa4 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -678,6 +678,7 @@ const clivalue_t valueTable[] = { { "osd_stat_endbatt", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_END_BATTERY])}, { "osd_stat_flytime", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_FLYTIME])}, { "osd_stat_armtime", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_ARMEDTIME])}, + { "osd_stat_bb_no", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_BLACKBOX_NUMBER])}, #endif // PG_SYSTEM_CONFIG diff --git a/src/main/io/osd.c b/src/main/io/osd.c index c06a5202a3..dcbd7257f9 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -737,17 +737,18 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->item_pos[OSD_ESC_TMP] = OSD_POS(18, 2) | VISIBLE_FLAG; osdConfig->item_pos[OSD_ESC_RPM] = OSD_POS(19, 2) | VISIBLE_FLAG; - osdConfig->enabled_stats[OSD_STAT_MAX_SPEED] = true; - osdConfig->enabled_stats[OSD_STAT_MIN_BATTERY] = true; - osdConfig->enabled_stats[OSD_STAT_MIN_RSSI] = true; - osdConfig->enabled_stats[OSD_STAT_MAX_CURRENT] = true; - osdConfig->enabled_stats[OSD_STAT_USED_MAH] = true; - osdConfig->enabled_stats[OSD_STAT_MAX_ALTITUDE] = false; - osdConfig->enabled_stats[OSD_STAT_BLACKBOX] = true; - osdConfig->enabled_stats[OSD_STAT_END_BATTERY] = false; - osdConfig->enabled_stats[OSD_STAT_FLYTIME] = false; - osdConfig->enabled_stats[OSD_STAT_ARMEDTIME] = true; - osdConfig->enabled_stats[OSD_STAT_MAX_DISTANCE] = false; + osdConfig->enabled_stats[OSD_STAT_MAX_SPEED] = true; + osdConfig->enabled_stats[OSD_STAT_MIN_BATTERY] = true; + osdConfig->enabled_stats[OSD_STAT_MIN_RSSI] = true; + osdConfig->enabled_stats[OSD_STAT_MAX_CURRENT] = true; + osdConfig->enabled_stats[OSD_STAT_USED_MAH] = true; + osdConfig->enabled_stats[OSD_STAT_MAX_ALTITUDE] = false; + osdConfig->enabled_stats[OSD_STAT_BLACKBOX] = true; + osdConfig->enabled_stats[OSD_STAT_END_BATTERY] = false; + osdConfig->enabled_stats[OSD_STAT_FLYTIME] = false; + osdConfig->enabled_stats[OSD_STAT_ARMEDTIME] = true; + osdConfig->enabled_stats[OSD_STAT_MAX_DISTANCE] = false; + osdConfig->enabled_stats[OSD_STAT_BLACKBOX_NUMBER] = true; osdConfig->units = OSD_UNIT_METRIC; @@ -1021,6 +1022,11 @@ static void osdShowStats(void) osdGetBlackboxStatusString(buff); osdDisplayStatisticLabel(top++, "BLACKBOX", buff); } + + if (osdConfig()->enabled_stats[OSD_STAT_BLACKBOX_NUMBER] && blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) { + itoa(blackboxGetLogNumber(), buff, 10); + osdDisplayStatisticLabel(top++, "BB LOG NUM", buff); + } #endif /* Reset time since last armed here to ensure this timer is at zero when back at "main" OSD screen */ diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 133517e7a1..1b500786eb 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -80,6 +80,7 @@ typedef enum { OSD_STAT_FLYTIME, OSD_STAT_ARMEDTIME, OSD_STAT_MAX_DISTANCE, + OSD_STAT_BLACKBOX_NUMBER, OSD_STAT_COUNT // MUST BE LAST } osd_stats_e; From 58fdace43cd701a61947142f8e769d4301bbc1e4 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 25 Jun 2017 05:03:41 +0100 Subject: [PATCH 13/18] Minor tidy of blackbox code. iNav alignment --- src/main/blackbox/blackbox.c | 242 +++++++++++++++++------------------ 1 file changed, 119 insertions(+), 123 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 40cf9c3bad..acb8b36297 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -69,6 +69,7 @@ #include "sensors/gyro.h" #include "sensors/sonar.h" + #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT) #define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_FLASH #elif defined(ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT) @@ -262,20 +263,20 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = { typedef enum BlackboxState { BLACKBOX_STATE_DISABLED = 0, - BLACKBOX_STATE_STOPPED, //1 - BLACKBOX_STATE_PREPARE_LOG_FILE, //2 - BLACKBOX_STATE_SEND_HEADER, //3 - BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER, //4 - BLACKBOX_STATE_SEND_GPS_H_HEADER, //5 - BLACKBOX_STATE_SEND_GPS_G_HEADER, //6 - BLACKBOX_STATE_SEND_SLOW_HEADER, //7 - BLACKBOX_STATE_SEND_SYSINFO, //8 - BLACKBOX_STATE_PAUSED, //9 - BLACKBOX_STATE_RUNNING, //10 - BLACKBOX_STATE_SHUTTING_DOWN, //11 - BLACKBOX_STATE_START_ERASE, //12 - BLACKBOX_STATE_ERASING, //13 - BLACKBOX_STATE_ERASED //14 + BLACKBOX_STATE_STOPPED, + BLACKBOX_STATE_PREPARE_LOG_FILE, + BLACKBOX_STATE_SEND_HEADER, + BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER, + BLACKBOX_STATE_SEND_GPS_H_HEADER, + BLACKBOX_STATE_SEND_GPS_G_HEADER, + BLACKBOX_STATE_SEND_SLOW_HEADER, + BLACKBOX_STATE_SEND_SYSINFO, + BLACKBOX_STATE_PAUSED, + BLACKBOX_STATE_RUNNING, + BLACKBOX_STATE_SHUTTING_DOWN, + BLACKBOX_STATE_START_ERASE, + BLACKBOX_STATE_ERASING, + BLACKBOX_STATE_ERASED } BlackboxState; @@ -309,7 +310,8 @@ typedef struct blackboxMainState_s { } blackboxMainState_t; typedef struct blackboxGpsState_s { - int32_t GPS_home[2], GPS_coord[2]; + int32_t GPS_home[2]; + int32_t GPS_coord[2]; uint8_t GPS_numSat; } blackboxGpsState_t; @@ -333,7 +335,6 @@ static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED; static uint32_t blackboxLastArmingBeep = 0; static uint32_t blackboxLastFlightModeFlags = 0; // New event tracking of flight modes - static struct { uint32_t headerIndex; @@ -352,7 +353,8 @@ static uint32_t blackboxConditionCache; STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) >= FLIGHT_LOG_FIELD_CONDITION_LAST, too_many_flight_log_conditions); static uint32_t blackboxIteration; -static uint16_t blackboxPFrameIndex, blackboxIFrameIndex; +static uint16_t blackboxPFrameIndex; +static uint16_t blackboxIFrameIndex; static uint16_t blackboxSlowFrameIterationTimer; static bool blackboxLoggedAnyFrames; @@ -623,8 +625,6 @@ static void blackboxWriteMainStateArrayUsingAveragePredictor(int arrOffsetInHist static void writeInterframe(void) { - int32_t deltas[8]; - blackboxMainState_t *blackboxCurrent = blackboxHistory[0]; blackboxMainState_t *blackboxLast = blackboxHistory[1]; @@ -638,6 +638,7 @@ static void writeInterframe(void) */ blackboxWriteSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time)); + int32_t deltas[8]; arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT); blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT); @@ -1198,16 +1199,16 @@ static bool blackboxWriteSysinfo(void) const controlRateConfig_t *currentControlRateProfile = controlRateProfiles(systemConfig()->activeRateProfile); switch (xmitState.headerIndex) { - BLACKBOX_PRINT_HEADER_LINE("Firmware type", "%s", "Cleanflight"); - BLACKBOX_PRINT_HEADER_LINE("Firmware revision", "%s %s (%s) %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, shortGitRevision, targetName); - BLACKBOX_PRINT_HEADER_LINE("Firmware date", "%s %s", buildDate, buildTime); - BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s", systemConfig()->name); - BLACKBOX_PRINT_HEADER_LINE("P interval", "%d/%d", blackboxConfig()->rate_num, blackboxConfig()->rate_denom); - BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", motorConfig()->minthrottle); - BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle); + BLACKBOX_PRINT_HEADER_LINE("Firmware type", "%s", "Cleanflight"); + BLACKBOX_PRINT_HEADER_LINE("Firmware revision", "%s %s (%s) %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, shortGitRevision, targetName); + BLACKBOX_PRINT_HEADER_LINE("Firmware date", "%s %s", buildDate, buildTime); + BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s", systemConfig()->name); + BLACKBOX_PRINT_HEADER_LINE("P interval", "%d/%d", blackboxConfig()->rate_num, blackboxConfig()->rate_denom); + BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", motorConfig()->minthrottle); + BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle); BLACKBOX_PRINT_HEADER_LINE("gyro_scale","0x%x", castFloatBytesToInt(1.0f)); - BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLowInt,motorOutputHighInt); - BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G); + BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLowInt,motorOutputHighInt); + BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G); BLACKBOX_PRINT_HEADER_LINE_CUSTOM( if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { @@ -1217,10 +1218,10 @@ static bool blackboxWriteSysinfo(void) } ); - BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage", "%u,%u,%u", batteryConfig()->vbatmincellvoltage, - batteryConfig()->vbatwarningcellvoltage, - batteryConfig()->vbatmaxcellvoltage); - BLACKBOX_PRINT_HEADER_LINE("vbatref", "%u", vbatReference); + BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage", "%u,%u,%u", batteryConfig()->vbatmincellvoltage, + batteryConfig()->vbatwarningcellvoltage, + batteryConfig()->vbatmaxcellvoltage); + BLACKBOX_PRINT_HEADER_LINE("vbatref", "%u", vbatReference); BLACKBOX_PRINT_HEADER_LINE_CUSTOM( if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) { @@ -1228,92 +1229,92 @@ static bool blackboxWriteSysinfo(void) } ); - BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", gyro.targetLooptime); - BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom", "%d", gyroConfig()->gyro_sync_denom); - BLACKBOX_PRINT_HEADER_LINE("pid_process_denom", "%d", pidConfig()->pid_process_denom); - BLACKBOX_PRINT_HEADER_LINE("rc_rate", "%d", currentControlRateProfile->rcRate8); - BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d", currentControlRateProfile->rcExpo8); - BLACKBOX_PRINT_HEADER_LINE("rc_rate_yaw", "%d", currentControlRateProfile->rcYawRate8); - BLACKBOX_PRINT_HEADER_LINE("rc_expo_yaw", "%d", currentControlRateProfile->rcYawExpo8); - BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlRateProfile->thrMid8); - BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlRateProfile->thrExpo8); - BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlRateProfile->dynThrPID); - BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint", "%d", currentControlRateProfile->tpa_breakpoint); - BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->rates[ROLL], - currentControlRateProfile->rates[PITCH], - currentControlRateProfile->rates[YAW]); - BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].P, - currentPidProfile->pid[PID_ROLL].I, - currentPidProfile->pid[PID_ROLL].D); - BLACKBOX_PRINT_HEADER_LINE("pitchPID", "%d,%d,%d", currentPidProfile->pid[PID_PITCH].P, - currentPidProfile->pid[PID_PITCH].I, - currentPidProfile->pid[PID_PITCH].D); - BLACKBOX_PRINT_HEADER_LINE("yawPID", "%d,%d,%d", currentPidProfile->pid[PID_YAW].P, - currentPidProfile->pid[PID_YAW].I, - currentPidProfile->pid[PID_YAW].D); - BLACKBOX_PRINT_HEADER_LINE("altPID", "%d,%d,%d", currentPidProfile->pid[PID_ALT].P, - currentPidProfile->pid[PID_ALT].I, - currentPidProfile->pid[PID_ALT].D); - BLACKBOX_PRINT_HEADER_LINE("posPID", "%d,%d,%d", currentPidProfile->pid[PID_POS].P, - currentPidProfile->pid[PID_POS].I, - currentPidProfile->pid[PID_POS].D); - BLACKBOX_PRINT_HEADER_LINE("posrPID", "%d,%d,%d", currentPidProfile->pid[PID_POSR].P, - currentPidProfile->pid[PID_POSR].I, - currentPidProfile->pid[PID_POSR].D); - BLACKBOX_PRINT_HEADER_LINE("navrPID", "%d,%d,%d", currentPidProfile->pid[PID_NAVR].P, - currentPidProfile->pid[PID_NAVR].I, - currentPidProfile->pid[PID_NAVR].D); - BLACKBOX_PRINT_HEADER_LINE("levelPID", "%d,%d,%d", currentPidProfile->pid[PID_LEVEL].P, - currentPidProfile->pid[PID_LEVEL].I, - currentPidProfile->pid[PID_LEVEL].D); - BLACKBOX_PRINT_HEADER_LINE("magPID", "%d", currentPidProfile->pid[PID_MAG].P); - BLACKBOX_PRINT_HEADER_LINE("velPID", "%d,%d,%d", currentPidProfile->pid[PID_VEL].P, - currentPidProfile->pid[PID_VEL].I, - currentPidProfile->pid[PID_VEL].D); - BLACKBOX_PRINT_HEADER_LINE("dterm_filter_type", "%d", currentPidProfile->dterm_filter_type); - BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz", "%d", currentPidProfile->dterm_lpf_hz); - BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz", "%d", currentPidProfile->yaw_lpf_hz); - BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz", "%d", currentPidProfile->dterm_notch_hz); - BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff", "%d", currentPidProfile->dterm_notch_cutoff); - BLACKBOX_PRINT_HEADER_LINE("iterm_windup", "%d", currentPidProfile->itermWindupPointPercent); - BLACKBOX_PRINT_HEADER_LINE("vbat_pid_gain", "%d", currentPidProfile->vbatPidCompensation); - BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle", "%d", currentPidProfile->pidAtMinThrottle); + BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", gyro.targetLooptime); + BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom", "%d", gyroConfig()->gyro_sync_denom); + BLACKBOX_PRINT_HEADER_LINE("pid_process_denom", "%d", pidConfig()->pid_process_denom); + BLACKBOX_PRINT_HEADER_LINE("rc_rate", "%d", currentControlRateProfile->rcRate8); + BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d", currentControlRateProfile->rcExpo8); + BLACKBOX_PRINT_HEADER_LINE("rc_rate_yaw", "%d", currentControlRateProfile->rcYawRate8); + BLACKBOX_PRINT_HEADER_LINE("rc_expo_yaw", "%d", currentControlRateProfile->rcYawExpo8); + BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlRateProfile->thrMid8); + BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlRateProfile->thrExpo8); + BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlRateProfile->dynThrPID); + BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint", "%d", currentControlRateProfile->tpa_breakpoint); + BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->rates[ROLL], + currentControlRateProfile->rates[PITCH], + currentControlRateProfile->rates[YAW]); + BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].P, + currentPidProfile->pid[PID_ROLL].I, + currentPidProfile->pid[PID_ROLL].D); + BLACKBOX_PRINT_HEADER_LINE("pitchPID", "%d,%d,%d", currentPidProfile->pid[PID_PITCH].P, + currentPidProfile->pid[PID_PITCH].I, + currentPidProfile->pid[PID_PITCH].D); + BLACKBOX_PRINT_HEADER_LINE("yawPID", "%d,%d,%d", currentPidProfile->pid[PID_YAW].P, + currentPidProfile->pid[PID_YAW].I, + currentPidProfile->pid[PID_YAW].D); + BLACKBOX_PRINT_HEADER_LINE("altPID", "%d,%d,%d", currentPidProfile->pid[PID_ALT].P, + currentPidProfile->pid[PID_ALT].I, + currentPidProfile->pid[PID_ALT].D); + BLACKBOX_PRINT_HEADER_LINE("posPID", "%d,%d,%d", currentPidProfile->pid[PID_POS].P, + currentPidProfile->pid[PID_POS].I, + currentPidProfile->pid[PID_POS].D); + BLACKBOX_PRINT_HEADER_LINE("posrPID", "%d,%d,%d", currentPidProfile->pid[PID_POSR].P, + currentPidProfile->pid[PID_POSR].I, + currentPidProfile->pid[PID_POSR].D); + BLACKBOX_PRINT_HEADER_LINE("navrPID", "%d,%d,%d", currentPidProfile->pid[PID_NAVR].P, + currentPidProfile->pid[PID_NAVR].I, + currentPidProfile->pid[PID_NAVR].D); + BLACKBOX_PRINT_HEADER_LINE("levelPID", "%d,%d,%d", currentPidProfile->pid[PID_LEVEL].P, + currentPidProfile->pid[PID_LEVEL].I, + currentPidProfile->pid[PID_LEVEL].D); + BLACKBOX_PRINT_HEADER_LINE("magPID", "%d", currentPidProfile->pid[PID_MAG].P); + BLACKBOX_PRINT_HEADER_LINE("velPID", "%d,%d,%d", currentPidProfile->pid[PID_VEL].P, + currentPidProfile->pid[PID_VEL].I, + currentPidProfile->pid[PID_VEL].D); + BLACKBOX_PRINT_HEADER_LINE("dterm_filter_type", "%d", currentPidProfile->dterm_filter_type); + BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz", "%d", currentPidProfile->dterm_lpf_hz); + BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz", "%d", currentPidProfile->yaw_lpf_hz); + BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz", "%d", currentPidProfile->dterm_notch_hz); + BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff", "%d", currentPidProfile->dterm_notch_cutoff); + BLACKBOX_PRINT_HEADER_LINE("iterm_windup", "%d", currentPidProfile->itermWindupPointPercent); + BLACKBOX_PRINT_HEADER_LINE("vbat_pid_gain", "%d", currentPidProfile->vbatPidCompensation); + BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle", "%d", currentPidProfile->pidAtMinThrottle); // Betaflight PID controller parameters - BLACKBOX_PRINT_HEADER_LINE("anti_gravity_threshold", "%d", currentPidProfile->itermThrottleThreshold); - BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain", "%d", currentPidProfile->itermAcceleratorGain); - BLACKBOX_PRINT_HEADER_LINE("setpoint_relaxation_ratio", "%d", currentPidProfile->setpointRelaxRatio); - BLACKBOX_PRINT_HEADER_LINE("dterm_setpoint_weight", "%d", currentPidProfile->dtermSetpointWeight); - BLACKBOX_PRINT_HEADER_LINE("acc_limit_yaw", "%d", currentPidProfile->yawRateAccelLimit); - BLACKBOX_PRINT_HEADER_LINE("acc_limit", "%d", currentPidProfile->rateAccelLimit); - BLACKBOX_PRINT_HEADER_LINE("pidsum_limit", "%d", currentPidProfile->pidSumLimit); - BLACKBOX_PRINT_HEADER_LINE("pidsum_limit_yaw", "%d", currentPidProfile->pidSumLimitYaw); + BLACKBOX_PRINT_HEADER_LINE("anti_gravity_threshold", "%d", currentPidProfile->itermThrottleThreshold); + BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain", "%d", currentPidProfile->itermAcceleratorGain); + BLACKBOX_PRINT_HEADER_LINE("setpoint_relaxation_ratio", "%d", currentPidProfile->setpointRelaxRatio); + BLACKBOX_PRINT_HEADER_LINE("dterm_setpoint_weight", "%d", currentPidProfile->dtermSetpointWeight); + BLACKBOX_PRINT_HEADER_LINE("acc_limit_yaw", "%d", currentPidProfile->yawRateAccelLimit); + BLACKBOX_PRINT_HEADER_LINE("acc_limit", "%d", currentPidProfile->rateAccelLimit); + BLACKBOX_PRINT_HEADER_LINE("pidsum_limit", "%d", currentPidProfile->pidSumLimit); + BLACKBOX_PRINT_HEADER_LINE("pidsum_limit_yaw", "%d", currentPidProfile->pidSumLimitYaw); // End of Betaflight controller parameters - BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband); - BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband); - BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d", gyroConfig()->gyro_lpf); - BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_type", "%d", gyroConfig()->gyro_soft_lpf_type); - BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz", "%d", gyroConfig()->gyro_soft_lpf_hz); - BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1, - gyroConfig()->gyro_soft_notch_hz_2); - BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1, - gyroConfig()->gyro_soft_notch_cutoff_2); - BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f)); - BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware); - BLACKBOX_PRINT_HEADER_LINE("baro_hardware", "%d", barometerConfig()->baro_hardware); - BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", compassConfig()->mag_hardware); - BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm", "%d", armingConfig()->gyro_cal_on_first_arm); - BLACKBOX_PRINT_HEADER_LINE("rc_interpolation", "%d", rxConfig()->rcInterpolation); - BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval", "%d", rxConfig()->rcInterpolationInterval); - BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle", "%d", rxConfig()->airModeActivateThreshold); - BLACKBOX_PRINT_HEADER_LINE("serialrx_provider", "%d", rxConfig()->serialrx_provider); - BLACKBOX_PRINT_HEADER_LINE("use_unsynced_pwm", "%d", motorConfig()->dev.useUnsyncedPwm); - BLACKBOX_PRINT_HEADER_LINE("motor_pwm_protocol", "%d", motorConfig()->dev.motorPwmProtocol); - BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d", motorConfig()->dev.motorPwmRate); - BLACKBOX_PRINT_HEADER_LINE("dshot_idle_value", "%d", motorConfig()->digitalIdleOffsetValue); - BLACKBOX_PRINT_HEADER_LINE("debug_mode", "%d", systemConfig()->debug_mode); - BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures); + BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband); + BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband); + BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d", gyroConfig()->gyro_lpf); + BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_type", "%d", gyroConfig()->gyro_soft_lpf_type); + BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz", "%d", gyroConfig()->gyro_soft_lpf_hz); + BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1, + gyroConfig()->gyro_soft_notch_hz_2); + BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1, + gyroConfig()->gyro_soft_notch_cutoff_2); + BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware); + BLACKBOX_PRINT_HEADER_LINE("baro_hardware", "%d", barometerConfig()->baro_hardware); + BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", compassConfig()->mag_hardware); + BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm", "%d", armingConfig()->gyro_cal_on_first_arm); + BLACKBOX_PRINT_HEADER_LINE("rc_interpolation", "%d", rxConfig()->rcInterpolation); + BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval", "%d", rxConfig()->rcInterpolationInterval); + BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle", "%d", rxConfig()->airModeActivateThreshold); + BLACKBOX_PRINT_HEADER_LINE("serialrx_provider", "%d", rxConfig()->serialrx_provider); + BLACKBOX_PRINT_HEADER_LINE("use_unsynced_pwm", "%d", motorConfig()->dev.useUnsyncedPwm); + BLACKBOX_PRINT_HEADER_LINE("motor_pwm_protocol", "%d", motorConfig()->dev.motorPwmProtocol); + BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d", motorConfig()->dev.motorPwmRate); + BLACKBOX_PRINT_HEADER_LINE("dshot_idle_value", "%d", motorConfig()->digitalIdleOffsetValue); + BLACKBOX_PRINT_HEADER_LINE("debug_mode", "%d", systemConfig()->debug_mode); + BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures); default: return true; @@ -1369,30 +1370,25 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) /* If an arming beep has played since it was last logged, write the time of the arming beep to the log as a synchronization point */ static void blackboxCheckAndLogArmingBeep(void) { - flightLogEvent_syncBeep_t eventData; - // Use != so that we can still detect a change if the counter wraps if (getArmingBeepTimeMicros() != blackboxLastArmingBeep) { blackboxLastArmingBeep = getArmingBeepTimeMicros(); - + flightLogEvent_syncBeep_t eventData; eventData.time = blackboxLastArmingBeep; - - blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *) &eventData); + blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *)&eventData); } } /* monitor the flight mode event status and trigger an event record if the state changes */ static void blackboxCheckAndLogFlightMode(void) { - flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags - // Use != so that we can still detect a change if the counter wraps if (memcmp(&rcModeActivationMask, &blackboxLastFlightModeFlags, sizeof(blackboxLastFlightModeFlags))) { + flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags eventData.lastFlags = blackboxLastFlightModeFlags; memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags)); memcpy(&eventData.flags, &rcModeActivationMask, sizeof(eventData.flags)); - - blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *) &eventData); + blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *)&eventData); } } From 779823fb7957efefb2fd8a317c22f8d5503768ef Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Fri, 23 Jun 2017 20:06:24 +1200 Subject: [PATCH 14/18] Cleaned up Dshot naming, containment. --- src/main/drivers/pwm_output.c | 33 ++++++++++++++----------- src/main/drivers/pwm_output.h | 17 ++++++++++--- src/main/drivers/pwm_output_dshot.c | 6 ++--- src/main/drivers/pwm_output_dshot_hal.c | 6 ++--- src/main/flight/mixer.c | 17 ------------- src/main/flight/mixer.h | 19 -------------- src/main/target/SITL/target.c | 4 +++ 7 files changed, 43 insertions(+), 59 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 2325890609..1f725554e8 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -49,7 +49,7 @@ static uint16_t freqBeep=0; #endif bool pwmMotorsEnabled = false; -bool isDigital = false; +bool isDshot = false; static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output) { @@ -160,9 +160,9 @@ static void pwmWriteMultiShot(uint8_t index, float value) } #ifdef USE_DSHOT -static void pwmWriteDigital(uint8_t index, float value) +static void pwmWriteDshot(uint8_t index, float value) { - pwmWriteDigitalInt(index, lrintf(value)); + pwmWriteDshotInt(index, lrintf(value)); } static uint8_t loadDmaBufferDshot(motorDmaOutput_t *const motor, uint16_t packet) @@ -277,24 +277,24 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8 break; #ifdef USE_DSHOT case PWM_TYPE_PROSHOT1000: - pwmWrite = &pwmWriteDigital; + pwmWrite = &pwmWriteDshot; loadDmaBuffer = &loadDmaBufferProshot; - pwmCompleteWrite = &pwmCompleteDigitalMotorUpdate; - isDigital = true; + pwmCompleteWrite = &pwmCompleteDshotMotorUpdate; + isDshot = true; break; case PWM_TYPE_DSHOT1200: case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: - pwmWrite = &pwmWriteDigital; + pwmWrite = &pwmWriteDshot; loadDmaBuffer = &loadDmaBufferDshot; - pwmCompleteWrite = &pwmCompleteDigitalMotorUpdate; - isDigital = true; + pwmCompleteWrite = &pwmCompleteDshotMotorUpdate; + isDshot = true; break; #endif } - if (!isDigital) { + if (!isDshot) { pwmCompleteWrite = useUnsyncedPwm ? &pwmCompleteWriteUnused : &pwmCompleteOneshotMotorUpdate; } @@ -313,8 +313,8 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8 motors[motorIndex].io = IOGetByTag(tag); #ifdef USE_DSHOT - if (isDigital) { - pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol, + if (isDshot) { + pwmDshotMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol, motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output); motors[motorIndex].enabled = true; continue; @@ -354,6 +354,11 @@ pwmOutputPort_t *pwmGetMotors(void) return motors; } +bool isMotorProtocolDshot(void) +{ + return isDshot; +} + #ifdef USE_DSHOT uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) { @@ -374,7 +379,7 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) void pwmWriteDshotCommand(uint8_t index, uint8_t command) { - if (isDigital && (command <= DSHOT_MAX_COMMAND)) { + if (isDshot && (command <= DSHOT_MAX_COMMAND)) { motorDmaOutput_t *const motor = getMotorDmaOutput(index); unsigned repeats; @@ -395,7 +400,7 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t command) for (; repeats; repeats--) { motor->requestTelemetry = true; - pwmWriteDigitalInt(index, command); + pwmWriteDshotInt(index, command); pwmCompleteMotorUpdate(0); delay(1); diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 16d7f37c36..c37fa960f2 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -30,6 +30,15 @@ #define DSHOT_MAX_COMMAND 47 +/* + DshotSettingRequest (KISS24). Spin direction, 3d and save Settings reqire 10 requests.. and the TLM Byte must always be high if 1-47 are used to send settings + + 3D Mode: + 0 = stop + 48 (low) - 1047 (high) -> negative direction + 1048 (low) - 2047 (high) -> positive direction + */ + typedef enum { DSHOT_CMD_MOTOR_STOP = 0, DSHOT_CMD_BEEP1, @@ -166,6 +175,8 @@ void servoDevInit(const servoDevConfig_t *servoDevConfig); void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); +bool isMotorProtocolDshot(void); + #ifdef USE_DSHOT typedef uint8_t loadDmaBufferFunc(motorDmaOutput_t *const motor, uint16_t packet); // function pointer used to encode a digital motor value into the DMA buffer representation @@ -175,9 +186,9 @@ extern loadDmaBufferFunc *loadDmaBuffer; uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType); void pwmWriteDshotCommand(uint8_t index, uint8_t command); -void pwmWriteDigitalInt(uint8_t index, uint16_t value); -void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output); -void pwmCompleteDigitalMotorUpdate(uint8_t motorCount); +void pwmWriteDshotInt(uint8_t index, uint16_t value); +void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output); +void pwmCompleteDshotMotorUpdate(uint8_t motorCount); #endif #ifdef BEEPER diff --git a/src/main/drivers/pwm_output_dshot.c b/src/main/drivers/pwm_output_dshot.c index 2c97bc0ef4..3058761024 100644 --- a/src/main/drivers/pwm_output_dshot.c +++ b/src/main/drivers/pwm_output_dshot.c @@ -54,7 +54,7 @@ uint8_t getTimerIndex(TIM_TypeDef *timer) return dmaMotorTimerCount-1; } -void pwmWriteDigitalInt(uint8_t index, uint16_t value) +void pwmWriteDshotInt(uint8_t index, uint16_t value) { motorDmaOutput_t *const motor = &dmaMotors[index]; @@ -70,7 +70,7 @@ void pwmWriteDigitalInt(uint8_t index, uint16_t value) DMA_Cmd(motor->timerHardware->dmaRef, ENABLE); } -void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) +void pwmCompleteDshotMotorUpdate(uint8_t motorCount) { UNUSED(motorCount); @@ -94,7 +94,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) } } -void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) +void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) { TIM_OCInitTypeDef TIM_OCInitStructure; DMA_InitTypeDef DMA_InitStructure; diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c index de270044bc..c4a65cfed4 100644 --- a/src/main/drivers/pwm_output_dshot_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal.c @@ -49,7 +49,7 @@ uint8_t getTimerIndex(TIM_TypeDef *timer) return dmaMotorTimerCount - 1; } -void pwmWriteDigitalInt(uint8_t index, uint16_t value) +void pwmWriteDshotInt(uint8_t index, uint16_t value) { motorDmaOutput_t *const motor = &dmaMotors[index]; @@ -74,7 +74,7 @@ void pwmWriteDigitalInt(uint8_t index, uint16_t value) } } -void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) +void pwmCompleteDshotMotorUpdate(uint8_t motorCount) { UNUSED(motorCount); } @@ -85,7 +85,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) HAL_DMA_IRQHandler(motor->TimHandle.hdma[motor->timerDmaSource]); } -void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) +void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) { motorDmaOutput_t * const motor = &dmaMotors[motorIndex]; motor->timerHardware = timerHardware; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index fa3382475a..4f8d8c2d44 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -336,23 +336,6 @@ bool mixerIsOutputSaturated(int axis, float errorRate) return false; } -bool isMotorProtocolDshot(void) { -#ifdef USE_DSHOT - switch(motorConfig()->dev.motorPwmProtocol) { - case PWM_TYPE_PROSHOT1000: - case PWM_TYPE_DSHOT1200: - case PWM_TYPE_DSHOT600: - case PWM_TYPE_DSHOT300: - case PWM_TYPE_DSHOT150: - return true; - default: - return false; - } -#else - return false; -#endif -} - // All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator // DSHOT scaling is done to the actual dshot range void initEscEndpoints(void) { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 665b39fd25..64535fd7c4 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -25,24 +25,6 @@ #define BRUSHED_MOTORS_PWM_RATE 16000 #define BRUSHLESS_MOTORS_PWM_RATE 480 -/* - DshotSettingRequest (KISS24). Spin direction, 3d and save Settings reqire 10 requests.. and the TLM Byte must always be high if 1-47 are used to send settings - 0 = stop - 1-5: beep - 6: ESC info request (FW Version and SN sent over the tlm wire) - 7: spin direction 1 - 8: spin direction 2 - 9: 3d mode off - 10: 3d mode on - 11: ESC settings request (saved settings over the TLM wire) - 12: save Settings - - 3D Mode: - 0 = stop - 48 (low) - 1047 (high) -> negative direction - 1048 (low) - 2047 (high) -> positive direction -*/ - // Digital protocol has fixed values #define DSHOT_DISARM_COMMAND 0 #define DSHOT_MIN_THROTTLE 48 @@ -140,6 +122,5 @@ void writeMotors(void); void stopMotors(void); void stopPwmAllMotors(void); -bool isMotorProtocolDshot(void); float convertExternalToMotor(uint16_t externalValue); uint16_t convertMotorToExternal(float motorValue); diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index bb0d756b00..efe3e18657 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -392,6 +392,10 @@ pwmOutputPort_t *pwmGetMotors(void) { bool pwmAreMotorsEnabled(void) { return pwmMotorsEnabled; } +bool isMotorProtocolDshot(void) +{ + return false; +} void pwmWriteMotor(uint8_t index, float value) { motorsPwm[index] = value - idlePulse; } From 7396e2665d1ec7063547c9883eab1af7c788dc51 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 25 Jun 2017 16:49:49 +1200 Subject: [PATCH 15/18] Removed 'boxId' from 'rcsplitSwitchState_t', cleaned up naming. --- src/main/io/rcsplit.c | 5 ++--- src/main/io/rcsplit.h | 9 ++++----- src/test/unit/rcsplit_unittest.cc | 6 +++--- 3 files changed, 9 insertions(+), 11 deletions(-) diff --git a/src/main/io/rcsplit.c b/src/main/io/rcsplit.c index e9533bed6d..774d40aa6d 100644 --- a/src/main/io/rcsplit.c +++ b/src/main/io/rcsplit.c @@ -40,8 +40,8 @@ // communicate with camera device variables serialPort_t *rcSplitSerialPort = NULL; -rcsplit_switch_state_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1]; -rcsplit_state_e cameraState = RCSPLIT_STATE_UNKNOWN; +rcsplitSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1]; +rcsplitState_e cameraState = RCSPLIT_STATE_UNKNOWN; static uint8_t crc_high_first(uint8_t *ptr, uint8_t len) { @@ -138,7 +138,6 @@ bool rcSplitInit(void) // set init value to true, to avoid the action auto run when the flight board start and the switch is on. for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) { uint8_t switchIndex = i - BOXCAMERA1; - switchStates[switchIndex].boxId = 1 << i; switchStates[switchIndex].isActivated = true; } diff --git a/src/main/io/rcsplit.h b/src/main/io/rcsplit.h index 6900700eee..85b4d347a6 100644 --- a/src/main/io/rcsplit.h +++ b/src/main/io/rcsplit.h @@ -22,15 +22,14 @@ #include "fc/fc_msp.h" typedef struct { - uint8_t boxId; bool isActivated; -} rcsplit_switch_state_t; +} rcsplitSwitchState_t; typedef enum { RCSPLIT_STATE_UNKNOWN = 0, RCSPLIT_STATE_INITIALIZING, RCSPLIT_STATE_IS_READY, -} rcsplit_state_e; +} rcsplitState_e; // packet header and tail #define RCSPLIT_PACKET_HEADER 0x55 @@ -51,6 +50,6 @@ bool rcSplitInit(void); void rcSplitProcess(timeUs_t currentTimeUs); // only for unit test -extern rcsplit_state_e cameraState; +extern rcsplitState_e cameraState; extern serialPort_t *rcSplitSerialPort; -extern rcsplit_switch_state_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1]; +extern rcsplitSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1]; diff --git a/src/test/unit/rcsplit_unittest.cc b/src/test/unit/rcsplit_unittest.cc index 67be33ae78..59364ce299 100644 --- a/src/test/unit/rcsplit_unittest.cc +++ b/src/test/unit/rcsplit_unittest.cc @@ -47,7 +47,7 @@ extern "C" { int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] - rcsplit_state_e unitTestRCsplitState() + rcsplitState_e unitTestRCsplitState() { return cameraState; } @@ -55,7 +55,7 @@ extern "C" { bool unitTestIsSwitchActivited(boxId_e boxId) { uint8_t adjustBoxID = boxId - BOXCAMERA1; - rcsplit_switch_state_t switchState = switchStates[adjustBoxID]; + rcsplitSwitchState_t switchState = switchStates[adjustBoxID]; return switchState.isActivated; } @@ -428,4 +428,4 @@ extern "C" { bool feature(uint32_t) { return false;} void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count) { UNUSED(instance); UNUSED(data); UNUSED(count); } -} \ No newline at end of file +} From b47df12759c8ad54505061b2d1b4def35f490f09 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 25 Jun 2017 10:12:21 +0100 Subject: [PATCH 16/18] Fix warnings in unit tests --- src/main/drivers/vtx_common.h | 2 +- src/test/unit/rx_ranges_unittest.cc | 64 ++++++++++++++--------------- src/test/unit/rx_rx_unittest.cc | 4 +- 3 files changed, 35 insertions(+), 35 deletions(-) diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index c3303ef27f..d72bf79271 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -35,7 +35,7 @@ typedef struct vtxDeviceCapability_s { } vtxDeviceCapability_t; typedef struct vtxDevice_s { - const struct vtxVTable_s const *vTable; + const struct vtxVTable_s * const vTable; vtxDeviceCapability_t capability; diff --git a/src/test/unit/rx_ranges_unittest.cc b/src/test/unit/rx_ranges_unittest.cc index d86d1db04e..5864a6639c 100644 --- a/src/test/unit/rx_ranges_unittest.cc +++ b/src/test/unit/rx_ranges_unittest.cc @@ -49,51 +49,51 @@ TEST(RxChannelRangeTest, TestRxChannelRanges) rcModeActivationMask = DE_ACTIVATE_ALL_BOXES; // BOXFAILSAFE must be OFF // No signal, special condition - EXPECT_EQ(applyRxChannelRangeConfiguraton(0, RANGE_CONFIGURATION(1000, 2000)), 0); - EXPECT_EQ(applyRxChannelRangeConfiguraton(0, RANGE_CONFIGURATION(1300, 1700)), 0); - EXPECT_EQ(applyRxChannelRangeConfiguraton(0, RANGE_CONFIGURATION(900, 2100)), 0); + EXPECT_EQ(0, applyRxChannelRangeConfiguraton(0, RANGE_CONFIGURATION(1000, 2000))); + EXPECT_EQ(0, applyRxChannelRangeConfiguraton(0, RANGE_CONFIGURATION(1300, 1700))); + EXPECT_EQ(0, applyRxChannelRangeConfiguraton(0, RANGE_CONFIGURATION(900, 2100))); // Exact mapping - EXPECT_EQ(applyRxChannelRangeConfiguraton(1000, RANGE_CONFIGURATION(1000, 2000)), 1000); - EXPECT_EQ(applyRxChannelRangeConfiguraton(1500, RANGE_CONFIGURATION(1000, 2000)), 1500); - EXPECT_EQ(applyRxChannelRangeConfiguraton(2000, RANGE_CONFIGURATION(1000, 2000)), 2000); - EXPECT_EQ(applyRxChannelRangeConfiguraton(700, RANGE_CONFIGURATION(1000, 2000)), 750); - EXPECT_EQ(applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(1000, 2000)), 2250); + EXPECT_EQ(1000, applyRxChannelRangeConfiguraton(1000, RANGE_CONFIGURATION(1000, 2000))); + EXPECT_EQ(1500, applyRxChannelRangeConfiguraton(1500, RANGE_CONFIGURATION(1000, 2000))); + EXPECT_EQ(2000, applyRxChannelRangeConfiguraton(2000, RANGE_CONFIGURATION(1000, 2000))); + EXPECT_EQ(750, applyRxChannelRangeConfiguraton(700, RANGE_CONFIGURATION(1000, 2000))); + EXPECT_EQ(2250, applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(1000, 2000))); // Reversed channel - EXPECT_EQ(applyRxChannelRangeConfiguraton(1000, RANGE_CONFIGURATION(2000, 1000)), 2000); - EXPECT_EQ(applyRxChannelRangeConfiguraton(1500, RANGE_CONFIGURATION(2000, 1000)), 1500); - EXPECT_EQ(applyRxChannelRangeConfiguraton(2000, RANGE_CONFIGURATION(2000, 1000)), 1000); + EXPECT_EQ(2000, applyRxChannelRangeConfiguraton(1000, RANGE_CONFIGURATION(2000, 1000))); + EXPECT_EQ(1500, applyRxChannelRangeConfiguraton(1500, RANGE_CONFIGURATION(2000, 1000))); + EXPECT_EQ(1000, applyRxChannelRangeConfiguraton(2000, RANGE_CONFIGURATION(2000, 1000))); // Shifted range - EXPECT_EQ(applyRxChannelRangeConfiguraton(900, RANGE_CONFIGURATION(900, 1900)), 1000); - EXPECT_EQ(applyRxChannelRangeConfiguraton(1400, RANGE_CONFIGURATION(900, 1900)), 1500); - EXPECT_EQ(applyRxChannelRangeConfiguraton(1900, RANGE_CONFIGURATION(900, 1900)), 2000); - EXPECT_EQ(applyRxChannelRangeConfiguraton(600, RANGE_CONFIGURATION(900, 1900)), 750); - EXPECT_EQ(applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(900, 1900)), 2250); + EXPECT_EQ(1000, applyRxChannelRangeConfiguraton(900, RANGE_CONFIGURATION(900, 1900))); + EXPECT_EQ(1500, applyRxChannelRangeConfiguraton(1400, RANGE_CONFIGURATION(900, 1900))); + EXPECT_EQ(2000, applyRxChannelRangeConfiguraton(1900, RANGE_CONFIGURATION(900, 1900))); + EXPECT_EQ(750, applyRxChannelRangeConfiguraton(600, RANGE_CONFIGURATION(900, 1900))); + EXPECT_EQ(2250, applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(900, 1900))); // Narrower range than expected - EXPECT_EQ(applyRxChannelRangeConfiguraton(1300, RANGE_CONFIGURATION(1300, 1700)), 1000); - EXPECT_EQ(applyRxChannelRangeConfiguraton(1500, RANGE_CONFIGURATION(1300, 1700)), 1500); - EXPECT_EQ(applyRxChannelRangeConfiguraton(1700, RANGE_CONFIGURATION(1300, 1700)), 2000); - EXPECT_EQ(applyRxChannelRangeConfiguraton(700, RANGE_CONFIGURATION(1300, 1700)), 750); - EXPECT_EQ(applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(1300, 1700)), 2250); + EXPECT_EQ(1000, applyRxChannelRangeConfiguraton(1300, RANGE_CONFIGURATION(1300, 1700))); + EXPECT_EQ(1500, applyRxChannelRangeConfiguraton(1500, RANGE_CONFIGURATION(1300, 1700))); + EXPECT_EQ(2000, applyRxChannelRangeConfiguraton(1700, RANGE_CONFIGURATION(1300, 1700))); + EXPECT_EQ(750, applyRxChannelRangeConfiguraton(700, RANGE_CONFIGURATION(1300, 1700))); + EXPECT_EQ(2250, applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(1300, 1700))); // Wider range than expected - EXPECT_EQ(applyRxChannelRangeConfiguraton(900, RANGE_CONFIGURATION(900, 2100)), 1000); - EXPECT_EQ(applyRxChannelRangeConfiguraton(1500, RANGE_CONFIGURATION(900, 2100)), 1500); - EXPECT_EQ(applyRxChannelRangeConfiguraton(2100, RANGE_CONFIGURATION(900, 2100)), 2000); - EXPECT_EQ(applyRxChannelRangeConfiguraton(600, RANGE_CONFIGURATION(900, 2100)), 750); - EXPECT_EQ(applyRxChannelRangeConfiguraton(2700, RANGE_CONFIGURATION(900, 2100)), 2250); + EXPECT_EQ(1000, applyRxChannelRangeConfiguraton(900, RANGE_CONFIGURATION(900, 2100))); + EXPECT_EQ(1500, applyRxChannelRangeConfiguraton(1500, RANGE_CONFIGURATION(900, 2100))); + EXPECT_EQ(2000, applyRxChannelRangeConfiguraton(2100, RANGE_CONFIGURATION(900, 2100))); + EXPECT_EQ(750, applyRxChannelRangeConfiguraton(600, RANGE_CONFIGURATION(900, 2100))); + EXPECT_EQ(2250, applyRxChannelRangeConfiguraton(2700, RANGE_CONFIGURATION(900, 2100))); // extreme out of range - EXPECT_EQ(applyRxChannelRangeConfiguraton(1, RANGE_CONFIGURATION(1000, 2000)), 750); - EXPECT_EQ(applyRxChannelRangeConfiguraton(1, RANGE_CONFIGURATION(1300, 1700)), 750); - EXPECT_EQ(applyRxChannelRangeConfiguraton(1, RANGE_CONFIGURATION(900, 2100)), 750); + EXPECT_EQ(750, applyRxChannelRangeConfiguraton(1, RANGE_CONFIGURATION(1000, 2000))); + EXPECT_EQ(750, applyRxChannelRangeConfiguraton(1, RANGE_CONFIGURATION(1300, 1700))); + EXPECT_EQ(750, applyRxChannelRangeConfiguraton(1, RANGE_CONFIGURATION(900, 2100))); - EXPECT_EQ(applyRxChannelRangeConfiguraton(10000, RANGE_CONFIGURATION(1000, 2000)), 2250); - EXPECT_EQ(applyRxChannelRangeConfiguraton(10000, RANGE_CONFIGURATION(1300, 1700)), 2250); - EXPECT_EQ(applyRxChannelRangeConfiguraton(10000, RANGE_CONFIGURATION(900, 2100)), 2250); + EXPECT_EQ(2250, applyRxChannelRangeConfiguraton(10000, RANGE_CONFIGURATION(1000, 2000))); + EXPECT_EQ(2250, applyRxChannelRangeConfiguraton(10000, RANGE_CONFIGURATION(1300, 1700))); + EXPECT_EQ(2250, applyRxChannelRangeConfiguraton(10000, RANGE_CONFIGURATION(900, 2100))); } diff --git a/src/test/unit/rx_rx_unittest.cc b/src/test/unit/rx_rx_unittest.cc index 5daad55c57..5d6b14d141 100644 --- a/src/test/unit/rx_rx_unittest.cc +++ b/src/test/unit/rx_rx_unittest.cc @@ -33,7 +33,7 @@ extern "C" { #include "config/parameter_group_ids.h" #include "io/beeper.h" - uint32_t rcModeActivationMask; + boxBitmask_t rcModeActivationMask; void rxResetFlightChannelStatus(void); bool rxHaveValidFlightChannels(void); @@ -63,7 +63,7 @@ TEST(RxTest, TestValidFlightChannels) { // given memset(&testData, 0, sizeof(testData)); - rcModeActivationMask = DE_ACTIVATE_ALL_BOXES; // BOXFAILSAFE must be OFF + memset(&rcModeActivationMask, 0, sizeof(rcModeActivationMask)); // BOXFAILSAFE must be OFF // and rxConfig_t rxConfig; From 7bd05b21483d6419dca3f77442a9b3326d4cf72c Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 25 Jun 2017 21:18:39 +1200 Subject: [PATCH 17/18] Refactored Dshot reverse condition block. --- src/main/fc/fc_core.c | 28 +++++++++++++++------------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 9f75443241..90a5c22e56 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -206,21 +206,23 @@ void mwArm(void) return; } if (!ARMING_FLAG(PREVENT_ARMING)) { - #ifdef USE_DSHOT - //TODO: Use BOXDSHOTREVERSE here - if (!feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { - reverseMotors = false; - for (unsigned index = 0; index < getMotorCount(); index++) { - pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL); +#ifdef USE_DSHOT + if (!feature(FEATURE_3D)) { + //TODO: Use BOXDSHOTREVERSE here + if (!IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { + reverseMotors = false; + for (unsigned index = 0; index < getMotorCount(); index++) { + pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL); + } + } else { + reverseMotors = true; + for (unsigned index = 0; index < getMotorCount(); index++) { + pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED); + } } } - if (!feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { - reverseMotors = true; - for (unsigned index = 0; index < getMotorCount(); index++) { - pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED); - } - } - #endif +#endif + ENABLE_ARMING_FLAG(ARMED); ENABLE_ARMING_FLAG(WAS_EVER_ARMED); headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); From ccb30565f1e6313d7ca76d8a3639741a21c01f35 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 25 Jun 2017 12:28:15 +0100 Subject: [PATCH 18/18] Moved busDevice_t out of sensor.h into bus.h --- src/main/drivers/accgyro/accgyro.h | 1 + src/main/drivers/accgyro/accgyro_mpu.h | 1 + src/main/drivers/accgyro/accgyro_spi_bmi160.h | 2 +- src/main/drivers/accgyro/accgyro_spi_icm20689.h | 2 +- src/main/drivers/accgyro/accgyro_spi_mpu6000.h | 2 +- src/main/drivers/accgyro/accgyro_spi_mpu6500.h | 2 +- src/main/drivers/accgyro/accgyro_spi_mpu9250.h | 2 +- src/main/drivers/bus.h | 8 ++++++++ src/main/drivers/compass/compass.h | 1 + src/main/drivers/sensor.h | 8 -------- src/main/sensors/gyro.h | 1 + 11 files changed, 17 insertions(+), 13 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index f0c35e35bb..e78952083f 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -20,6 +20,7 @@ #include "platform.h" #include "common/axis.h" #include "drivers/exti.h" +#include "drivers/bus.h" #include "drivers/sensor.h" #include "drivers/accgyro/accgyro_mpu.h" #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD) diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 8b1ea5063b..07910797a0 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -17,6 +17,7 @@ #pragma once +#include "drivers/bus.h" #include "drivers/exti.h" #include "drivers/sensor.h" diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi160.h b/src/main/drivers/accgyro/accgyro_spi_bmi160.h index fad3dcd0bf..1c51c23c93 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi160.h +++ b/src/main/drivers/accgyro/accgyro_spi_bmi160.h @@ -34,7 +34,7 @@ #pragma once -#include "drivers/sensor.h" +#include "drivers/bus.h" enum pios_bmi160_orientation { // clockwise rotation from board forward PIOS_BMI160_TOP_0DEG, diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.h b/src/main/drivers/accgyro/accgyro_spi_icm20689.h index 0a3d862f6e..9eeb0c24a4 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.h +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.h @@ -16,7 +16,7 @@ */ #pragma once -#include "drivers/sensor.h" +#include "drivers/bus.h" #define ICM20689_WHO_AM_I_CONST (0x98) #define ICM20689_BIT_RESET (0x80) diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.h b/src/main/drivers/accgyro/accgyro_spi_mpu6000.h index 2c0fa464ca..7eb9b6918c 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.h +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6000.h @@ -1,7 +1,7 @@ #pragma once -#include "drivers/sensor.h" +#include "drivers/bus.h" #define MPU6000_CONFIG 0x1A diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.h b/src/main/drivers/accgyro/accgyro_spi_mpu6500.h index e2d1ef91b2..a23d2607c7 100755 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.h +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6500.h @@ -17,7 +17,7 @@ #pragma once -#include "drivers/sensor.h" +#include "drivers/bus.h" uint8_t mpu6500SpiDetect(const busDevice_t *bus); diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.h b/src/main/drivers/accgyro/accgyro_spi_mpu9250.h index 14eba13434..4518bd8e26 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.h +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.h @@ -1,7 +1,7 @@ #pragma once -#include "drivers/sensor.h" +#include "drivers/bus.h" #define mpu9250_CONFIG 0x1A diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index d1c1961896..08de5e1a54 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -19,6 +19,14 @@ #include "platform.h" +#include "drivers/io_types.h" + +typedef union busDevice_u { + struct deviceSpi_s { + IO_t csnPin; + } spi; +} busDevice_t; + #ifdef TARGET_BUS_INIT void targetBusInit(void); #endif diff --git a/src/main/drivers/compass/compass.h b/src/main/drivers/compass/compass.h index 08ebc1f3c6..cb70068ba9 100644 --- a/src/main/drivers/compass/compass.h +++ b/src/main/drivers/compass/compass.h @@ -17,6 +17,7 @@ #pragma once +#include "drivers/bus.h" #include "drivers/sensor.h" typedef struct magDev_s { diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h index ecf59fd1d3..e39e401b21 100644 --- a/src/main/drivers/sensor.h +++ b/src/main/drivers/sensor.h @@ -20,8 +20,6 @@ #include #include -#include "drivers/io_types.h" - typedef enum { ALIGN_DEFAULT = 0, // driver-provided alignment CW0_DEG = 1, @@ -34,12 +32,6 @@ typedef enum { CW270_DEG_FLIP = 8 } sensor_align_e; -typedef union busDevice_t { - struct deviceSpi_s { - IO_t csnPin; - } spi; -} busDevice_t; - typedef bool (*sensorInitFuncPtr)(void); // sensor init prototype typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype typedef bool (*sensorInterruptFuncPtr)(void); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index e8b4365b64..ba7693e586 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -19,6 +19,7 @@ #include "common/axis.h" #include "config/parameter_group.h" +#include "drivers/bus.h" #include "drivers/sensor.h" typedef enum {