Automatic spektrum bind pin determination
This commit is contained in:
parent
e7a02c819c
commit
c7f62e47a6
|
@ -61,6 +61,7 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
|
|||
"LED_STRIP",
|
||||
"TRANSPONDER",
|
||||
"VTX",
|
||||
"COMPASS_CS"
|
||||
"COMPASS_CS",
|
||||
"RX_BIND_PLUG",
|
||||
};
|
||||
|
||||
|
|
|
@ -62,6 +62,7 @@ typedef enum {
|
|||
OWNER_TRANSPONDER,
|
||||
OWNER_VTX,
|
||||
OWNER_COMPASS_CS,
|
||||
OWNER_RX_BIND_PLUG,
|
||||
OWNER_TOTAL_COUNT
|
||||
} resourceOwner_e;
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -24,6 +24,7 @@
|
|||
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "drivers/io.h"
|
||||
#include "rx/rx.h"
|
||||
#include "rx/msp.h"
|
||||
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -28,6 +28,7 @@
|
|||
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/rx_nrf24l01.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
|
|
|
@ -28,6 +28,7 @@
|
|||
|
||||
#include "build/build_config.h"
|
||||
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/rx_nrf24l01.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
|
|
|
@ -30,6 +30,7 @@
|
|||
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/rx_nrf24l01.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
|
|
|
@ -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,
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -19,6 +19,7 @@
|
|||
#include <platform.h>
|
||||
|
||||
#ifdef TARGET_CONFIG
|
||||
#include "drivers/io.h"
|
||||
#include "rx/rx.h"
|
||||
#include "io/serial.h"
|
||||
|
||||
|
|
|
@ -19,6 +19,7 @@
|
|||
#include <platform.h>
|
||||
|
||||
#ifdef TARGET_CONFIG
|
||||
#include "drivers/io.h"
|
||||
#include "rx/rx.h"
|
||||
#include "io/serial.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
|
||||
|
||||
|
|
|
@ -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"
|
||||
}
|
||||
|
|
|
@ -31,6 +31,7 @@ extern "C" {
|
|||
|
||||
#include "sensors/battery.h"
|
||||
|
||||
#include "drivers/io.h"
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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"
|
||||
|
|
Loading…
Reference in New Issue