Merge pull request #7543 from dthubereng/add_mode_logic_to_msp

Add MODE_RANGES_EXTRA to MSP

docs/API/MSP_extensions.md

@@ -28,7 +28,32 @@ Unassigned slots have rangeStartStep == rangeEndStep. Each element contains the
 | rangeStartStep | uint8 | The start value for this element in 'blocks' of 25  where 0 == 900 and 48 == 2100 |
 | rangeEndStep | uint8 | The end value for this element in 'blocks' of 25 where 0 == 900 and 48 == 2100 |
 
-Thus, for a cleanflight firmware with 40 slots 160 bytes would be returned in response to MSP\_MODE\_RANGES,
+Thus, for a cleanflight firmware with 40 slots 160 bytes would be returned in response to MSP\_MODE\_RANGES.
+
+### MSP\_MODE\_RANGES\_EXTRA
+
+The MSP\_MODE\_RANGES\_EXTRA returns the extra mode setting parameters from the flight controller. It should be invoked
+in conjunction with MSP\_MODE\_RANGES before any modification is made to the configuration.
+
+The message returns the number of extra elements followed by a group of bytes for each 'slot' available in the flight
+controller. The number of slots should be the same as for MSP\_MODE\_RANGES, calculated from the size of the returned
+message and the number of bytes per group.
+
+| Command | Msg Id | Direction | Notes |
+|---------|--------|-----------|-------|
+| MSP\_MODE\_RANGES\_EXTRA | 238 | to FC | Following this command, the FC returns a block of bytes for each auxiliary mode 'slot'|
+
+The return message is prepended with the number of bytes per element (3 bytes). Each element contains the
+following fields:
+
+| Data | Type | Notes |
+|------|------|-------|
+| permanentId | uint8 | See Modes.md for a definition of the permanent ids |
+| modeLogic | uint8 | 0 = Logic AND; 1 = Logic OR |
+| linkedTo | uint8 | Permanent id to which this mode is linked. Range is ignored if linked. |
+
+Thus, for a cleanflight firmware with 20 slots, 61 bytes (including prepended size) would be returned in response to
+MSP\_MODE\_RANGES\_EXTRA.
 
 ### MSP\_SET\_MODE\_RANGE
 

src/main/msp/msp.c

@@ -1031,6 +1031,18 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
         }
         break;
 
+    case MSP_MODE_RANGES_EXTRA:
+        sbufWriteU8(dst, MAX_MODE_ACTIVATION_CONDITION_COUNT);          // prepend number of EXTRAs array elements
+
+        for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
+            const modeActivationCondition_t *mac = modeActivationConditions(i);
+            const box_t *box = findBoxByBoxId(mac->modeId);
+            sbufWriteU8(dst, box->permanentId);     // each element is aligned with MODE_RANGES by the permanentId
+            sbufWriteU8(dst, mac->modeLogic);
+            sbufWriteU8(dst, mac->linkedTo);
+        }
+        break;
+
     case MSP_ADJUSTMENT_RANGES:
         for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
             const adjustmentRange_t *adjRange = adjustmentRanges(i);
@@ -1723,7 +1735,12 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
                 mac->auxChannelIndex = sbufReadU8(src);
                 mac->range.startStep = sbufReadU8(src);
                 mac->range.endStep = sbufReadU8(src);
+                if (sbufBytesRemaining(src) != 0) {
+                    mac->modeLogic = sbufReadU8(src);
 
+                    i = sbufReadU8(src);
+                    mac->linkedTo = findBoxByPermanentId(i)->boxId;
+                }
                 rcControlsInit();
             } else {
                 return MSP_RESULT_ERROR;

src/main/msp/msp_protocol.h

@@ -326,6 +326,7 @@
 #define MSP_GPSSVINFO            164    //out message         get Signal Strength (only U-Blox)
 #define MSP_GPSSTATISTICS        166    //out message         get GPS debugging data
 #define MSP_MULTIPLE_MSP         230    //out message         request multiple MSPs in one request - limit is the TX buffer; returns each MSP in the order they were requested starting with length of MSP; MSPs with input arguments are not supported
+#define MSP_MODE_RANGES_EXTRA    238    //out message         Reads the extra mode range data
 #define MSP_ACC_TRIM             240    //out message         get acc angle trim values
 #define MSP_SET_ACC_TRIM         239    //in message          set acc angle trim values
 #define MSP_SERVO_MIX_RULES      241    //out message         Returns servo mixer configuration