diff --git a/make/source.mk b/make/source.mk index 76cef6945c..37c0783062 100644 --- a/make/source.mk +++ b/make/source.mk @@ -124,6 +124,7 @@ COMMON_SRC = \ rx/sumh.c \ rx/xbus.c \ rx/fport.c \ + rx/msp_override.c \ sensors/acceleration.c \ sensors/boardalignment.c \ sensors/compass.c \ diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index ec0588fd8a..61f0117ab4 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -757,6 +757,9 @@ const clivalue_t valueTable[] = { { "rx_min_usec", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rx_min_usec) }, { "rx_max_usec", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rx_max_usec) }, { "serialrx_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, halfDuplex) }, +#if defined(USE_RX_MSP_OVERRIDE) + { "msp_override_channels_mask", VAR_UINT32 | MASTER_VALUE, .config.u32Max = (1 << MAX_SUPPORTED_RC_CHANNEL_COUNT) - 1, PG_RX_CONFIG, offsetof(rxConfig_t, msp_override_channels_mask)}, +#endif #ifdef USE_RX_SPI { "rx_spi_protocol", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RX_SPI }, PG_RX_SPI_CONFIG, offsetof(rxSpiConfig_t, rx_spi_protocol) }, { "rx_spi_bus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_RX_SPI_CONFIG, offsetof(rxSpiConfig_t, spibus) }, diff --git a/src/main/config/config.c b/src/main/config/config.c index bf9ef30eb4..75afdac767 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -596,6 +596,14 @@ static void validateAndFixConfig(void) } } } +#if defined(USE_RX_MSP_OVERRIDE) + for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { + const modeActivationCondition_t *mac = modeActivationConditions(i); + if (mac->modeId == BOXMSPOVERRIDE && ((1 << (mac->auxChannelIndex) & (rxConfig()->msp_override_channels_mask)))) { + rxConfigMutable()->msp_override_channels_mask &= ~(1 << (mac->auxChannelIndex + NON_AUX_CHANNEL_COUNT)); + } + } +#endif } void validateAndFixGyroConfig(void) diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 0f95fefc85..b8bb947ec9 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -74,6 +74,7 @@ typedef enum { BOXACROTRAINER, BOXVTXCONTROLDISABLE, BOXLAUNCHCONTROL, + BOXMSPOVERRIDE, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/msp/msp_box.c b/src/main/msp/msp_box.c index f58c64ef34..4c1bbaed27 100644 --- a/src/main/msp/msp_box.c +++ b/src/main/msp/msp_box.c @@ -97,6 +97,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = { { BOXACROTRAINER, "ACRO TRAINER", 47 }, { BOXVTXCONTROLDISABLE, "DISABLE VTX CONTROL", 48}, { BOXLAUNCHCONTROL, "LAUNCH CONTROL", 49 }, + { BOXMSPOVERRIDE, "MSP OVERRIDE", 50}, }; // mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index diff --git a/src/main/pg/rx.c b/src/main/pg/rx.c index e075b0b827..43626a94a7 100644 --- a/src/main/pg/rx.c +++ b/src/main/pg/rx.c @@ -73,6 +73,7 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig) .srxl2_baud_fast = true, .sbus_baud_fast = false, .crsf_use_rx_snr = false, + .msp_override_channels_mask = 0, ); #ifdef RX_CHANNELS_TAER diff --git a/src/main/pg/rx.h b/src/main/pg/rx.h index 4f46ca9523..12397069b8 100644 --- a/src/main/pg/rx.h +++ b/src/main/pg/rx.h @@ -66,6 +66,8 @@ typedef struct rxConfig_s { uint8_t srxl2_baud_fast; // Select Spektrum SRXL2 fast baud rate uint8_t sbus_baud_fast; // Select SBus fast baud rate uint8_t crsf_use_rx_snr; // Use RX SNR (in dB) instead of RSSI dBm for CRSF + + uint32_t msp_override_channels_mask; // Channels to override when the MSP override mode is enabled } rxConfig_t; PG_DECLARE(rxConfig_t, rxConfig); diff --git a/src/main/rx/msp.c b/src/main/rx/msp.c index beaa6d874b..3304810f26 100644 --- a/src/main/rx/msp.c +++ b/src/main/rx/msp.c @@ -36,7 +36,7 @@ static uint16_t mspFrame[MAX_SUPPORTED_RC_CHANNEL_COUNT]; static bool rxMspFrameDone = false; -static uint16_t rxMspReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan) +uint16_t rxMspReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan) { UNUSED(rxRuntimeState); return mspFrame[chan]; diff --git a/src/main/rx/msp.h b/src/main/rx/msp.h index 4b55393e83..afc749747c 100644 --- a/src/main/rx/msp.h +++ b/src/main/rx/msp.h @@ -22,5 +22,6 @@ struct rxConfig_s; struct rxRuntimeState_s; +uint16_t rxMspReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan); void rxMspInit(const struct rxConfig_s *rxConfig, struct rxRuntimeState_s *rxRuntimeState); void rxMspFrameReceive(uint16_t *frame, int channelCount); diff --git a/src/main/rx/msp_override.c b/src/main/rx/msp_override.c new file mode 100644 index 0000000000..5411ad18cb --- /dev/null +++ b/src/main/rx/msp_override.c @@ -0,0 +1,44 @@ +/* + * 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 . + */ + +#include "platform.h" + +#if defined(USE_RX_MSP_OVERRIDE) + +#include "rx/msp_override.h" +#include "rx/msp.h" +#include "fc/rc_modes.h" +#include "common/maths.h" + + +uint16_t rxMspOverrideReadRawRc(const rxRuntimeState_t *rxRuntimeState, const rxConfig_t *rxConfig, uint8_t chan) +{ + uint16_t rxSample = (rxRuntimeState->rcReadRawFn)(rxRuntimeState, chan); + + uint16_t overrideSample = rxMspReadRawRC(rxRuntimeState, chan); + bool override = (1 << chan) & rxConfig->msp_override_channels_mask; + + if (IS_RC_MODE_ACTIVE(BOXMSPOVERRIDE) && override) { + return overrideSample; + } else { + return rxSample; + } +} +#endif diff --git a/src/main/rx/msp_override.h b/src/main/rx/msp_override.h new file mode 100644 index 0000000000..56cec29a9f --- /dev/null +++ b/src/main/rx/msp_override.h @@ -0,0 +1,27 @@ +/* + * 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 . + */ + +#pragma once + +#include "rx/rx.h" +#include "pg/rx.h" + +uint16_t rxMspOverrideReadRawRc(const rxRuntimeState_t *rxRuntimeState, const rxConfig_t *rxConfig, uint8_t chan); + diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 278590aad6..95e7a5d5f9 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -67,6 +67,7 @@ #include "rx/crsf.h" #include "rx/rx_spi.h" #include "rx/targetcustomserial.h" +#include "rx/msp_override.h" const char rcChannelLetters[] = "AERT12345678abcdefgh"; @@ -591,7 +592,15 @@ static void readRxChannelsApplyRanges(void) const uint8_t rawChannel = channel < RX_MAPPABLE_CHANNEL_COUNT ? rxConfig()->rcmap[channel] : channel; // sample the channel - uint16_t sample = rxRuntimeState.rcReadRawFn(&rxRuntimeState, rawChannel); + uint16_t sample; +#if defined(USE_RX_MSP_OVERRIDE) + if (rxConfig()->msp_override_channels_mask) { + sample = rxMspOverrideReadRawRc(&rxRuntimeState, rxConfig(), rawChannel); + } else +#endif + { + sample = rxRuntimeState.rcReadRawFn(&rxRuntimeState, rawChannel); + } // apply the rx calibration if (channel < NON_AUX_CHANNEL_COUNT) { diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index 5fcfaf04dc..cfa3407651 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -368,4 +368,5 @@ #define USE_INTERPOLATED_SP #define USE_CUSTOM_BOX_NAMES #define USE_BATTERY_VOLTAGE_SAG_COMPENSATION +#define USE_RX_MSP_OVERRIDE #endif