Add mspGetOptionalIndex()

rotorflight · Nov 19, 2024 · fcb42e2 · fcb42e2
1 parent 099f71d
commit fcb42e2
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 0 deletions.
diff --git a/src/main/common/types.h b/src/main/common/types.h
@@ -27,3 +27,9 @@ typedef unsigned char uchar;
 
 typedef uint32_t bitmap_t;
 typedef uint64_t bitmap64_t;
+
+typedef struct {
+    int min;
+    int max;
+} range_t;
+
diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
@@ -1990,6 +1990,23 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
     return !unsupportedCommand;
 }
 
+void mspGetOptionalIndex(sbuf_t *src, const range_t *range, int *value)
+{
+    if (sbufBytesRemaining(src) > 0) {
+        *value = constrain(sbufReadU8(src), range->min, range->max);
+    }
+}
+
+void mspGetOptionalIndexRange(sbuf_t *src, const range_t *range, range_t *value)
+{
+    if (sbufBytesRemaining(src) > 0) {
+        value->min = value->max = constrain(sbufReadU8(src), range->min, range->max);
+        if (sbufBytesRemaining(src) > 0) {
+            value->max = constrain(sbufReadU8(src), value->min, range->max);
+        }
+    }
+}
+
 static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
 {