Merge pull request #517 from sherlockflight/servo-cli

Add "servo" CLI command to get/set servo conf, and include in dump

src/main/io/serial_cli.c

@@ -97,6 +97,7 @@ static void cliProfile(char *cmdline);
 static void cliRateProfile(char *cmdline);
 static void cliReboot(void);
 static void cliSave(char *cmdline);
+static void cliServo(char *cmdline);
 static void cliSet(char *cmdline);
 static void cliGet(char *cmdline);
 static void cliStatus(char *cmdline);
@@ -198,6 +199,7 @@ const clicmd_t cmdTable[] = {
     { "profile", "index (0 to 2)", cliProfile },
     { "rateprofile", "index (0 to 2)", cliRateProfile },
     { "save", "save and reboot", cliSave },
+    { "servo", "servo config", cliServo },
     { "set", "name=value or blank or * for list", cliSet },
     { "status", "show system status", cliStatus },
     { "version", "", cliVersion },
@@ -477,14 +479,18 @@ static char *processChannelRangeArgs(char *ptr, channelRange_t *range, uint8_t *
     return ptr;
 }
 
+// Check if a string's length is zero
+static bool isEmpty(const char *string)
+{
+    return *string == '\0';
+}
+
 static void cliAux(char *cmdline)
 {
     int i, val = 0;
-    uint8_t len;
     char *ptr;
 
-    len = strlen(cmdline);
+    if (isEmpty(cmdline)) {
-    if (len == 0) {
         // print out aux channel settings
         for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
             modeActivationCondition_t *mac = &currentProfile->modeActivationConditions[i];
@@ -533,11 +539,9 @@ static void cliAux(char *cmdline)
 static void cliAdjustmentRange(char *cmdline)
 {
     int i, val = 0;
-    uint8_t len;
     char *ptr;
 
-    len = strlen(cmdline);
+    if (isEmpty(cmdline)) {
-    if (len == 0) {
         // print out adjustment ranges channel settings
         for (i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
             adjustmentRange_t *ar = &currentProfile->adjustmentRanges[i];
@@ -612,9 +616,7 @@ static void cliCMix(char *cmdline)
     float mixsum[3];
     char *ptr;
 
-    len = strlen(cmdline);
+    if (isEmpty(cmdline)) {
-
-    if (len == 0) {
         cliPrint("Custom mixer: \r\nMotor\tThr\tRoll\tPitch\tYaw\r\n");
         for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
             if (masterConfig.customMixer[i].throttle == 0.0f)
@@ -698,12 +700,10 @@ static void cliCMix(char *cmdline)
 static void cliLed(char *cmdline)
 {
     int i;
-    uint8_t len;
     char *ptr;
     char ledConfigBuffer[20];
 
-    len = strlen(cmdline);
+    if (isEmpty(cmdline)) {
-    if (len == 0) {
         for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) {
             generateLedConfig(i, ledConfigBuffer, sizeof(ledConfigBuffer));
             printf("led %u %s\r\n", i, ledConfigBuffer);
@@ -714,7 +714,7 @@ static void cliLed(char *cmdline)
         if (i < MAX_LED_STRIP_LENGTH) {
             ptr = strchr(cmdline, ' ');
             if (!parseLedStripConfig(i, ++ptr)) {
-                printf("Parse error\r\n", MAX_LED_STRIP_LENGTH);
+                cliPrint("Parse error\r\n");
             }
         } else {
             printf("Invalid led index: must be < %u\r\n", MAX_LED_STRIP_LENGTH);
@@ -725,11 +725,9 @@ static void cliLed(char *cmdline)
 static void cliColor(char *cmdline)
 {
     int i;
-    uint8_t len;
     char *ptr;
 
-    len = strlen(cmdline);
+    if (isEmpty(cmdline)) {
-    if (len == 0) {
         for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) {
             printf("color %u %d,%u,%u\r\n", i, masterConfig.colors[i].h, masterConfig.colors[i].s, masterConfig.colors[i].v);
         }
@@ -739,7 +737,7 @@ static void cliColor(char *cmdline)
         if (i < CONFIGURABLE_COLOR_COUNT) {
             ptr = strchr(cmdline, ' ');
             if (!parseColor(i, ++ptr)) {
-                printf("Parse error\r\n", CONFIGURABLE_COLOR_COUNT);
+                cliPrint("Parse error\r\n");
             }
         } else {
             printf("Invalid color index: must be < %u\r\n", CONFIGURABLE_COLOR_COUNT);
@@ -748,6 +746,74 @@ static void cliColor(char *cmdline)
 }
 #endif
 
+static void cliServo(char *cmdline)
+{
+    enum { SERVO_ARGUMENT_COUNT = 6 };
+    int16_t arguments[SERVO_ARGUMENT_COUNT];
+
+    servoParam_t *servo;
+
+    int i;
+    char *ptr;
+
+    if (isEmpty(cmdline)) {
+        // print out servo settings
+        for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
+            servo = &currentProfile->servoConf[i];
+
+            printf("servo %u %d %d %d %d %d\r\n",
+                i,
+                servo->min,
+                servo->max,
+                servo->middle,
+                servo->rate,
+                servo->forwardFromChannel
+            );
+        }
+    } else {
+        int validArgumentCount = 0;
+
+        ptr = cmdline;
+
+        // Command line is integers (possibly negative) separated by spaces, no other characters allowed.
+
+        // If command line doesn't fit the format, don't modify the config
+        while (*ptr) {
+            if (*ptr == '-' || (*ptr >= '0' && *ptr <= '9')) {
+                if (validArgumentCount >= SERVO_ARGUMENT_COUNT) {
+                    cliPrint("Parse error\r\n");
+                    return;
+                }
+
+                arguments[validArgumentCount++] = atoi(ptr);
+
+                do {
+                    ptr++;
+                } while (*ptr >= '0' && *ptr <= '9');
+            } else if (*ptr == ' ') {
+                ptr++;
+            } else {
+                cliPrint("Parse error\r\n");
+                return;
+            }
+        }
+
+        // Check we got the right number of args and the servo index is correct (don't validate the other values)
+        if (validArgumentCount != SERVO_ARGUMENT_COUNT || arguments[0] < 0 || arguments[0] >= MAX_SUPPORTED_SERVOS) {
+            cliPrint("Parse error\r\n");
+            return;
+        }
+
+        servo = &currentProfile->servoConf[arguments[0]];
+
+        servo->min = arguments[1];
+        servo->max = arguments[2];
+        servo->middle = arguments[3];
+        servo->rate = arguments[4];
+        servo->forwardFromChannel = arguments[5];
+    }
+}
+
 static void dumpValues(uint16_t mask)
 {
     uint32_t i;
@@ -801,11 +867,11 @@ static void cliDump(char *cmdline)
 
     if (dumpMask & DUMP_MASTER) {
 
-        printf("\r\n# version\r\n");
+        cliPrint("\r\n# version\r\n");
         cliVersion(NULL);
 
-        printf("\r\n# dump master\r\n");
+        cliPrint("\r\n# dump master\r\n");
-        printf("\r\n# mixer\r\n");
+        cliPrint("\r\n# mixer\r\n");
 
 #ifndef USE_QUAD_MIXER_ONLY
         printf("mixer %s\r\n", mixerNames[masterConfig.mixerMode - 1]);
@@ -820,23 +886,23 @@ static void cliDump(char *cmdline)
                 yaw = masterConfig.customMixer[i].yaw;
                 printf("cmix %d", i + 1);
                 if (thr < 0)
-                    printf(" ");
+                    cliWrite(' ');
                 printf("%s", ftoa(thr, buf));
                 if (roll < 0)
-                    printf(" ");
+                    cliWrite(' ');
                 printf("%s", ftoa(roll, buf));
                 if (pitch < 0)
-                    printf(" ");
+                    cliWrite(' ');
                 printf("%s", ftoa(pitch, buf));
                 if (yaw < 0)
-                    printf(" ");
+                    cliWrite(' ');
                 printf("%s\r\n", ftoa(yaw, buf));
             }
             printf("cmix %d 0 0 0 0\r\n", i + 1);
         }
 #endif
 
-        printf("\r\n\r\n# feature\r\n");
+        cliPrint("\r\n\r\n# feature\r\n");
 
         mask = featureMask();
         for (i = 0; ; i++) { // disable all feature first
@@ -851,7 +917,7 @@ static void cliDump(char *cmdline)
                 printf("feature %s\r\n", featureNames[i]);
         }
 
-        printf("\r\n\r\n# map\r\n");
+        cliPrint("\r\n\r\n# map\r\n");
 
         for (i = 0; i < 8; i++)
             buf[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i];
@@ -859,10 +925,10 @@ static void cliDump(char *cmdline)
         printf("map %s\r\n", buf);
 
 #ifdef LED_STRIP
-        printf("\r\n\r\n# led\r\n");
+        cliPrint("\r\n\r\n# led\r\n");
         cliLed("");
 
-        printf("\r\n\r\n# color\r\n");
+        cliPrint("\r\n\r\n# color\r\n");
         cliColor("");
 #endif
         printSectionBreak();
@@ -870,28 +936,32 @@ static void cliDump(char *cmdline)
     }
 
     if (dumpMask & DUMP_PROFILE) {
-        printf("\r\n# dump profile\r\n");
+        cliPrint("\r\n# dump profile\r\n");
 
-        printf("\r\n# profile\r\n");
+        cliPrint("\r\n# profile\r\n");
         cliProfile("");
 
-        printf("\r\n# aux\r\n");
+        cliPrint("\r\n# aux\r\n");
 
         cliAux("");
 
-        printf("\r\n# adjrange\r\n");
+        cliPrint("\r\n# adjrange\r\n");
 
         cliAdjustmentRange("");
 
+        cliPrint("\r\n# servo\r\n");
+
+        cliServo("");
+
         printSectionBreak();
 
         dumpValues(PROFILE_VALUE);
     }
 
     if (dumpMask & DUMP_CONTROL_RATE_PROFILE) {
-        printf("\r\n# dump rates\r\n");
+        cliPrint("\r\n# dump rates\r\n");
 
-        printf("\r\n# rateprofile\r\n");
+        cliPrint("\r\n# rateprofile\r\n");
         cliRateProfile("");
 
         printSectionBreak();
@@ -1088,12 +1158,11 @@ static void cliMotor(char *cmdline)
 {
     int motor_index = 0;
     int motor_value = 0;
-    int len, index = 0;
+    int index = 0;
     char *pch = NULL;
 
-    len = strlen(cmdline);
+    if (isEmpty(cmdline)) {
-    if (len == 0) {
+        cliPrint("Usage:\r\nmotor index [value] - show [or set] motor value\r\n");
-        printf("Usage:\r\nmotor index [value] - show [or set] motor value\r\n");
         return;
     }
 
@@ -1122,7 +1191,7 @@ static void cliMotor(char *cmdline)
     }
 
     if (motor_value < PWM_RANGE_MIN || motor_value > PWM_RANGE_MAX) {
-        printf("Invalid motor value, 1000..2000\r\n");
+        cliPrint("Invalid motor value, 1000..2000\r\n");
         return;
     }
 
@@ -1132,11 +1201,9 @@ static void cliMotor(char *cmdline)
 
 static void cliProfile(char *cmdline)
 {
-    uint8_t len;
     int i;
 
-    len = strlen(cmdline);
+    if (isEmpty(cmdline)) {
-    if (len == 0) {
         printf("profile %d\r\n", getCurrentProfile());
         return;
     } else {
@@ -1152,11 +1219,9 @@ static void cliProfile(char *cmdline)
 
 static void cliRateProfile(char *cmdline)
 {
-    uint8_t len;
     int i;
 
-    len = strlen(cmdline);
+    if (isEmpty(cmdline)) {
-    if (len == 0) {
         printf("rateprofile %d\r\n", getCurrentControlRateProfile());
         return;
     } else {
@@ -1350,7 +1415,7 @@ static void cliGet(char *cmdline)
             val = &valueTable[i];
             printf("%s = ", valueTable[i].name);
             cliPrintVar(val, 0);
-            printf("\r\n");
+            cliPrint("\r\n");
 
             matchedCommands++;
         }