From e93adf376240a6bcd04f699a4d909a0e88bd596d Mon Sep 17 00:00:00 2001 From: b14ckyy Date: Sun, 5 Apr 2026 22:52:16 +0200 Subject: [PATCH 1/5] Add MSP2_INAV_SET_AUX_RC (0x2230) --- docs/development/msp/msp_messages.json | 26 ++++++ src/main/fc/fc_msp.c | 105 +++++++++++++++++++++++++ src/main/msp/msp_protocol_v2_inav.h | 2 + src/main/rx/rx.c | 17 ++++ src/main/rx/rx.h | 4 + 5 files changed, 154 insertions(+) diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 12d67369bd7..866eb87b3b8 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -10943,6 +10943,32 @@ "notes": "All attitude angles are in deci-degrees.", "description": "Provides estimates of current attitude, local NEU position, and velocity." }, + "MSP2_INAV_SET_AUX_RC": { + "code": 8752, + "mspv": 2, + "request": { + "payload": [ + { + "name": "definitionByte", + "ctype": "uint8_t", + "desc": "Packed start channel and resolution. Bits 7-3: start channel index (valid range 8-31 for CH9-CH32; 0-7 rejected as error). Bits 2-0: resolution mode (0=2-bit, 1=4-bit, 2=8-bit, 3=16-bit; 4-7 reserved/error).", + "units": "" + }, + { + "name": "channelData", + "ctype": "uint8_t", + "desc": "Packed channel values, sequential from start channel. Number of channels is derived from data size and resolution. Value 0 means skip (no update). Sub-byte modes (2-bit, 4-bit) are packed MSB-first. 2-bit values 1-3 map to 1000/1500/2000us. 4-bit values 1-15 map to 1000 + (val-1)*1000/14 us. 8-bit values 1-255 map to 1000 + (val-1)*1000/254 us. 16-bit values are direct PWM.", + "units": "PWM (encoded)", + "array": true, + "array_size": 0 + } + ] + }, + "reply": null, + "variable_len": true, + "notes": "CH1-CH8 (index 0-7) are protected and will return `MSP_RESULT_ERROR`. Constraint: `startChannel + channelCount <= 32`. Values persist until overwritten; no timeout. Applied as a post-RX overlay in `calculateRxChannelsAndUpdateFailsafe()` after MSP RC Override but before failsafe. Does not require `USE_RX_MSP` or MSP-RC-OVERRIDE flight mode. Does not affect failsafe detection. Recommended to send with `MSP_FLAG_DONT_REPLY` (flags=0x01) to save bandwidth on telemetry passthrough links. 16-bit mode requires even number of data bytes.", + "description": "Bandwidth-efficient auxiliary RC channel update. Sets CH9-CH32 with configurable resolution (2/4/8/16-bit) without affecting primary flight controls. Designed for extending channel count beyond native RC link capacity via MSP passthrough." + }, "MSP2_BETAFLIGHT_BIND": { "code": 12288, "mspv": 2, diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index cf5308067e9..d0e673f5fcc 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2412,6 +2412,110 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } break; #endif + + case MSP2_INAV_SET_AUX_RC: + { + if (dataSize < 2) { + return MSP_RESULT_ERROR; + } + + const uint8_t defByte = sbufReadU8(src); + const uint8_t startChannel = defByte >> 3; // Bits 7-3: start channel index (0-31) + const uint8_t resolutionMode = defByte & 0x07; // Bits 2-0: resolution + + // Safety: CH1-CH8 (index 0-7) are protected + if (startChannel < 8) { + return MSP_RESULT_ERROR; + } + + const uint8_t dataBytes = dataSize - 1; + uint8_t channelCount; + uint8_t bitsPerChannel; + + switch (resolutionMode) { + case 0: // 2-bit + bitsPerChannel = 2; + channelCount = dataBytes * 4; + break; + case 1: // 4-bit + bitsPerChannel = 4; + channelCount = dataBytes * 2; + break; + case 2: // 8-bit + bitsPerChannel = 8; + channelCount = dataBytes; + break; + case 3: // 16-bit + bitsPerChannel = 16; + if (dataBytes % 2 != 0) { + return MSP_RESULT_ERROR; + } + channelCount = dataBytes / 2; + break; + default: + return MSP_RESULT_ERROR; + } + + if (channelCount == 0 || startChannel + channelCount > 32) { + return MSP_RESULT_ERROR; + } + + // Decode and apply channel values + if (bitsPerChannel >= 8) { + // Byte-aligned modes: 8-bit and 16-bit + for (int i = 0; i < channelCount; i++) { + uint16_t rawValue; + if (bitsPerChannel == 16) { + rawValue = sbufReadU16(src); + } else { + rawValue = sbufReadU8(src); + } + + if (rawValue == 0) { + continue; // skip: no update + } + + uint16_t pwmValue; + if (bitsPerChannel == 16) { + pwmValue = constrain(rawValue, 750, 2250); + } else { + // 8-bit: 1-255 → 1000-2000 + pwmValue = 1000 + ((uint32_t)(rawValue - 1) * 1000) / 254; + } + + rxMspAuxOverlaySet(startChannel + i, pwmValue); + } + } else { + // Sub-byte modes: 2-bit and 4-bit + const uint8_t mask = (1 << bitsPerChannel) - 1; + const uint8_t channelsPerByte = 8 / bitsPerChannel; + int ch = 0; + + for (int byteIdx = 0; byteIdx < (int)dataBytes && ch < channelCount; byteIdx++) { + const uint8_t dataByte = sbufReadU8(src); + for (int sub = channelsPerByte - 1; sub >= 0 && ch < channelCount; sub--, ch++) { + const uint8_t rawValue = (dataByte >> (sub * bitsPerChannel)) & mask; + + if (rawValue == 0) { + continue; // skip: no update + } + + uint16_t pwmValue; + if (bitsPerChannel == 2) { + // 2-bit: 1→1000, 2→1500, 3→2000 + pwmValue = 1000 + (rawValue - 1) * 500; + } else { + // 4-bit: 1-15 → 1000-2000 + pwmValue = 1000 + ((uint32_t)(rawValue - 1) * 1000) / 14; + } + + rxMspAuxOverlaySet(startChannel + ch, pwmValue); + } + } + } + } + break; + case MSP2_COMMON_SET_MOTOR_MIXER: sbufReadU8Safe(&tmp_u8, src); if ((dataSize == 9) && (tmp_u8 < MAX_SUPPORTED_MOTORS)) { @@ -4486,6 +4590,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro sbuf_t *dst = &reply->buf; sbuf_t *src = &cmd->buf; const uint16_t cmdMSP = cmd->cmd; + // initialize reply by default reply->cmd = cmd->cmd; diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 0b893916895..aacccc7806b 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -126,3 +126,5 @@ #define MSP2_INAV_SET_GVAR 0x2214 #define MSP2_INAV_FULL_LOCAL_POSE 0x2220 + +#define MSP2_INAV_SET_AUX_RC 0x2230 diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 0531904d063..7df78cc5e50 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -94,6 +94,9 @@ static bool isRxSuspended = false; static rcChannel_t rcChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT]; +// MSP aux channel overlay: non-zero values override rcChannels[].data for CH9-CH32 +static uint16_t mspAuxOverlay[MAX_SUPPORTED_RC_CHANNEL_COUNT]; + rxLinkStatistics_t rxLinkStatistics; rxRuntimeConfig_t rxRuntimeConfig; static uint8_t rcSampleIndex = 0; @@ -512,6 +515,13 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) } #endif + // Apply MSP aux channel overlay (CH9-CH32) + for (int i = 8; i < 32; i++) { + if (mspAuxOverlay[i] > 0) { + rcChannels[i].data = mspAuxOverlay[i]; + } + } + // Update failsafe if (rxFlightChannelsValid && rxSignalReceived) { failsafeOnValidDataReceived(); @@ -663,6 +673,13 @@ int16_t rxGetChannelValue(unsigned channelNumber) } } +void rxMspAuxOverlaySet(uint8_t channelIndex, uint16_t value) +{ + if (channelIndex >= 8 && channelIndex < 32) { + mspAuxOverlay[channelIndex] = value; + } +} + void lqTrackerReset(rxLinkQualityTracker_e * lqTracker) { lqTracker->lastUpdatedMs = millis(); diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 3ed6add3e48..6eb63c83325 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -232,3 +232,7 @@ void resumeRxSignal(void); // filtering and some extra processing like value holding // during failsafe. int16_t rxGetChannelValue(unsigned channelNumber); + +// MSP aux channel overlay (CH9-CH32). Sets a channel value that persists +// across RX update cycles. value=0 clears the overlay for that channel. +void rxMspAuxOverlaySet(uint8_t channelIndex, uint16_t value); From 6b0aaa8f63ea2cfc3be2711485fac2584d1440f5 Mon Sep 17 00:00:00 2001 From: b14ckyy Date: Thu, 9 Apr 2026 09:42:59 +0200 Subject: [PATCH 2/5] Guard overlay against active RX and MSP_RC_OVERRIDE channels - Clamp 16-bit PWM values to 750-2250 (prevent int16_t overflow) - Reject oversized payloads (dataSize > 49) - Skip channels actively provided by primary RX (SBUS/CRSF/etc.) - When MSP is the RX provider, use actual MSP_SET_RAW_RC channel count - Skip channels controlled by MSP_RC_OVERRIDE bitmask when active --- src/main/fc/fc_msp.c | 3 ++- src/main/rx/msp.c | 7 +++++++ src/main/rx/msp.h | 1 + src/main/rx/rx.c | 29 +++++++++++++++++++++++++---- src/main/rx/rx.h | 2 +- 5 files changed, 36 insertions(+), 6 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 4fff9e169c9..9606302abd3 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2415,7 +2415,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP2_INAV_SET_AUX_RC: { - if (dataSize < 2) { + // Max valid payload: 1 def byte + 24 channels × 2 bytes (16-bit) = 49 bytes + if (dataSize < 2 || dataSize > 49) { return MSP_RESULT_ERROR; } diff --git a/src/main/rx/msp.c b/src/main/rx/msp.c index b338fbc2d97..e2d310423a6 100644 --- a/src/main/rx/msp.c +++ b/src/main/rx/msp.c @@ -31,6 +31,7 @@ static uint16_t mspFrame[MAX_SUPPORTED_RC_CHANNEL_COUNT]; static bool rxMspFrameDone = false; +static uint8_t mspLastChannelCount = 0; static uint16_t rxMspReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan) { @@ -49,9 +50,15 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount) mspFrame[i] = 0; } + mspLastChannelCount = channelCount; rxMspFrameDone = true; } +uint8_t rxMspGetLastChannelCount(void) +{ + return mspLastChannelCount; +} + static uint8_t rxMspFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxRuntimeConfig); diff --git a/src/main/rx/msp.h b/src/main/rx/msp.h index 78a9bfd7132..c99fe44d641 100644 --- a/src/main/rx/msp.h +++ b/src/main/rx/msp.h @@ -21,3 +21,4 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount); void rxMspInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); +uint8_t rxMspGetLastChannelCount(void); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 7df78cc5e50..d1b8b7833f0 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -515,10 +515,31 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) } #endif - // Apply MSP aux channel overlay (CH9-CH32) - for (int i = 8; i < 32; i++) { - if (mspAuxOverlay[i] > 0) { - rcChannels[i].data = mspAuxOverlay[i]; + // Apply MSP aux channel overlay — only to channels NOT actively + // provided by the primary RX (avoids jitter from competing sources) + { + // Determine the effective RX channel count + uint8_t activeRxChannels = rxChannelCount; +#ifdef USE_RX_MSP + // When MSP is the primary RX, rxChannelCount is always 32. + // Use the actual channel count from the last MSP_SET_RAW_RC message. + if (rxConfig()->receiverType == RX_TYPE_MSP) { + const uint8_t mspChannels = rxMspGetLastChannelCount(); + if (mspChannels > 0) { + activeRxChannels = mspChannels; + } + } +#endif + for (int i = MAX(8, activeRxChannels); i < 32; i++) { + if (mspAuxOverlay[i] > 0) { +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + // Skip channels controlled by MSP RC Override when active + if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && (rxConfig()->mspOverrideChannels & (1U << i))) { + continue; + } +#endif + rcChannels[i].data = mspAuxOverlay[i]; + } } } diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 6eb63c83325..1c2989fa3dc 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -234,5 +234,5 @@ void resumeRxSignal(void); int16_t rxGetChannelValue(unsigned channelNumber); // MSP aux channel overlay (CH9-CH32). Sets a channel value that persists -// across RX update cycles. value=0 clears the overlay for that channel. +// across RX update cycles. value=0 ignores that channel and skips it. void rxMspAuxOverlaySet(uint8_t channelIndex, uint16_t value); From 3bf93e8f093e0d1a7a80b0f9d00ab16e2952c4ea Mon Sep 17 00:00:00 2001 From: b14ckyy Date: Thu, 9 Apr 2026 17:45:32 +0200 Subject: [PATCH 3/5] Restrict overlay to CH13-CH32, remove rxChannelCount guard MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Raise minimum channel from CH9 to CH13 (index 12) All common protocols deliver at least 12 channels on the wire. - Remove rxChannelCount-based guard (protocol drivers report max capacity, not actual sender channel count — unusable as guard) - Remove rxMspGetLastChannelCount (no longer needed) - Keep MSP_RC_OVERRIDE bitmask guard --- src/main/fc/fc_msp.c | 4 ++-- src/main/rx/msp.c | 7 ------- src/main/rx/msp.h | 1 - src/main/rx/rx.c | 33 +++++++++------------------------ src/main/rx/rx.h | 2 +- 5 files changed, 12 insertions(+), 35 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 9606302abd3..b4ba53ea724 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2424,8 +2424,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) const uint8_t startChannel = defByte >> 3; // Bits 7-3: start channel index (0-31) const uint8_t resolutionMode = defByte & 0x07; // Bits 2-0: resolution - // Safety: CH1-CH8 (index 0-7) are protected - if (startChannel < 8) { + // Safety: CH1-CH12 (index 0-11) are protected + if (startChannel < 12) { return MSP_RESULT_ERROR; } diff --git a/src/main/rx/msp.c b/src/main/rx/msp.c index e2d310423a6..b338fbc2d97 100644 --- a/src/main/rx/msp.c +++ b/src/main/rx/msp.c @@ -31,7 +31,6 @@ static uint16_t mspFrame[MAX_SUPPORTED_RC_CHANNEL_COUNT]; static bool rxMspFrameDone = false; -static uint8_t mspLastChannelCount = 0; static uint16_t rxMspReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan) { @@ -50,15 +49,9 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount) mspFrame[i] = 0; } - mspLastChannelCount = channelCount; rxMspFrameDone = true; } -uint8_t rxMspGetLastChannelCount(void) -{ - return mspLastChannelCount; -} - static uint8_t rxMspFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxRuntimeConfig); diff --git a/src/main/rx/msp.h b/src/main/rx/msp.h index c99fe44d641..78a9bfd7132 100644 --- a/src/main/rx/msp.h +++ b/src/main/rx/msp.h @@ -21,4 +21,3 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount); void rxMspInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); -uint8_t rxMspGetLastChannelCount(void); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index d1b8b7833f0..b85350d8333 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -515,31 +515,16 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) } #endif - // Apply MSP aux channel overlay — only to channels NOT actively - // provided by the primary RX (avoids jitter from competing sources) - { - // Determine the effective RX channel count - uint8_t activeRxChannels = rxChannelCount; -#ifdef USE_RX_MSP - // When MSP is the primary RX, rxChannelCount is always 32. - // Use the actual channel count from the last MSP_SET_RAW_RC message. - if (rxConfig()->receiverType == RX_TYPE_MSP) { - const uint8_t mspChannels = rxMspGetLastChannelCount(); - if (mspChannels > 0) { - activeRxChannels = mspChannels; - } - } -#endif - for (int i = MAX(8, activeRxChannels); i < 32; i++) { - if (mspAuxOverlay[i] > 0) { + // Apply MSP aux channel overlay (CH13-CH32) + for (int i = 12; i < 32; i++) { + if (mspAuxOverlay[i] > 0) { #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) - // Skip channels controlled by MSP RC Override when active - if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && (rxConfig()->mspOverrideChannels & (1U << i))) { - continue; - } -#endif - rcChannels[i].data = mspAuxOverlay[i]; + // Skip channels controlled by MSP RC Override when active + if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && (rxConfig()->mspOverrideChannels & (1U << i))) { + continue; } +#endif + rcChannels[i].data = mspAuxOverlay[i]; } } @@ -696,7 +681,7 @@ int16_t rxGetChannelValue(unsigned channelNumber) void rxMspAuxOverlaySet(uint8_t channelIndex, uint16_t value) { - if (channelIndex >= 8 && channelIndex < 32) { + if (channelIndex >= 12 && channelIndex < 32) { mspAuxOverlay[channelIndex] = value; } } diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 1c2989fa3dc..5b5bcea50c3 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -233,6 +233,6 @@ void resumeRxSignal(void); // during failsafe. int16_t rxGetChannelValue(unsigned channelNumber); -// MSP aux channel overlay (CH9-CH32). Sets a channel value that persists +// MSP aux channel overlay (CH13-CH32). Sets a channel value that persists // across RX update cycles. value=0 ignores that channel and skips it. void rxMspAuxOverlaySet(uint8_t channelIndex, uint16_t value); From 86615b9f40bcbabebce6c5c1dbae7dd89fcfe9f2 Mon Sep 17 00:00:00 2001 From: b14ckyy Date: Thu, 9 Apr 2026 18:12:57 +0200 Subject: [PATCH 4/5] Re-add MSP RX channel count guard for overlay When MSP is the primary RX provider (e.g. mLRS), skip channels already covered by MSP_SET_RAW_RC to avoid wasting bandwidth on overlapping updates. --- src/main/rx/msp.c | 7 +++++++ src/main/rx/msp.h | 1 + src/main/rx/rx.c | 26 +++++++++++++++++++------- 3 files changed, 27 insertions(+), 7 deletions(-) diff --git a/src/main/rx/msp.c b/src/main/rx/msp.c index b338fbc2d97..e2d310423a6 100644 --- a/src/main/rx/msp.c +++ b/src/main/rx/msp.c @@ -31,6 +31,7 @@ static uint16_t mspFrame[MAX_SUPPORTED_RC_CHANNEL_COUNT]; static bool rxMspFrameDone = false; +static uint8_t mspLastChannelCount = 0; static uint16_t rxMspReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan) { @@ -49,9 +50,15 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount) mspFrame[i] = 0; } + mspLastChannelCount = channelCount; rxMspFrameDone = true; } +uint8_t rxMspGetLastChannelCount(void) +{ + return mspLastChannelCount; +} + static uint8_t rxMspFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxRuntimeConfig); diff --git a/src/main/rx/msp.h b/src/main/rx/msp.h index 78a9bfd7132..c99fe44d641 100644 --- a/src/main/rx/msp.h +++ b/src/main/rx/msp.h @@ -21,3 +21,4 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount); void rxMspInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); +uint8_t rxMspGetLastChannelCount(void); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index b85350d8333..b897238ed1d 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -516,15 +516,27 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) #endif // Apply MSP aux channel overlay (CH13-CH32) - for (int i = 12; i < 32; i++) { - if (mspAuxOverlay[i] > 0) { -#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) - // Skip channels controlled by MSP RC Override when active - if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && (rxConfig()->mspOverrideChannels & (1U << i))) { - continue; + { + int overlayStart = 12; +#ifdef USE_RX_MSP + // When MSP is the primary RX, skip channels covered by MSP_SET_RAW_RC + if (rxConfig()->receiverType == RX_TYPE_MSP) { + const uint8_t mspChannels = rxMspGetLastChannelCount(); + if (mspChannels > overlayStart) { + overlayStart = mspChannels; } + } #endif - rcChannels[i].data = mspAuxOverlay[i]; + for (int i = overlayStart; i < 32; i++) { + if (mspAuxOverlay[i] > 0) { +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + // Skip channels controlled by MSP RC Override when active + if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && (rxConfig()->mspOverrideChannels & (1U << i))) { + continue; + } +#endif + rcChannels[i].data = mspAuxOverlay[i]; + } } } From f025071bcc84efc9df6e0247304045a52e4e30e5 Mon Sep 17 00:00:00 2001 From: b14ckyy Date: Thu, 9 Apr 2026 18:50:20 +0200 Subject: [PATCH 5/5] Add MSP2_INAV_SET_AUX_RC documentation - Rx.md: Add 'MSP Auxiliary RC Channel Overlay' section with key properties, use case, priority order, and safeguard descriptions - Rx.md: Fix MSP RX channel count (18 -> 34) - msp_messages.json: Update channel range to CH13-CH32, add clamp range, payload size limits, and guard descriptions --- docs/Rx.md | 27 +++++++++++++++++++++++++- docs/development/msp/msp_messages.json | 8 ++++---- 2 files changed, 30 insertions(+), 5 deletions(-) diff --git a/docs/Rx.md b/docs/Rx.md index 0f0ae64a453..3c9f5497448 100644 --- a/docs/Rx.md +++ b/docs/Rx.md @@ -201,7 +201,7 @@ bind_msp_rx ## MultiWii serial protocol (MSP RX) -Allows you to use MSP commands as the RC input. Up to 18 channels are supported. +Allows you to use MSP commands as the RC input. Up to 34 channels are supported. Note: * It is necessary to update `MSP_SET_RAW_RC` at 5Hz or faster. * `MSP_SET_RAW_RC` uses the defined RC channel map @@ -213,6 +213,31 @@ Note: Enables the use of a joystick in the INAV SITL with a flight simulator. See the [SITL documentation](SITL/SITL.md). +## MSP Auxiliary RC Channel Overlay (MSP2_INAV_SET_AUX_RC) + +Allows extending the available RC channel count beyond the native RC link capacity using `MSP2_INAV_SET_AUX_RC` (`0x2230`). This is a lightweight, bandwidth-efficient alternative to `MSP_SET_RAW_RC` for auxiliary channels only. + +**Key properties:** +- Controls **CH13–CH32** only (CH1–CH12 are protected and rejected) +- Configurable resolution: 2-bit (3 positions), 4-bit (~71µs steps), 8-bit (~3.9µs steps), or 16-bit (raw PWM) +- Value `0` = skip (no update) — previous value persists indefinitely +- No flight mode or special configuration required — always active +- Does **not** affect failsafe detection +- Recommended to send with `MSP_FLAG_DONT_REPLY` (`flags=0x01`) on telemetry passthrough links + +**Typical use case:** A Lua script on the radio sends `MSP2_INAV_SET_AUX_RC` via SmartPort/CRSF/ELRS telemetry passthrough to control auxiliary functions (lights, camera triggers, gimbal modes) on channels beyond the RC link's native capacity. + +**Priority order** (last writer wins): +1. Primary RX (SBUS, CRSF, FPort, etc.) +2. MSP RC Override (if active) +3. **MSP AUX Overlay** (CH13–CH32) + +**Important:** For serial RX protocols, the firmware cannot detect which channels the sender actively uses. If AUX_RC targets a channel that the RX link also sends, AUX_RC will override it. Configure the start channel above your RC link's active channel range. + +When MSP is the primary RX provider (`receiver_type = MSP`), channels covered by `MSP_SET_RAW_RC` are automatically protected. Channels in the `msp_override_channels` bitmask are also protected when MSP RC Override mode is active. + +See the [MSP documentation](development/msp/README.md) for the full message format. + ## Configuration The receiver type can be set from the configurator or CLI. diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 866eb87b3b8..bbb7eac3429 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -10951,13 +10951,13 @@ { "name": "definitionByte", "ctype": "uint8_t", - "desc": "Packed start channel and resolution. Bits 7-3: start channel index (valid range 8-31 for CH9-CH32; 0-7 rejected as error). Bits 2-0: resolution mode (0=2-bit, 1=4-bit, 2=8-bit, 3=16-bit; 4-7 reserved/error).", + "desc": "Packed start channel and resolution. Bits 7-3: start channel index (valid range 12-31 for CH13-CH32; 0-11 rejected as error). Bits 2-0: resolution mode (0=2-bit, 1=4-bit, 2=8-bit, 3=16-bit; 4-7 reserved/error).", "units": "" }, { "name": "channelData", "ctype": "uint8_t", - "desc": "Packed channel values, sequential from start channel. Number of channels is derived from data size and resolution. Value 0 means skip (no update). Sub-byte modes (2-bit, 4-bit) are packed MSB-first. 2-bit values 1-3 map to 1000/1500/2000us. 4-bit values 1-15 map to 1000 + (val-1)*1000/14 us. 8-bit values 1-255 map to 1000 + (val-1)*1000/254 us. 16-bit values are direct PWM.", + "desc": "Packed channel values, sequential from start channel. Number of channels is derived from data size and resolution. Value 0 means skip (no update). Sub-byte modes (2-bit, 4-bit) are packed MSB-first. 2-bit values 1-3 map to 1000/1500/2000us. 4-bit values 1-15 map to 1000 + (val-1)*1000/14 us. 8-bit values 1-255 map to 1000 + (val-1)*1000/254 us. 16-bit values are direct PWM, clamped to 750-2250us.", "units": "PWM (encoded)", "array": true, "array_size": 0 @@ -10966,8 +10966,8 @@ }, "reply": null, "variable_len": true, - "notes": "CH1-CH8 (index 0-7) are protected and will return `MSP_RESULT_ERROR`. Constraint: `startChannel + channelCount <= 32`. Values persist until overwritten; no timeout. Applied as a post-RX overlay in `calculateRxChannelsAndUpdateFailsafe()` after MSP RC Override but before failsafe. Does not require `USE_RX_MSP` or MSP-RC-OVERRIDE flight mode. Does not affect failsafe detection. Recommended to send with `MSP_FLAG_DONT_REPLY` (flags=0x01) to save bandwidth on telemetry passthrough links. 16-bit mode requires even number of data bytes.", - "description": "Bandwidth-efficient auxiliary RC channel update. Sets CH9-CH32 with configurable resolution (2/4/8/16-bit) without affecting primary flight controls. Designed for extending channel count beyond native RC link capacity via MSP passthrough." + "notes": "CH1-CH12 (index 0-11) are protected and will return `MSP_RESULT_ERROR`. Payload size must be 2-49 bytes. Constraint: `startChannel + channelCount <= 32`. Values persist until overwritten; no timeout. Applied as a post-RX overlay in `calculateRxChannelsAndUpdateFailsafe()` after MSP RC Override but before failsafe. Does not require `USE_RX_MSP` or MSP-RC-OVERRIDE flight mode. Does not affect failsafe detection. When MSP is the primary RX provider, channels covered by `MSP_SET_RAW_RC` are automatically skipped. Channels in the `mspOverrideChannels` bitmask are skipped when MSP RC Override mode is active. Recommended to send with `MSP_FLAG_DONT_REPLY` (flags=0x01) to save bandwidth on telemetry passthrough links. 16-bit mode requires even number of data bytes and values are clamped to 750-2250us.", + "description": "Bandwidth-efficient auxiliary RC channel update. Sets CH13-CH32 with configurable resolution (2/4/8/16-bit) without affecting primary flight controls. Designed for extending channel count beyond native RC link capacity via MSP passthrough." }, "MSP2_BETAFLIGHT_BIND": { "code": 12288,