diff --git a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp index b68a58fda5..31b07ad3c1 100644 --- a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp +++ b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp @@ -3,6 +3,12 @@ * Code by Randy Mackay. DIYDrones.com */ +/* on Linux run with + ./waf configure --board linux + ./waf --targets examples/AP_Motors_test + ./build/linux/examples/AP_Motors_test +*/ + // Libraries #include #include @@ -11,6 +17,7 @@ #include #include #include +#include const AP_HAL::HAL& hal = AP_HAL::get_HAL(); @@ -41,14 +48,69 @@ void setup() hal.console->printf("AP_Motors library test ver 1.0\n"); // motor initialisation + motors.set_dt(1.0/400.0); motors.set_update_rate(490); + +#if NUM_OUTPUTS == 8 + motors.init(AP_Motors::MOTOR_FRAME_OCTA, AP_Motors::MOTOR_FRAME_TYPE_X); +#elif NUM_OUTPUTS == 6 + motors.init(AP_Motors::MOTOR_FRAME_HEXA, AP_Motors::MOTOR_FRAME_TYPE_X); +#else motors.init(AP_Motors::MOTOR_FRAME_QUAD, AP_Motors::MOTOR_FRAME_TYPE_X); +#endif + + char frame_and_type_string[30]; + motors.get_frame_and_type_string(frame_and_type_string, ARRAY_SIZE(frame_and_type_string)); + hal.console->printf("%s\n", frame_and_type_string); + #if HELI_TEST == 0 motors.update_throttle_range(); motors.set_throttle_avg_max(0.5f); #endif motors.output_min(); + // allow command line args for single run + uint8_t argc; + char * const *argv; + hal.util->commandline_arguments(argc, argv); + if (argc > 1) { + for (uint8_t i = 2; i < argc; i++) { + const char* arg = argv[i]; + + const char *eq = strchr(arg, '='); + if (eq == NULL) { + ::printf("Expected argument with \"=\"\n"); + exit(1); + } + char cmd[20] {}; + strncpy(cmd, arg, eq-arg); + const float value = atof(eq+1); + if (strcmp(cmd,"yaw_headroom") == 0) { + motors.set_yaw_headroom(value); + + } else if (strcmp(cmd,"throttle_avg_max") == 0) { + motors.set_throttle_avg_max(value); + + } else { + ::printf("Expected \"yaw_headroom\" or \"throttle_avg_max\"\n"); + exit(1); + } + + } + if (strcmp(argv[1],"t") == 0) { + motor_order_test(); + + } else if (strcmp(argv[1],"s") == 0) { + stability_test(); + + } else { + ::printf("Expected first argument, 't' or 's'\n"); + + } + hal.scheduler->delay(1000); + exit(0); + } + hal.scheduler->delay(1000); } @@ -71,9 +133,11 @@ void loop() // test motors if (value == 't' || value == 'T') { motor_order_test(); + hal.console->printf("finished test.\n"); } if (value == 's' || value == 'S') { stability_test(); + hal.console->printf("finished test.\n"); } } @@ -90,75 +154,58 @@ void motor_order_test() hal.scheduler->delay(2000); } motors.armed(false); - hal.console->printf("finished test.\n"); } // stability_test void stability_test() { - int16_t roll_in, pitch_in, yaw_in, avg_out; - float throttle_in; + hal.console->printf("Throttle average max: %0.4f\n", motors.get_throttle_avg_max()); + hal.console->printf("Yaw headroom: %i\n", motors.get_yaw_headroom()); - int16_t throttle_tests[] = {0, 100, 200, 300, 400, 500, 600, 700, 800, 900, 1000}; - uint8_t throttle_tests_num = sizeof(throttle_tests) / sizeof(int16_t); - int16_t rpy_tests[] = {0, 1000, 2000, 3000, 4500, -1000, -2000, -3000, -4500}; - uint8_t rpy_tests_num = sizeof(rpy_tests) / sizeof(int16_t); + const float throttle_tests[] = {0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0}; + const uint8_t throttle_tests_num = ARRAY_SIZE(throttle_tests); + const float rpy_tests[] = {-1.0, -0.9, -0.8, -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0}; + const uint8_t rpy_tests_num = ARRAY_SIZE(rpy_tests); // arm motors motors.armed(true); motors.set_interlock(true); SRV_Channels::enable_aux_servos(); -#if NUM_OUTPUTS <= 4 - hal.console->printf("Roll,Pitch,Yaw,Thr,Mot1,Mot2,Mot3,Mot4,AvgOut,LimRP,LimY,LimThD,LimThU\n"); // quad -#elif NUM_OUTPUTS <= 6 - hal.console->printf("Roll,Pitch,Yaw,Thr,Mot1,Mot2,Mot3,Mot4,Mot5,Mot6,AvgOut,LimRP,LimY,LimThD,LimThU\n"); // hexa -#else - hal.console->printf("Roll,Pitch,Yaw,Thr,Mot1,Mot2,Mot3,Mot4,Mot5,Mot6,Mot7,Mot8,AvgOut,LimRP,LimY,LimThD,LimThU\n"); // octa -#endif + hal.console->printf("Roll,Pitch,Yaw,Thr,"); + for (uint8_t i=0; iprintf("Mot%i,",i+1); + } + for (uint8_t i=0; iprintf("Mot%i_norm,",i+1); + } + hal.console->printf("LimR,LimP,LimY,LimThD,LimThU\n"); // run stability test for (uint8_t y=0; yread(0) + hal.rcout->read(1) + hal.rcout->read(2) + hal.rcout->read(3))/4); // display input and output -#if NUM_OUTPUTS <= 4 - hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // quad -#elif NUM_OUTPUTS <= 6 - hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // hexa -#else - hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // octa -#endif - (int)roll_in, - (int)pitch_in, - (int)yaw_in, - (double)throttle_in, - (int)hal.rcout->read(0), - (int)hal.rcout->read(1), - (int)hal.rcout->read(2), - (int)hal.rcout->read(3), -#if NUM_OUTPUTS >= 6 - (int)hal.rcout->read(4), - (int)hal.rcout->read(5), -#endif -#if NUM_OUTPUTS >= 8 - (int)hal.rcout->read(6), - (int)hal.rcout->read(7), -#endif - (int)avg_out, + hal.console->printf("%0.2f,%0.2f,%0.2f,%0.2f,", roll_in, pitch_in, yaw_in, throttle_in); + for (uint8_t i=0; iprintf("%d,",(int)hal.rcout->read(i)); + } + for (uint8_t i=0; iprintf("%0.4f,", motors.get_thrust_rpyt_out(i)); + } + hal.console->printf("%d,%d,%d,%d,%d\n", (int)motors.limit.roll, (int)motors.limit.pitch, (int)motors.limit.yaw, @@ -176,7 +223,6 @@ void stability_test() motors.set_throttle(0); motors.armed(false); - hal.console->printf("finished test.\n"); } void update_motors()