Copter: motorsync cli test

ArduCopter/test.pde

@@ -12,6 +12,7 @@ static int8_t   test_gps(uint8_t argc,                  const Menu::arg *argv);
 static int8_t   test_ins(uint8_t argc,                  const Menu::arg *argv);
 static int8_t   test_logging(uint8_t argc,              const Menu::arg *argv);
 static int8_t   test_motors(uint8_t argc,               const Menu::arg *argv);
+static int8_t   test_motorsync(uint8_t argc,            const Menu::arg *argv);
 static int8_t   test_optflow(uint8_t argc,              const Menu::arg *argv);
 static int8_t   test_radio_pwm(uint8_t argc,            const Menu::arg *argv);
 static int8_t   test_radio(uint8_t argc,                const Menu::arg *argv);
@@ -36,6 +37,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
     {"ins",                 test_ins},
     {"logging",             test_logging},
     {"motors",              test_motors},
+    {"motorsync",           test_motorsync},
     {"optflow",             test_optflow},
     {"pwm",                 test_radio_pwm},
     {"radio",               test_radio},
@@ -275,6 +277,129 @@ test_motors(uint8_t argc, const Menu::arg *argv)
     }
 }
 
+
+// test_motorsync - suddenly increases pwm output to motors to test if ESC loses sync
+static int8_t
+test_motorsync(uint8_t argc, const Menu::arg *argv)
+{
+    bool     test_complete = false;
+    bool     spin_motors = false;
+    uint32_t spin_start_time;
+    uint32_t last_run_time;
+    int16_t  last_throttle = 0;
+    int16_t  c;
+
+    // check if radio is calibration
+    pre_arm_rc_checks();
+    if (!ap.pre_arm_rc_check) {
+        cliSerial->print_P(PSTR("radio not calibrated\n"));
+        return 0;
+    }
+
+    // print warning that motors will spin
+    // ask user to raise throttle
+    // inform how to stop test
+    cliSerial->print_P(PSTR("This sends sudden outputs to the motors based on the pilot's throttle to test for ESC loss of sync. Motors will spin so mount props up-side-down!\n   Hold throttle low, then raise throttle stick to desired level and press A.  Motors will spin for 2 sec and then return to low.\nPress any key to exit.\n"));
+
+    // clear out user input
+    while (cliSerial->available()) {
+        cliSerial->read();
+    }
+
+    // disable throttle and battery failsafe
+    g.failsafe_throttle = FS_THR_DISABLED;
+    g.failsafe_battery_enabled = FS_BATT_DISABLED;
+
+    // read radio
+    read_radio();
+
+    // exit immediately if throttle is not zero
+    if( g.rc_3.control_in != 0 ) {
+        cliSerial->print_P(PSTR("throttle not zero\n"));
+        return 0;
+    }
+
+    // clear out any user input
+    while (cliSerial->available()) {
+        cliSerial->read();
+    }
+
+    // enable motors and pass through throttle
+    init_rc_out();
+    output_min();
+    motors.armed(true);
+
+    // initialise run time
+    last_run_time = millis();
+
+    // main run while the test is not complete
+    while(!test_complete) {
+        // 50hz loop
+        if( millis() - last_run_time > 20 ) {
+            last_run_time = millis();
+
+            // read radio input
+            read_radio();
+
+            // display throttle value
+            if (abs(g.rc_3.control_in-last_throttle) > 10) {
+                cliSerial->printf_P(PSTR("\nThr:%d"),g.rc_3.control_in);
+                last_throttle = g.rc_3.control_in;
+            }
+
+            // decode user input
+            if (cliSerial->available()) {
+                c = cliSerial->read();
+                if (c == 'a' || c == 'A') {
+                    spin_motors = true;
+                    spin_start_time = millis();
+                    // display user's throttle level
+                    cliSerial->printf_P(PSTR("\nSpin motors at:%d"),(int)g.rc_3.control_in);
+                    // clear out any other use input queued up
+                    while (cliSerial->available()) {
+                        cliSerial->read();
+                    }
+                }else{
+                    // any other input ends the test
+                    test_complete = true;
+                    motors.armed(false);
+                }
+            }
+
+            // check if time to stop motors
+            if (spin_motors) {
+                if ((millis() - spin_start_time) > 2000) {
+                    spin_motors = false;
+                    cliSerial->printf_P(PSTR("\nMotors stopped"));
+                }
+            }
+
+            // output to motors
+            if (spin_motors) {
+                // pass pilot throttle through to motors
+                motors.throttle_pass_through();
+            }else{
+                // spin motors at minimum
+                output_min();
+            }
+        }
+    }
+
+    // stop motors
+    motors.output_min();
+    motors.armed(false);
+
+    // clear out any user input
+    while( cliSerial->available() ) {
+        cliSerial->read();
+    }
+
+    // display completion message
+    cliSerial->printf_P(PSTR("\nTest complete\n"));
+
+    return 0;
+}
+
 static int8_t
 test_optflow(uint8_t argc, const Menu::arg *argv)
 {