cleaned up cliMotor(), added MSP_SET_MOTORS and enabled CAP_DYNBALANCE so the motors can be controlled from GUI

src/cli.c

@@ -602,6 +602,8 @@ static void cliExit(char *cmdline)
     *cliBuffer = '\0';
     bufferIndex = 0;
     cliMode = 0;
+    // incase some idiot leaves a motor running during motortest, clear it here
+    mixerResetMotors();
     // save and reboot... I think this makes the most sense
     cliSave(cmdline);
 }
@@ -734,37 +736,45 @@ static void cliMixer(char *cmdline)
 
 static void cliMotor(char *cmdline)
 {
-    int motor_index;
-    int motor_value;
-    int len;
-    char *tmp = NULL;
-    char *sep = " ";
+    int motor_index = 0;
+    int motor_value = 0;
+    int len, index = 0;
+    char *pch = NULL;
 
     len = strlen(cmdline);
-
-    tmp = strtok(cmdline, sep);
-    if (tmp == NULL) {
-        printf("Usage:\r\n");
-        printf("motor [index] shows the output value of one motor\r\n");
-        printf("motor [index] [value] sets the output value of one motor\r\n");
+    if (len == 0) {
+        printf("Usage:\r\nmotor index [value] - show [or set] motor value\r\n");
         return;
     }
-    motor_index = atoi(tmp);
+
+    pch = strtok(cmdline, " ");
+    while (pch != NULL) {
+        switch (index) {
+            case 0:
+                motor_index = atoi(pch);
+                break;
+            case 1:
+                motor_value = atoi(pch);
+                break;
+        }
+        index++;
+        pch = strtok(NULL, " ");
+    }
 
     if (motor_index < 0 || motor_index >= MAX_MOTORS) {
         printf("No such motor, use a number [0, %d]\r\n", MAX_MOTORS);
         return;
     }
 
-    tmp = strtok(NULL, sep);
-    if (tmp == NULL) {
-        tmp = strtok(cmdline, sep);
-        motor_index = atoi(tmp);
-        printf("Motor %d is set at %d\r\n", motor_index,
-            motor_disarmed[motor_index]);
+    if (index < 2) {
+        printf("Motor %d is set at %d\r\n", motor_index, motor_disarmed[motor_index]);
+        return;
+    }
+
+    if (motor_value < 1000 || motor_value > 2000) {
+        printf("Invalid motor value, 1000..2000\r\n");
         return;
     }
-    motor_value = atoi(tmp);
 
     printf("Setting motor %d to %d\r\n", motor_index, motor_value);
     motor_disarmed[motor_index] = motor_value;

src/mixer.c

@@ -211,11 +211,15 @@ void mixerInit(void)
             }
         }
     }
+    mixerResetMotors();
+}
 
+void mixerResetMotors(void)
+{
+    int i;
     // set disarmed motor values
-    for (i = 0; i < MAX_MOTORS; i++) {
+    for (i = 0; i < MAX_MOTORS; i++)
         motor_disarmed[i] = feature(FEATURE_3D) ? mcfg.neutral3d : mcfg.mincommand;
-    }
 }
 
 void mixerLoadMix(int index)

src/mw.h

@@ -412,6 +412,7 @@ void Sonar_update(void);
 
 // Output
 void mixerInit(void);
+void mixerResetMotors(void);
 void mixerLoadMix(int index);
 void writeServos(void);
 void writeMotors(void);

src/serial.c

@@ -3,7 +3,8 @@
 
 // Multiwii Serial Protocol 0
 #define MSP_VERSION              0
-#define PLATFORM_32BIT           ((uint32_t)1 << 31)
+#define CAP_PLATFORM_32BIT          ((uint32_t)1 << 31)
+#define CAP_DYNBALANCE              ((uint32_t)1 << 2)
 
 #define MSP_IDENT                100    //out message         multitype + multiwii version + protocol version + capability variable
 #define MSP_STATUS               101    //out message         cycletime & errors_count & sensor present & box activation & current setting number
@@ -89,6 +90,8 @@ struct box_t {
 static uint8_t availableBoxes[CHECKBOXITEMS];
 // this is the number of filled indexes in above array
 static uint8_t numberBoxItems = 0;
+// from mixer.c
+extern int16_t motor_disarmed[MAX_MOTORS];
 
 static const char pidnames[] =
     "ROLL;"
@@ -328,6 +331,10 @@ static void evaluateCommand(void)
         read8();                            // vbatlevel_crit (unused)
         headSerialReply(0);
         break;
+    case MSP_SET_MOTOR:
+        for (i = 0; i < 8; i++)
+            motor_disarmed[i] = read16();
+        break;
     case MSP_SELECT_SETTING:
         if (!f.ARMED) {
             mcfg.current_profile = read8();
@@ -347,7 +354,7 @@ static void evaluateCommand(void)
         serialize8(VERSION);                // multiwii version
         serialize8(mcfg.mixerConfiguration); // type of multicopter
         serialize8(MSP_VERSION);            // MultiWii Serial Protocol Version
-        serialize32(PLATFORM_32BIT);        // "capability"
+        serialize32(CAP_PLATFORM_32BIT | CAP_DYNBALANCE);        // "capability"
         break;
     case MSP_STATUS:
         headSerialReply(11);