diff --git a/src/main/config/config.c b/src/main/config/config.c index 5ee80256e0..a7bb455416 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -334,6 +334,7 @@ static void resetConf(void) resetTelemetryConfig(&masterConfig.telemetryConfig); masterConfig.rxConfig.serialrx_provider = 0; + masterConfig.rxConfig.spektrum_sat_bind = 0; masterConfig.rxConfig.midrc = 1500; masterConfig.rxConfig.mincheck = 1100; masterConfig.rxConfig.maxcheck = 1900; diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 6a0f16c0dd..0996b2d4ea 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -30,6 +30,7 @@ void failureMode(uint8_t mode); // bootloader/IAP void systemReset(void); void systemResetToBootloader(void); +bool isMPUSoftReset(void); void enableGPIOPowerUsageAndNoiseReductions(void); // current crystal frequency - 8 or 12MHz diff --git a/src/main/drivers/system_stm32f10x.c b/src/main/drivers/system_stm32f10x.c index a212b75b6e..b3d887da70 100644 --- a/src/main/drivers/system_stm32f10x.c +++ b/src/main/drivers/system_stm32f10x.c @@ -22,11 +22,19 @@ #include "platform.h" #include "gpio.h" +#include "system.h" #define AIRCR_VECTKEY_MASK ((uint32_t)0x05FA0000) +#define BKP_SOFTRESET (0x50F7B007) void systemReset(void) { + // write magic value that we're doing a soft reset + RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR | RCC_APB1Periph_BKP, ENABLE); + PWR->CR |= PWR_CR_DBP; + *((uint16_t *)BKP_BASE + 0x04) = BKP_SOFTRESET & 0xffff; + *((uint16_t *)BKP_BASE + 0x08) = (BKP_SOFTRESET & 0xffff0000) >> 16; + // Generate system reset SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04; } @@ -52,3 +60,11 @@ void enableGPIOPowerUsageAndNoiseReductions(void) gpioInit(GPIOB, &gpio); gpioInit(GPIOC, &gpio); } + +bool isMPUSoftReset(void) +{ + if ((*((uint16_t *)BKP_BASE + 0x04) | *((uint16_t *)BKP_BASE + 0x08) << 16) == BKP_SOFTRESET) + return true; + else + return false; +} diff --git a/src/main/drivers/system_stm32f30x.c b/src/main/drivers/system_stm32f30x.c index b530e4802a..8dcae72937 100644 --- a/src/main/drivers/system_stm32f30x.c +++ b/src/main/drivers/system_stm32f30x.c @@ -67,3 +67,11 @@ void enableGPIOPowerUsageAndNoiseReductions(void) gpioInit(GPIOE, &gpio); gpioInit(GPIOF, &gpio); } + +bool isMPUSoftReset(void) +{ + if (RCC->CSR & RCC_CSR_SFTRSTF) + return true; + else + return false; +} diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 0d8131f150..6d4295c754 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -51,6 +51,7 @@ #include "io/rc_controls.h" #include "io/serial.h" #include "io/ledstrip.h" +#include "rx/spektrum.h" #include "sensors/battery.h" #include "sensors/boardalignment.h" #include "sensors/sensors.h" @@ -271,6 +272,7 @@ const clivalue_t valueTable[] = { #endif { "serialrx_provider", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.serialrx_provider, 0, SERIALRX_PROVIDER_MAX }, + { "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.spektrum_sat_bind, SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX}, { "telemetry_provider", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.telemetry_provider, 0, TELEMETRY_PROVIDER_MAX }, { "telemetry_switch", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.telemetry_switch, 0, 1 }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 59b5ab49b1..697a75f8c1 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1016,6 +1016,8 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(masterConfig.rxConfig.maxcheck); serialize16(masterConfig.rxConfig.midrc); serialize16(masterConfig.rxConfig.mincheck); + serialize8(masterConfig.rxConfig.spektrum_sat_bind); + break; case MSP_RSSI_CONFIG: headSerialReply(1); @@ -1291,6 +1293,7 @@ static bool processInCommand(void) masterConfig.rxConfig.maxcheck = read16(); masterConfig.rxConfig.midrc = read16(); masterConfig.rxConfig.mincheck = read16(); + masterConfig.rxConfig.spektrum_sat_bind = read8(); break; case MSP_SET_RSSI_CONFIG: diff --git a/src/main/main.c b/src/main/main.c index be109e3395..a68b97ddf0 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -105,7 +105,7 @@ void imuInit(void); void displayInit(rxConfig_t *intialRxConfig); void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse); void loop(void); - +void spektrumBind(rxConfig_t *rxConfig); #ifdef STM32F303xC // from system_stm32f30x.c @@ -150,6 +150,20 @@ void init(void) systemInit(); +#ifdef SPEKTRUM_BIND + if (feature(FEATURE_RX_SERIAL)) { + switch (masterConfig.rxConfig.serialrx_provider) { + case SERIALRX_SPEKTRUM1024: + case SERIALRX_SPEKTRUM2048: + // 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() + spektrumBind(&masterConfig.rxConfig); + break; + } + } +#endif + delay(100); timerInit(); // timer must be initialized before any channel is allocated diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 2c6807c3d1..7d06f8e9d1 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -65,6 +65,7 @@ extern int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2 typedef struct rxConfig_s { uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first. + uint8_t spektrum_sat_bind; // number of bind pulses for Spektrum satellite receivers uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here uint16_t mincheck; // minimum rc end uint16_t maxcheck; // maximum rc end diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index f1e756dbad..d933c3ce25 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -21,12 +21,15 @@ #include "platform.h" +#include "drivers/gpio.h" #include "drivers/system.h" #include "drivers/serial.h" #include "drivers/serial_uart.h" #include "io/serial.h" +#include "config/config.h" + #include "rx/rx.h" #include "rx/spektrum.h" @@ -138,3 +141,78 @@ static uint16_t spektrumReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t ch return data; } + +#ifdef SPEKTRUM_BIND + +bool spekShouldBind(uint8_t spektrum_sat_bind) +{ +#ifdef HARDWARE_BIND_PLUG + gpio_config_t cfg = { + BINDPLUG_PIN, + Mode_IPU, + Speed_2MHz + }; + gpioInit(BINDPLUG_PORT, &cfg); + + // Check status of bind plug and exit if not active + delayMicroseconds(10); // allow configuration to settle + if (digitalIn(BINDPLUG_PORT, BINDPLUG_PIN)) { + return false; + } +#endif + + return !( + isMPUSoftReset() || + spektrum_sat_bind == SPEKTRUM_SAT_BIND_DISABLED || + 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. + * 9 = DSMX 11ms / DSMX 22ms + * 5 = DSM2 11ms 2048 / DSM2 22ms 1024 + */ +void spektrumBind(rxConfig_t *rxConfig) +{ + int i; + + if (!spekShouldBind(rxConfig->spektrum_sat_bind)) { + return; + } + + gpio_config_t cfg = { + BIND_PIN, + Mode_Out_OD, + Speed_2MHz + }; + gpioInit(BIND_PORT, &cfg); + + // RX line, set high + digitalHi(BIND_PORT, BIND_PIN); + + // Bind window is around 20-140ms after powerup + delay(60); + + for (i = 0; i < rxConfig->spektrum_sat_bind; i++) { + + // RX line, drive low for 120us + digitalLo(BIND_PORT, BIND_PIN); + delayMicroseconds(120); + + // RX line, drive high for 120us + digitalHi(BIND_PORT, BIND_PIN); + delayMicroseconds(120); + } + +#ifndef HARDWARE_BIND_PLUG + // 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 + if (!isMPUSoftReset()) { + rxConfig->spektrum_sat_bind = 0; + saveConfigAndNotify(); + } +#endif + +} +#endif diff --git a/src/main/rx/spektrum.h b/src/main/rx/spektrum.h index 501d707c44..ea40e44aef 100644 --- a/src/main/rx/spektrum.h +++ b/src/main/rx/spektrum.h @@ -17,5 +17,8 @@ #pragma once +#define SPEKTRUM_SAT_BIND_DISABLED 0 +#define SPEKTRUM_SAT_BIND_MAX 10 + bool spektrumFrameComplete(void); void spektrumUpdateSerialRxFunctionConstraint(functionConstraint_t *functionConstraint); diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index d933362b1d..e8607dd059 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -76,3 +76,8 @@ #define TELEMETRY #define SERIAL_RX #define AUTOTUNE + +#define SPEKTRUM_BIND +// USART3, PB11 (Flexport) +#define BIND_PORT GPIOB +#define BIND_PIN Pin_11 diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index 0e2da29e77..60e74e528f 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -61,3 +61,8 @@ #define SENSORS_SET (SENSOR_ACC | SENSOR_MAG) #define SERIAL_RX + +#define SPEKTRUM_BIND +// USART2, PA3 +#define BIND_PORT GPIOA +#define BIND_PIN Pin_3 diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index e99bd4575a..7bdc8eed31 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -99,3 +99,7 @@ #define SERIAL_RX #define AUTOTUNE +#define SPEKTRUM_BIND +// USART2, PA3 +#define BIND_PORT GPIOA +#define BIND_PIN Pin_3 diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 8fc63bb802..3226382e37 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -112,3 +112,8 @@ #define TELEMETRY #define SERIAL_RX #define AUTOTUNE + +#define SPEKTRUM_BIND +// USART2, PA3 +#define BIND_PORT GPIOA +#define BIND_PIN Pin_3 diff --git a/src/main/target/NAZE32PRO/target.h b/src/main/target/NAZE32PRO/target.h index 12ae41b82f..5376ef9aaa 100644 --- a/src/main/target/NAZE32PRO/target.h +++ b/src/main/target/NAZE32PRO/target.h @@ -46,3 +46,8 @@ #define TELEMETRY #define SERIAL_RX #define AUTOTUNE + +#define SPEKTRUM_BIND +// USART2, PA3 +#define BIND_PORT GPIOA +#define BIND_PIN Pin_3 diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 156986f876..a26c9eaa98 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -82,3 +82,7 @@ #define GPS #define DISPLAY +#define SPEKTRUM_BIND +// USART2, PA3 +#define BIND_PORT GPIOA +#define BIND_PIN Pin_3