diff --git a/make/source.mk b/make/source.mk
index 4f909f432c..d1ebf3ef9d 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -102,11 +102,13 @@ COMMON_SRC = \
io/rcdevice.c \
io/rcdevice_cam.c \
msp/msp_serial.c \
+ rx/crsf.c \
+ rx/eleres.c \
rx/fport.c \
rx/ibus.c \
rx/jetiexbus.c \
rx/msp.c \
- rx/uib_rx.c \
+ rx/msp_override.c \
rx/nrf24_cx10.c \
rx/nrf24_inav.c \
rx/nrf24_h8_3d.c \
@@ -115,14 +117,13 @@ COMMON_SRC = \
rx/pwm.c \
rx/rx.c \
rx/rx_spi.c \
- rx/crsf.c \
rx/sbus.c \
rx/sbus_channels.c \
rx/spektrum.c \
rx/sumd.c \
rx/sumh.c \
+ rx/uib_rx.c \
rx/xbus.c \
- rx/eleres.c \
scheduler/scheduler.c \
sensors/acceleration.c \
sensors/battery.c \
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index e9400d53af..84f3470d4b 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -65,6 +65,7 @@
#include "navigation/navigation.h"
#include "rx/rx.h"
+#include "rx/msp_override.h"
#include "sensors/diagnostics.h"
#include "sensors/acceleration.h"
@@ -363,6 +364,9 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
{"wind", 0, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
{"wind", 1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
{"wind", 2, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+ {"mspOverrideFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
+#endif
{"IMUTemperature", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
#ifdef USE_BARO
{"baroTemperature", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
@@ -475,6 +479,9 @@ typedef struct blackboxSlowState_s {
uint16_t powerSupplyImpedance;
uint16_t sagCompensatedVBat;
int16_t wind[XYZ_AXIS_COUNT];
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+ uint16_t mspOverrideFlags;
+#endif
int16_t imuTemperature;
#ifdef USE_BARO
int16_t baroTemperature;
@@ -1071,6 +1078,10 @@ static void writeSlowFrame(void)
blackboxWriteSigned16VBArray(slowHistory.wind, XYZ_AXIS_COUNT);
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+ blackboxWriteUnsignedVB(slowHistory.mspOverrideFlags);
+#endif
+
blackboxWriteSignedVB(slowHistory.imuTemperature);
#ifdef USE_BARO
@@ -1113,6 +1124,10 @@ static void loadSlowState(blackboxSlowState_t *slow)
#endif
}
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+ slow->mspOverrideFlags = (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) ? 2 : 0) + (mspOverrideIsInFailsafe() ? 1 : 0);
+#endif
+
bool valid_temp;
valid_temp = getIMUTemperature(&slow->imuTemperature);
if (!valid_temp) slow->imuTemperature = TEMPERATURE_INVALID_VALUE;
diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c
index 1ca0951ba6..e509eb9132 100755
--- a/src/main/cms/cms_menu_osd.c
+++ b/src/main/cms/cms_menu_osd.c
@@ -249,6 +249,10 @@ static const OSD_Entry menuOsdElemsEntries[] =
OSD_ELEMENT_ENTRY("G-FORCE Y", OSD_GFORCE),
OSD_ELEMENT_ENTRY("G-FORCE Z", OSD_GFORCE),
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+ OSD_ELEMENT_ENTRY("RC SOURCE", OSD_RC_SOURCE),
+#endif
+
OSD_ELEMENT_ENTRY("IMU TEMP", OSD_IMU_TEMPERATURE),
#ifdef USE_BARO
OSD_ELEMENT_ENTRY("BARO TEMP", OSD_BARO_TEMPERATURE),
@@ -268,7 +272,7 @@ static const OSD_Entry menuOsdElemsEntries[] =
OSD_BACK_AND_END_ENTRY,
};
-#if defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT) && defined(USE_TEMPERATURE_SENSOR)
+#if defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT) && defined(USE_TEMPERATURE_SENSOR) && defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
// All CMS OSD elements should be enabled in this case. The menu has 2 extra
// elements (label, back+end), but there's an OSD element that we intentionally
// don't show here (OSD_DEBUG).
diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index b8ff879a4c..03e7bd29eb 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -85,6 +85,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXUSER1, "USER1", 47 },
{ BOXUSER2, "USER2", 48 },
{ BOXLOITERDIRCHN, "LOITER CHANGE", 49 },
+ { BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@@ -292,6 +293,10 @@ void initActiveBoxIds(void)
#endif
#endif
#endif
+
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+ activeBoxIds[activeBoxIdCount++] = BOXMSPRCOVERRIDE;
+#endif
}
#define IS_ENABLED(mask) (mask == 0 ? 0 : 1)
@@ -305,49 +310,52 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
// Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
// Requires new Multiwii protocol version to fix
// It would be preferable to setting the enabled bits based on BOXINDEX.
- CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)), BOXANGLE);
- CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)), BOXHORIZON);
- CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HEADING_MODE)), BOXHEADINGHOLD);
- CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)), BOXHEADFREE);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)), BOXHEADADJ);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)), BOXCAMSTAB);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)), BOXFPVANGLEMIX);
- CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(MANUAL_MODE)), BOXMANUAL);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)), BOXBEEPERON);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)), BOXLEDLOW);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLIGHTS)), BOXLIGHTS);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)), BOXOSD);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)), BOXTELEMETRY);
- CHECK_ACTIVE_BOX(IS_ENABLED(ARMING_FLAG(ARMED)), BOXARM);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)), BOXBLACKBOX);
- CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)), BOXFAILSAFE);
- CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)), BOXNAVALTHOLD);
- CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_POSHOLD_MODE)), BOXNAVPOSHOLD);
- CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_CRUISE_MODE)), BOXNAVCRUISE);
- CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_RTH_MODE)), BOXNAVRTH);
- CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)), BOXNAVWP);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)), BOXAIRMODE);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGCSNAV)), BOXGCSNAV);
+ CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)), BOXANGLE);
+ CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)), BOXHORIZON);
+ CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HEADING_MODE)), BOXHEADINGHOLD);
+ CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)), BOXHEADFREE);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)), BOXHEADADJ);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)), BOXCAMSTAB);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)), BOXFPVANGLEMIX);
+ CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(MANUAL_MODE)), BOXMANUAL);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)), BOXBEEPERON);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)), BOXLEDLOW);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLIGHTS)), BOXLIGHTS);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)), BOXOSD);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)), BOXTELEMETRY);
+ CHECK_ACTIVE_BOX(IS_ENABLED(ARMING_FLAG(ARMED)), BOXARM);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)), BOXBLACKBOX);
+ CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)), BOXFAILSAFE);
+ CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)), BOXNAVALTHOLD);
+ CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_POSHOLD_MODE)), BOXNAVPOSHOLD);
+ CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_CRUISE_MODE)), BOXNAVCRUISE);
+ CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_RTH_MODE)), BOXNAVRTH);
+ CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)), BOXNAVWP);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)), BOXAIRMODE);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGCSNAV)), BOXGCSNAV);
#ifdef USE_FLM_FLAPERON
- CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FLAPERON)), BOXFLAPERON);
+ CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FLAPERON)), BOXFLAPERON);
#endif
- CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(TURN_ASSISTANT)), BOXTURNASSIST);
- CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_LAUNCH_MODE)), BOXNAVLAUNCH);
- CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(AUTO_TUNE)), BOXAUTOTUNE);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOTRIM)), BOXAUTOTRIM);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXKILLSWITCH)), BOXKILLSWITCH);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHOMERESET)), BOXHOMERESET);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMERA1)), BOXCAMERA1);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMERA2)), BOXCAMERA2);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMERA3)), BOXCAMERA3);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT1)), BOXOSDALT1);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT2)), BOXOSDALT2);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT3)), BOXOSDALT3);
+ CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(TURN_ASSISTANT)), BOXTURNASSIST);
+ CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_LAUNCH_MODE)), BOXNAVLAUNCH);
+ CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(AUTO_TUNE)), BOXAUTOTUNE);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOTRIM)), BOXAUTOTRIM);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXKILLSWITCH)), BOXKILLSWITCH);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHOMERESET)), BOXHOMERESET);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMERA1)), BOXCAMERA1);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMERA2)), BOXCAMERA2);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMERA3)), BOXCAMERA3);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT1)), BOXOSDALT1);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT2)), BOXOSDALT2);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT3)), BOXOSDALT3);
CHECK_ACTIVE_BOX(IS_ENABLED(navigationTerrainFollowingEnabled()), BOXSURFACE);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBRAKING)), BOXBRAKING);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER1)), BOXUSER1);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER2)), BOXUSER2);
- CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)),BOXLOITERDIRCHN);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBRAKING)), BOXBRAKING);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER1)), BOXUSER1);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER2)), BOXUSER2);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)), BOXLOITERDIRCHN);
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE)), BOXMSPRCOVERRIDE);
+#endif
memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
for (uint32_t i = 0; i < activeBoxIdCount; i++) {
diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h
index 43f5946384..2058c822d3 100755
--- a/src/main/fc/rc_modes.h
+++ b/src/main/fc/rc_modes.h
@@ -26,47 +26,48 @@
#define BOXID_NONE 255
typedef enum {
- BOXARM = 0,
- BOXANGLE = 1,
- BOXHORIZON = 2,
- BOXNAVALTHOLD = 3, // old BOXBARO
- BOXHEADINGHOLD = 4, // old MAG
- BOXHEADFREE = 5,
- BOXHEADADJ = 6,
- BOXCAMSTAB = 7,
- BOXNAVRTH = 8, // old GPSHOME
- BOXNAVPOSHOLD = 9, // old GPSHOLD
- BOXMANUAL = 10,
- BOXBEEPERON = 11,
- BOXLEDLOW = 12,
- BOXLIGHTS = 13,
- BOXNAVLAUNCH = 14,
- BOXOSD = 15,
- BOXTELEMETRY = 16,
- BOXBLACKBOX = 17,
- BOXFAILSAFE = 18,
- BOXNAVWP = 19,
- BOXAIRMODE = 20,
- BOXHOMERESET = 21,
- BOXGCSNAV = 22,
- BOXKILLSWITCH = 23, // old HEADING LOCK
- BOXSURFACE = 24,
- BOXFLAPERON = 25,
- BOXTURNASSIST = 26,
- BOXAUTOTRIM = 27,
- BOXAUTOTUNE = 28,
- BOXCAMERA1 = 29,
- BOXCAMERA2 = 30,
- BOXCAMERA3 = 31,
- BOXOSDALT1 = 32,
- BOXOSDALT2 = 33,
- BOXOSDALT3 = 34,
- BOXNAVCRUISE = 35,
- BOXBRAKING = 36,
- BOXUSER1 = 37,
- BOXUSER2 = 38,
- BOXFPVANGLEMIX = 39,
- BOXLOITERDIRCHN = 40,
+ BOXARM = 0,
+ BOXANGLE = 1,
+ BOXHORIZON = 2,
+ BOXNAVALTHOLD = 3, // old BOXBARO
+ BOXHEADINGHOLD = 4, // old MAG
+ BOXHEADFREE = 5,
+ BOXHEADADJ = 6,
+ BOXCAMSTAB = 7,
+ BOXNAVRTH = 8, // old GPSHOME
+ BOXNAVPOSHOLD = 9, // old GPSHOLD
+ BOXMANUAL = 10,
+ BOXBEEPERON = 11,
+ BOXLEDLOW = 12,
+ BOXLIGHTS = 13,
+ BOXNAVLAUNCH = 14,
+ BOXOSD = 15,
+ BOXTELEMETRY = 16,
+ BOXBLACKBOX = 17,
+ BOXFAILSAFE = 18,
+ BOXNAVWP = 19,
+ BOXAIRMODE = 20,
+ BOXHOMERESET = 21,
+ BOXGCSNAV = 22,
+ BOXKILLSWITCH = 23, // old HEADING LOCK
+ BOXSURFACE = 24,
+ BOXFLAPERON = 25,
+ BOXTURNASSIST = 26,
+ BOXAUTOTRIM = 27,
+ BOXAUTOTUNE = 28,
+ BOXCAMERA1 = 29,
+ BOXCAMERA2 = 30,
+ BOXCAMERA3 = 31,
+ BOXOSDALT1 = 32,
+ BOXOSDALT2 = 33,
+ BOXOSDALT3 = 34,
+ BOXNAVCRUISE = 35,
+ BOXBRAKING = 36,
+ BOXUSER1 = 37,
+ BOXUSER2 = 38,
+ BOXFPVANGLEMIX = 39,
+ BOXLOITERDIRCHN = 40,
+ BOXMSPRCOVERRIDE = 41,
CHECKBOX_ITEM_COUNT
} boxId_e;
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 596e4bad5d..4510fe02a8 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -416,6 +416,11 @@ groups:
- name: serialrx_halfduplex
field: halfDuplex
type: bool
+ - name: msp_override_channels
+ field: mspOverrideChannels
+ condition: USE_MSP_RC_OVERRIDE
+ min: 0
+ max: 65535
- name: PG_BLACKBOX_CONFIG
type: blackboxConfig_t
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 77f3edfbb4..407e2ddbd1 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -83,6 +83,7 @@
#include "navigation/navigation_private.h"
#include "rx/rx.h"
+#include "rx/msp_override.h"
#include "sensors/acceleration.h"
#include "sensors/battery.h"
@@ -2505,6 +2506,16 @@ static bool osdDrawSingleElement(uint8_t item)
return true;
}
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+ case OSD_RC_SOURCE:
+ {
+ const char *source_text = IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && !mspOverrideIsInFailsafe() ? "MSP" : "STD";
+ if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && mspOverrideIsInFailsafe()) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+ displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, source_text, elemAttr);
+ return true;
+ }
+#endif
+
default:
return false;
}
@@ -2708,6 +2719,10 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
osdConfig->item_pos[0][OSD_GFORCE_Y] = OSD_POS(12, 6);
osdConfig->item_pos[0][OSD_GFORCE_Z] = OSD_POS(12, 7);
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+ osdConfig->item_pos[0][OSD_RC_SOURCE] = OSD_POS(3, 4);
+#endif
+
// Under OSD_FLYMODE. TODO: Might not be visible on NTSC?
osdConfig->item_pos[0][OSD_MESSAGES] = OSD_POS(1, 13) | OSD_VISIBLE_FLAG;
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 3ce76fdc28..d729824786 100755
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -141,6 +141,7 @@ typedef enum {
OSD_GFORCE_X,
OSD_GFORCE_Y,
OSD_GFORCE_Z,
+ OSD_RC_SOURCE,
OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e;
diff --git a/src/main/rx/msp_override.c b/src/main/rx/msp_override.c
new file mode 100755
index 0000000000..cf526a7d46
--- /dev/null
+++ b/src/main/rx/msp_override.c
@@ -0,0 +1,240 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#include
+#include
+
+#include "platform.h"
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#include "common/maths.h"
+#include "common/utils.h"
+
+#include "config/feature.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "drivers/time.h"
+
+#include "fc/config.h"
+#include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
+
+#include "flight/failsafe.h"
+
+#include "rx/rx.h"
+#include "rx/msp.h"
+#include "rx/msp_override.h"
+
+
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+
+static bool rxDataProcessingRequired = false;
+
+static bool rxSignalReceived = false;
+static bool rxFlightChannelsValid = false;
+static bool rxFailsafe = true;
+
+static timeMs_t rxDataFailurePeriod;
+static timeMs_t rxDataRecoveryPeriod;
+static timeMs_t validRxDataReceivedAt = 0;
+static timeMs_t validRxDataFailedAt = 0;
+
+static timeUs_t rxNextUpdateAtUs = 0;
+static timeUs_t needRxSignalBefore = 0;
+
+static uint16_t mspOverrideCtrlChannels = 0; // bitmask representing which channels are used to control MSP override
+static rcChannel_t mspRcChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT];
+
+static rxRuntimeConfig_t rxRuntimeConfigMSP;
+
+
+void mspOverrideInit(void)
+{
+ timeMs_t nowMs = millis();
+
+ for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
+ mspRcChannels[i].raw = PWM_RANGE_MIDDLE;
+ mspRcChannels[i].data = PWM_RANGE_MIDDLE;
+ mspRcChannels[i].expiresAt = nowMs + MAX_INVALID_RX_PULSE_TIME;
+ }
+
+ mspRcChannels[THROTTLE].raw = (feature(FEATURE_3D)) ? PWM_RANGE_MIDDLE : rxConfig()->rx_min_usec;
+ mspRcChannels[THROTTLE].data = mspRcChannels[THROTTLE].raw;
+
+ // Initialize ARM switch to OFF position when arming via switch is defined
+ for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
+ if (modeActivationConditions(i)->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationConditions(i)->range)) {
+ // ARM switch is defined, determine an OFF value
+ uint16_t value;
+ if (modeActivationConditions(i)->range.startStep > 0) {
+ value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationConditions(i)->range.startStep - 1));
+ } else {
+ value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationConditions(i)->range.endStep + 1));
+ }
+ // Initialize ARM AUX channel to OFF value
+ rcChannel_t *armChannel = &mspRcChannels[modeActivationConditions(i)->auxChannelIndex + NON_AUX_CHANNEL_COUNT];
+ armChannel->raw = value;
+ armChannel->data = value;
+ }
+
+ // Find which channels are used to control MSP override
+ if (modeActivationConditions(i)->modeId == BOXMSPRCOVERRIDE && IS_RANGE_USABLE(&modeActivationConditions(i)->range)) {
+ mspOverrideCtrlChannels |= 1 << (modeActivationConditions(i)->auxChannelIndex + NON_AUX_CHANNEL_COUNT);
+ }
+ }
+
+ rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND;
+ rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY + failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND;
+
+ rxMspInit(rxConfig(), &rxRuntimeConfigMSP);
+}
+
+bool mspOverrideIsReceivingSignal(void)
+{
+ return rxSignalReceived;
+}
+
+bool mspOverrideAreFlightChannelsValid(void)
+{
+ return rxFlightChannelsValid;
+}
+
+bool mspOverrideIsInFailsafe(void)
+{
+ return rxFailsafe;
+}
+
+bool mspOverrideUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
+{
+ UNUSED(currentDeltaTime);
+
+ if (rxSignalReceived) {
+ if (currentTimeUs >= needRxSignalBefore) {
+ rxSignalReceived = false;
+ }
+ }
+
+ const uint8_t frameStatus = rxRuntimeConfigMSP.rcFrameStatusFn(&rxRuntimeConfigMSP);
+ if (frameStatus & RX_FRAME_COMPLETE) {
+ rxDataProcessingRequired = true;
+ rxSignalReceived = true;
+ needRxSignalBefore = currentTimeUs + rxRuntimeConfigMSP.rxSignalTimeout;
+ }
+
+ if (cmpTimeUs(currentTimeUs, rxNextUpdateAtUs) > 0) {
+ rxDataProcessingRequired = true;
+ }
+
+ return rxDataProcessingRequired; // data driven or 50Hz
+}
+
+bool mspOverrideCalculateChannels(timeUs_t currentTimeUs)
+{
+ int16_t rcStaging[MAX_SUPPORTED_RC_CHANNEL_COUNT];
+ const timeMs_t currentTimeMs = millis();
+
+ if (!rxDataProcessingRequired) {
+ return false;
+ }
+
+ rxDataProcessingRequired = false;
+ rxNextUpdateAtUs = currentTimeUs + DELAY_50_HZ;
+
+ rxFlightChannelsValid = true;
+
+ // Read and process channel data
+ for (int channel = 0; channel < rxRuntimeConfigMSP.channelCount; channel++) {
+ const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel);
+
+ // sample the channel
+ uint16_t sample = (*rxRuntimeConfigMSP.rcReadRawFn)(&rxRuntimeConfigMSP, rawChannel);
+
+ // apply the rx calibration to flight channel
+ if (channel < NON_AUX_CHANNEL_COUNT && sample != PPM_RCVR_TIMEOUT) {
+ sample = scaleRange(sample, rxChannelRangeConfigs(channel)->min, rxChannelRangeConfigs(channel)->max, PWM_RANGE_MIN, PWM_RANGE_MAX);
+ sample = MIN(MAX(PWM_PULSE_MIN, sample), PWM_PULSE_MAX);
+ }
+
+ // Store as rxRaw
+ mspRcChannels[channel].raw = sample;
+
+ // Apply invalid pulse value logic
+ if (!isRxPulseValid(sample)) {
+ sample = mspRcChannels[channel].data; // hold channel, replace with old value
+ if ((currentTimeMs > mspRcChannels[channel].expiresAt) && (channel < NON_AUX_CHANNEL_COUNT)) {
+ rxFlightChannelsValid = false;
+ }
+ } else {
+ mspRcChannels[channel].expiresAt = currentTimeMs + MAX_INVALID_RX_PULSE_TIME;
+ }
+
+ // Save channel value
+ rcStaging[channel] = sample;
+ }
+
+ // Update channel input value if receiver is not in failsafe mode
+ // If receiver is in failsafe (not receiving signal or sending invalid channel values) - last good input values are retained
+ if (rxFlightChannelsValid && rxSignalReceived) {
+ for (int channel = 0; channel < rxRuntimeConfigMSP.channelCount; channel++) {
+ mspRcChannels[channel].data = rcStaging[channel];
+ }
+ }
+
+ // Update failsafe
+ if (rxFlightChannelsValid && rxSignalReceived) {
+ validRxDataReceivedAt = millis();
+ if ((validRxDataReceivedAt - validRxDataFailedAt) > rxDataRecoveryPeriod) {
+ rxFailsafe = false;
+ }
+ } else {
+ validRxDataFailedAt = millis();
+ if ((validRxDataFailedAt - validRxDataReceivedAt) > rxDataFailurePeriod) {
+ rxFailsafe = true;
+ }
+ }
+
+ return true;
+}
+
+void mspOverrideChannels(rcChannel_t *rcChannels)
+{
+ for (uint16_t channel = 0, channelMask = 1; channel < rxRuntimeConfigMSP.channelCount; ++channel, channelMask <<= 1) {
+ if (rxConfig()->mspOverrideChannels & ~mspOverrideCtrlChannels & channelMask) {
+ rcChannels[channel].raw = rcChannels[channel].data = mspRcChannels[channel].data;
+ }
+ }
+}
+
+uint16_t mspOverrideGetRefreshRate(void)
+{
+ return rxRuntimeConfigMSP.rxRefreshRate;
+}
+
+int16_t mspOverrideGetChannelValue(unsigned channelNumber)
+{
+ return mspRcChannels[channelNumber].data;
+}
+
+int16_t mspOverrideGetRawChannelValue(unsigned channelNumber)
+{
+ return mspRcChannels[channelNumber].raw;
+}
+
+#endif // defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
diff --git a/src/main/rx/msp_override.h b/src/main/rx/msp_override.h
new file mode 100644
index 0000000000..7929ed4d9a
--- /dev/null
+++ b/src/main/rx/msp_override.h
@@ -0,0 +1,29 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#pragma once
+
+void mspOverrideInit(void);
+bool mspOverrideUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime);
+bool mspOverrideCalculateChannels(timeUs_t currentTimeUs);
+void mspOverrideChannels(rcChannel_t *rcChannels);
+bool mspOverrideIsReceivingSignal(void);
+bool mspOverrideIsInFailsafe(void);
+bool mspOverrideAreFlightChannelsValid(void);
+uint16_t mspOverrideGetRefreshRate(void);
+int16_t mspOverrideGetChannelValue(unsigned channelNumber);
+int16_t mspOverrideGetRawChannelValue(unsigned channelNumber);
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index 791fe1ae35..8879d111e3 100755
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -49,30 +49,25 @@
#include "io/serial.h"
#include "rx/rx.h"
+#include "rx/crsf.h"
+#include "rx/eleres.h"
+#include "rx/ibus.h"
+#include "rx/jetiexbus.h"
#include "rx/fport.h"
+#include "rx/msp.h"
+#include "rx/msp_override.h"
#include "rx/pwm.h"
+#include "rx/rx_spi.h"
#include "rx/sbus.h"
#include "rx/spektrum.h"
#include "rx/sumd.h"
#include "rx/sumh.h"
-#include "rx/msp.h"
-#include "rx/xbus.h"
-#include "rx/ibus.h"
-#include "rx/jetiexbus.h"
-#include "rx/rx_spi.h"
-#include "rx/crsf.h"
-#include "rx/eleres.h"
#include "rx/uib_rx.h"
+#include "rx/xbus.h"
//#define DEBUG_RX_SIGNAL_LOSS
-typedef struct rcChannel_s {
- int16_t raw; // Value received via RX - [1000;2000]
- int16_t data; // Value after processing - [1000;2000]
- timeMs_t expiresAt; // Time when this value becomes too old and it's discarded
-} rcChannel_t;
-
const char rcChannelLetters[] = "AERT";
static uint16_t rssi = 0; // range: [0;1023]
@@ -85,6 +80,10 @@ static rssiSource_e rssiSource;
static bool rxDataProcessingRequired = false;
static bool auxiliaryProcessingRequired = false;
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+static bool mspOverrideDataProcessingRequired = false;
+#endif
+
static bool rxSignalReceived = false;
static bool rxFlightChannelsValid = false;
static bool rxIsInFailsafeMode = true;
@@ -96,15 +95,13 @@ static uint8_t skipRxSamples = 0;
static rcChannel_t rcChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT];
-#define MAX_INVALID_PULS_TIME 300
-
#define SKIP_RC_ON_SUSPEND_PERIOD 1500000 // 1.5 second period in usec (call frequency independent)
#define SKIP_RC_SAMPLES_ON_RESUME 2 // flush 2 samples to drop wrong measurements (timing independent)
rxRuntimeConfig_t rxRuntimeConfig;
static uint8_t rcSampleIndex = 0;
-PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 7);
+PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 8);
#ifndef RX_SPI_DEFAULT_PROTOCOL
#define RX_SPI_DEFAULT_PROTOCOL 0
@@ -135,6 +132,9 @@ PG_RESET_TEMPLATE(rxConfig_t, rxConfig,
.rssiMax = RSSI_VISIBLE_VALUE_MAX,
.sbusSyncInterval = SBUS_DEFAULT_INTERFRAME_DELAY_US,
.rcFilterFrequency = 50,
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+ .mspOverrideChannels = 15,
+#endif
);
void resetAllRxChannelRangeConfigurations(void)
@@ -172,7 +172,7 @@ static uint8_t nullFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
return RX_FRAME_PENDING;
}
-static bool isPulseValid(uint16_t pulseDuration)
+bool isRxPulseValid(uint16_t pulseDuration)
{
return pulseDuration >= rxConfig()->rx_min_usec &&
pulseDuration <= rxConfig()->rx_max_usec;
@@ -251,7 +251,7 @@ void rxInit(void)
for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
rcChannels[i].raw = PWM_RANGE_MIDDLE;
rcChannels[i].data = PWM_RANGE_MIDDLE;
- rcChannels[i].expiresAt = nowMs + MAX_INVALID_PULS_TIME;
+ rcChannels[i].expiresAt = nowMs + MAX_INVALID_RX_PULSE_TIME;
}
rcChannels[THROTTLE].raw = (feature(FEATURE_3D)) ? PWM_RANGE_MIDDLE : rxConfig()->rx_min_usec;
@@ -327,6 +327,12 @@ void rxInit(void)
}
rxUpdateRSSISource();
+
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+ if (rxConfig()->receiverType != RX_TYPE_MSP) {
+ mspOverrideInit();
+ }
+#endif
}
void rxUpdateRSSISource(void)
@@ -381,7 +387,7 @@ void rxUpdateRSSISource(void)
}
}
-static uint8_t calculateChannelRemapping(const uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap)
+uint8_t calculateChannelRemapping(const uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap)
{
if (channelToRemap < channelMapEntryCount) {
return channelMap[channelToRemap];
@@ -439,7 +445,16 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
rxDataProcessingRequired = true;
}
- return rxDataProcessingRequired || auxiliaryProcessingRequired; // data driven or 50Hz
+ bool result = rxDataProcessingRequired || auxiliaryProcessingRequired;
+
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+ if (rxConfig()->receiverType != RX_TYPE_MSP) {
+ mspOverrideDataProcessingRequired = mspOverrideUpdateCheck(currentTimeUs, currentDeltaTime);
+ result = result || mspOverrideDataProcessingRequired;
+ }
+#endif
+
+ return result;
}
#define FILTERING_SAMPLE_COUNT 5
@@ -477,11 +492,15 @@ static uint16_t applyChannelFiltering(uint8_t chan, uint16_t sample)
bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
{
- UNUSED(currentTimeUs);
-
int16_t rcStaging[MAX_SUPPORTED_RC_CHANNEL_COUNT];
const timeMs_t currentTimeMs = millis();
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+ if ((rxConfig()->receiverType != RX_TYPE_MSP) && mspOverrideDataProcessingRequired) {
+ mspOverrideCalculateChannels(currentTimeUs);
+ }
+#endif
+
if (auxiliaryProcessingRequired) {
auxiliaryProcessingRequired = !rxRuntimeConfig.rcProcessFrameFn(&rxRuntimeConfig);
}
@@ -521,13 +540,13 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
rcChannels[channel].raw = sample;
// Apply invalid pulse value logic
- if (!isPulseValid(sample)) {
+ if (!isRxPulseValid(sample)) {
sample = rcChannels[channel].data; // hold channel, replace with old value
if ((currentTimeMs > rcChannels[channel].expiresAt) && (channel < NON_AUX_CHANNEL_COUNT)) {
rxFlightChannelsValid = false;
}
} else {
- rcChannels[channel].expiresAt = currentTimeMs + MAX_INVALID_PULS_TIME;
+ rcChannels[channel].expiresAt = currentTimeMs + MAX_INVALID_RX_PULSE_TIME;
}
// Save channel value
@@ -548,6 +567,12 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
}
}
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+ if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && !mspOverrideIsInFailsafe()) {
+ mspOverrideChannels(rcChannels);
+ }
+#endif
+
// Update failsafe
if (rxFlightChannelsValid && rxSignalReceived) {
failsafeOnValidDataReceived();
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
index 2412e5dde1..b5d6a987bf 100644
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@@ -90,6 +90,8 @@ extern const char rcChannelLetters[];
#define MAX_MAPPABLE_RX_INPUTS 4
+#define MAX_INVALID_RX_PULSE_TIME 300
+
#define RSSI_VISIBLE_VALUE_MIN 0
#define RSSI_VISIBLE_VALUE_MAX 100
#define RSSI_VISIBLE_FACTOR (RSSI_MAX_VALUE/(float)RSSI_VISIBLE_VALUE_MAX)
@@ -120,6 +122,7 @@ typedef struct rxConfig_s {
uint16_t rx_min_usec;
uint16_t rx_max_usec;
uint8_t rcFilterFrequency; // RC filter cutoff frequency (smoothness vs response sharpness)
+ uint16_t mspOverrideChannels; // Channels to override with MSP RC when BOXMSPRCOVERRIDE is active
} rxConfig_t;
PG_DECLARE(rxConfig_t, rxConfig);
@@ -143,6 +146,12 @@ typedef struct rxRuntimeConfig_s {
void *frameData;
} rxRuntimeConfig_t;
+typedef struct rcChannel_s {
+ int16_t raw; // Value received via RX - [1000;2000]
+ int16_t data; // Value after processing - [1000;2000]
+ timeMs_t expiresAt; // Time when this value becomes too old and it's discarded
+} rcChannel_t;
+
typedef enum {
RSSI_SOURCE_NONE = 0,
RSSI_SOURCE_ADC,
@@ -159,7 +168,9 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime);
bool rxIsReceivingSignal(void);
bool rxAreFlightChannelsValid(void);
bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs);
+bool isRxPulseValid(uint16_t pulseDuration);
+uint8_t calculateChannelRemapping(const uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap);
void parseRcChannels(const char *input);
// filtered = true indicates that newRssi comes from a source which already does
diff --git a/src/main/target/common.h b/src/main/target/common.h
index 2f5c9ec995..c9c655528f 100755
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -133,6 +133,7 @@
#define USE_MSP_OVER_TELEMETRY
// These are rather exotic serial protocols
#define USE_RX_MSP
+//#define USE_MSP_RC_OVERRIDE
#define USE_SERIALRX_SUMD
#define USE_SERIALRX_SUMH
#define USE_SERIALRX_XBUS