Add MSP2_INAV_SET_AUX_RC (0x2230)

Author: b14ckyy

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,

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 = rawValue;
+                    } 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;
 

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

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();

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);