From 786d7277050be8b9a5b460d2155d6536f94f03e9 Mon Sep 17 00:00:00 2001
From: Davide Bertola <dade@dadeb.it>
Date: Fri, 1 Nov 2013 11:28:29 +0100
Subject: [PATCH] Add cli 'motor' command

It allows to get and set single motor output
value.
---
 src/cli.c   | 43 +++++++++++++++++++++++++++++++++++++++++++
 src/mixer.c | 11 +++++++++--
 2 files changed, 52 insertions(+), 2 deletions(-)

diff --git a/src/cli.c b/src/cli.c
index 002c46018d..052b834a46 100644
--- a/src/cli.c
+++ b/src/cli.c
@@ -12,6 +12,7 @@ static void cliFeature(char *cmdline);
 static void cliHelp(char *cmdline);
 static void cliMap(char *cmdline);
 static void cliMixer(char *cmdline);
+static void cliMotor(char *cmdline);
 static void cliProfile(char *cmdline);
 static void cliSave(char *cmdline);
 static void cliSet(char *cmdline);
@@ -25,6 +26,9 @@ extern uint8_t accHardware;
 // from config.c RC Channel mapping
 extern const char rcChannelLetters[];
 
+// from mixer.c
+extern int16_t motor_disarmed[MAX_MOTORS];
+
 // buffer
 static char cliBuffer[48];
 static uint32_t bufferIndex = 0;
@@ -74,6 +78,7 @@ const clicmd_t cmdTable[] = {
     { "help", "", cliHelp },
     { "map", "mapping of rc channel order", cliMap },
     { "mixer", "mixer name or list", cliMixer },
+    { "motor", "get/set motor output value", cliMotor },
     { "profile", "index (0 to 2)", cliProfile },
     { "save", "save and reboot", cliSave },
     { "set", "name=value or blank or * for list", cliSet },
@@ -725,6 +730,44 @@ static void cliMixer(char *cmdline)
     }
 }
 
+static void cliMotor(char *cmdline)
+{
+    int motor_index;
+    int motor_value;
+    int len;
+    char *tmp = NULL;
+    char *sep = " ";
+
+    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");
+        return;
+    }
+    motor_index = atoi(tmp);
+
+    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]);
+        return;
+    }
+    motor_value = atoi(tmp);
+
+    printf("Setting motor %d to %d\r\n", motor_index, motor_value);
+    motor_disarmed[motor_index] = motor_value;
+}
+
 static void cliProfile(char *cmdline)
 {
     uint8_t len;
diff --git a/src/mixer.c b/src/mixer.c
index 87d31aa72b..a86f108a4f 100755
--- a/src/mixer.c
+++ b/src/mixer.c
@@ -3,6 +3,7 @@
 
 static uint8_t numberMotor = 0;
 int16_t motor[MAX_MOTORS];
+int16_t motor_disarmed[MAX_MOTORS];
 int16_t servo[MAX_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
 
 static motorMixer_t currentMixer[MAX_MOTORS];
@@ -192,6 +193,11 @@ void mixerInit(void)
             }
         }
     }
+
+    // set disarmed motor values
+    for (i = 0; i < MAX_MOTORS; i++) {
+        motor_disarmed[i] = feature(FEATURE_3D) ? mcfg.neutral3d : mcfg.mincommand;
+    }
 }
 
 void mixerLoadMix(int index)
@@ -436,7 +442,8 @@ void mixTable(void)
                     motor[i] = mcfg.mincommand;
             }
         }
-        if (!f.ARMED)
-            motor[i] = feature(FEATURE_3D) ? mcfg.neutral3d : mcfg.mincommand;
+        if (!f.ARMED) {
+            motor[i] = motor_disarmed[i];
+        }
     }
 }