Added command to bind RX to MSP.

This commit is contained in:
mikeller 2020-02-03 00:53:43 +13:00 committed by Michael Keller
parent 97704dda58
commit 39b62cb6c1
8 changed files with 174 additions and 63 deletions

View File

@ -108,6 +108,7 @@ COMMON_SRC = \
rx/msp.c \
rx/pwm.c \
rx/rx.c \
rx/rx_bind.c \
rx/rx_spi.c \
rx/rx_spi_common.c \
rx/crsf.c \
@ -345,7 +346,8 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
io/vtx_control.c \
io/spektrum_vtx_control.c \
osd/osd.c \
osd/osd_elements.c
osd/osd_elements.c \
rx/rx_bind.c
# F4 and F7 optimizations
ifneq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))

View File

@ -142,9 +142,8 @@ bool cliMode = false;
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pg/rx.h"
#include "pg/rx_spi_cc2500.h"
#include "pg/rx_spi.h"
#include "pg/rx_pwm.h"
#include "pg/rx_spi_cc2500.h"
#include "pg/serial_uart.h"
#include "pg/sdio.h"
#include "pg/timerio.h"
@ -152,10 +151,8 @@ bool cliMode = false;
#include "pg/usb.h"
#include "pg/vtx_table.h"
#include "rx/rx.h"
#include "rx/spektrum.h"
#include "rx/rx_spi_common.h"
#include "rx/srxl2.h"
#include "rx/rx_bind.h"
#include "rx/rx_spi.h"
#include "scheduler/scheduler.h"
@ -3343,62 +3340,13 @@ static void cliBeeper(char *cmdline)
}
#endif
#if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2)
#if defined(USE_RX_BIND)
void cliRxBind(char *cmdline){
UNUSED(cmdline);
switch (rxRuntimeState.rxProvider) {
default:
if (!startRxBind()) {
cliPrint("Not supported.");
break;
case RX_PROVIDER_SERIAL:
switch (rxRuntimeState.serialrxProvider) {
default:
cliPrint("Not supported.");
break;
#if defined(USE_SERIALRX_SRXL2)
case SERIALRX_SRXL2:
srxl2Bind();
cliPrint("Binding SRXL2 receiver...");
break;
#endif
}
break;
#if defined(USE_RX_SPI)
case RX_PROVIDER_SPI:
switch (rxSpiConfig()->rx_spi_protocol) {
default:
cliPrint("Not supported.");
break;
#if defined(USE_RX_FRSKY_SPI)
#if defined(USE_RX_FRSKY_SPI_D)
case RX_SPI_FRSKY_D:
#endif
#if defined(USE_RX_FRSKY_SPI_X)
case RX_SPI_FRSKY_X:
case RX_SPI_FRSKY_X_LBT:
#endif
#endif // USE_RX_FRSKY_SPI
#ifdef USE_RX_SFHSS_SPI
case RX_SPI_SFHSS:
#endif
#ifdef USE_RX_FLYSKY
case RX_SPI_A7105_FLYSKY:
case RX_SPI_A7105_FLYSKY_2A:
#endif
#ifdef USE_RX_SPEKTRUM
case RX_SPI_CYRF6936_DSM:
#endif
#if defined(USE_RX_FRSKY_SPI) || defined(USE_RX_SFHSS_SPI) || defined(USE_RX_FLYSKY) || defined(USE_RX_SPEKTRUM)
rxSpiBind();
} else {
cliPrint("Binding...");
break;
#endif
}
break;
#endif
}
}
#endif
@ -6260,7 +6208,7 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("beeper", "enable/disable beeper for a condition", "list\r\n"
"\t<->[name]", cliBeeper),
#endif // USE_BEEPER
#if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2)
#if defined(USE_RX_BIND)
CLI_COMMAND_DEF("bind_rx", "initiate binding for RX SPI or SRXL2", NULL, cliRxBind),
#endif
#if defined(USE_FLASH_BOOT_LOADER)

View File

@ -103,6 +103,7 @@
#include "msp/msp_box.h"
#include "msp/msp_protocol.h"
#include "msp/msp_protocol_v2_betaflight.h"
#include "msp/msp_protocol_v2_common.h"
#include "msp/msp_serial.h"
@ -120,6 +121,7 @@
#include "pg/vtx_table.h"
#include "rx/rx.h"
#include "rx/rx_bind.h"
#include "rx/msp.h"
#include "scheduler/scheduler.h"
@ -599,6 +601,7 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce
#define TARGET_HAS_FLASH_BOOTLOADER_BIT 3
#define TARGET_SUPPORTS_CUSTOM_DEFAULTS_BIT 4
#define TARGET_HAS_CUSTOM_DEFAULTS_BIT 5
#define TARGET_SUPPORTS_RX_BIND_BIT 6
uint8_t targetCapabilities = 0;
#ifdef USE_VCP
@ -619,6 +622,9 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce
targetCapabilities |= 1 << TARGET_HAS_CUSTOM_DEFAULTS_BIT;
}
#endif
#if defined(USE_RX_BIND)
targetCapabilities |= (getRxBindSupported() << TARGET_SUPPORTS_RX_BIND_BIT);
#endif
sbufWriteU8(dst, targetCapabilities);
@ -3233,6 +3239,14 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
break;
#endif
#endif // USE_BOARD_INFO
#if defined(USE_RX_BIND)
case MSP2_BETAFLIGHT_BIND:
if (!startRxBind()) {
return MSP_RESULT_ERROR;
}
break;
#endif
default:
// we do not know how to handle the (valid) message, indicate error MSP $M!
return MSP_RESULT_ERROR;

View File

@ -0,0 +1,21 @@
/*
* This file is part of Cleanflight, Betaflight and INAV.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight, Betaflight and INAV are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#define MSP2_BETAFLIGHT_BIND 0x3000

View File

@ -33,15 +33,14 @@
#include "common/utils.h"
#include "common/filter.h"
#include "config/config.h"
#include "config/config_reset.h"
#include "config/feature.h"
#include "drivers/adc.h"
#include "drivers/rx/rx_pwm.h"
#include "drivers/rx/rx_spi.h"
#include "drivers/time.h"
#include "config/config.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"

99
src/main/rx/rx_bind.c Normal file
View File

@ -0,0 +1,99 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include "platform.h"
#if defined(USE_RX_BIND)
#include "rx/rx_spi_common.h"
#include "rx/srxl2.h"
#include "rx_bind.h"
static bool doRxBind(bool doBind)
{
switch (rxRuntimeState.rxProvider) {
default:
return false;
case RX_PROVIDER_SERIAL:
switch (rxRuntimeState.serialrxProvider) {
default:
return false;
#if defined(USE_SERIALRX_SRXL2)
case SERIALRX_SRXL2:
if (doBind) {
srxl2Bind();
}
break;
#endif
}
break;
#if defined(USE_RX_SPI)
case RX_PROVIDER_SPI:
switch (rxSpiConfig()->rx_spi_protocol) {
default:
return false;
#if defined(USE_RX_FRSKY_SPI)
#if defined(USE_RX_FRSKY_SPI_D)
case RX_SPI_FRSKY_D:
#endif
#if defined(USE_RX_FRSKY_SPI_X)
case RX_SPI_FRSKY_X:
case RX_SPI_FRSKY_X_LBT:
#endif
#endif // USE_RX_FRSKY_SPI
#ifdef USE_RX_SFHSS_SPI
case RX_SPI_SFHSS:
#endif
#ifdef USE_RX_FLYSKY
case RX_SPI_A7105_FLYSKY:
case RX_SPI_A7105_FLYSKY_2A:
#endif
#ifdef USE_RX_SPEKTRUM
case RX_SPI_CYRF6936_DSM:
#endif
#if defined(USE_RX_FRSKY_SPI) || defined(USE_RX_SFHSS_SPI) || defined(USE_RX_FLYSKY) || defined(USE_RX_SPEKTRUM)
if (doBind) {
rxSpiBind();
}
break;
#endif
}
break;
#endif
}
return true;
}
bool startRxBind(void)
{
return doRxBind(true);
}
bool getRxBindSupported(void)
{
return doRxBind(false);
}
#endif

24
src/main/rx/rx_bind.h Normal file
View File

@ -0,0 +1,24 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
bool startRxBind(void);
bool getRxBindSupported(void);

View File

@ -377,3 +377,7 @@ extern uint8_t __config_end;
#if defined(USE_CUSTOM_DEFAULTS)
#define USE_CUSTOM_DEFAULTS_ADDRESS
#endif
#if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2)
#define USE_RX_BIND
#endif