From 111d0854a7a3daa24ad3d6ffc8d6672ec7a80ed3 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 18 Jan 2014 17:00:43 +0900
Subject: [PATCH] Copter: motorsync cli test
---
ArduCopter/test.pde | 125 ++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 125 insertions(+)
diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde
index 5b45c8167..1f7b21ba8 100644
--- a/ArduCopter/test.pde
+++ b/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)
{
--
GitLab