From b78e59ea30df1a9f44f8f0f54ad1e589d9c2316e Mon Sep 17 00:00:00 2001 From: Randy Mackay <rmackay9@yahoo.com> Date: Wed, 9 Apr 2014 21:20:56 +0900 Subject: [PATCH] AP_Motors: add stability patch test to example sketch --- .../AP_Motors_test/AP_Motors_test.pde | 118 ++++++++++++++++-- 1 file changed, 110 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde index 00ac1beb7..3b96604a8 100644 --- a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde +++ b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde @@ -55,8 +55,10 @@ void setup() motors.set_update_rate(490); motors.set_frame_orientation(AP_MOTORS_X_FRAME); motors.set_min_throttle(130); + motors.set_mid_throttle(500); motors.Init(); // initialise motors + // setup radio if (rc3.radio_min == 0) { // cope with AP_Param not being loaded rc3.radio_min = 1000; @@ -65,6 +67,11 @@ void setup() // cope with AP_Param not being loaded rc3.radio_max = 2000; } + // set rc channel ranges + rc1.set_angle(4500); + rc2.set_angle(4500); + rc3.set_range(130, 1000); + rc4.set_angle(4500); motors.enable(); motors.output_min(); @@ -75,10 +82,10 @@ void setup() // loop void loop() { - int value; + int16_t value; // display help - hal.console->println("Press 't' to test motors. Be careful they will spin!"); + hal.console->println("Press 't' to run motor orders test, 's' to run stability patch test. Be careful the motors will spin!"); // wait for user to enter something while( !hal.console->available() ) { @@ -89,13 +96,108 @@ void loop() value = hal.console->read(); // test motors - if( value == 't' || value == 'T' ) { - hal.console->println("testing motors..."); - motors.armed(true); - motors.output_test(); - motors.armed(false); - hal.console->println("finished test."); + if (value == 't' || value == 'T') { + motor_order_test(); } + if (value == 's' || value == 'S') { + stability_test(); + } +} + +// stability_test +void motor_order_test() +{ + hal.console->println("testing motor order"); + motors.armed(true); + motors.output_test(); + motors.armed(false); + hal.console->println("finished test."); + +} + +// stability_test +void stability_test() +{ + int16_t value, roll_in, pitch_in, yaw_in, throttle_in, throttle_radio_in, avg_out; + + int16_t testing_array[][4] = { + // roll, pitch, yaw, throttle + { 0, 0, 0, 0}, + { 0, 0, 0, 100}, + { 0, 0, 0, 200}, + { 0, 0, 0, 300}, + { 4500, 0, 0, 300}, + { -4500, 0, 0, 300}, + { 0, 4500, 0, 300}, + { 0, -4500, 0, 300}, + { 0, 0, 4500, 300}, + { 0, 0, -4500, 300}, + { 0, 0, 0, 400}, + { 0, 0, 0, 500}, + { 0, 0, 0, 600}, + { 0, 0, 0, 700}, + { 0, 0, 0, 800}, + { 0, 0, 0, 900}, + { 0, 0, 0, 1000}, + { 4500, 0, 0, 1000}, + { -4500, 0, 0, 1000}, + { 0, 4500, 0, 1000}, + { 0, -4500, 0, 1000}, + { 0, 0, 4500, 1000}, + { 0, 0, -4500, 1000}, + {5000, 1000, 0, 1000}, + {5000, 2000, 0, 1000}, + {5000, 3000, 0, 1000}, + {5000, 4000, 0, 1000}, + {5000, 5000, 0, 1000}, + {5000, 0, 1000, 1000}, + {5000, 0, 2000, 1000}, + {5000, 0, 3000, 1000}, + {5000, 0, 4500, 1000} + }; + uint32_t testing_array_rows = 32; + + hal.console->printf_P(PSTR("\nTesting stability patch\nThrottle Min:%d Max:%d\n"),(int)rc3.radio_min,(int)rc3.radio_max); + + // arm motors + motors.armed(true); + + // run stability test + for (int16_t i=0; i < testing_array_rows; i++) { + roll_in = testing_array[i][0]; + pitch_in = testing_array[i][1]; + yaw_in = testing_array[i][2]; + throttle_in = testing_array[i][3]; + motors.set_pitch(roll_in); + motors.set_roll(pitch_in); + motors.set_yaw(yaw_in); + motors.set_throttle(throttle_in); + motors.output(); + // calc average output + throttle_radio_in = rc3.radio_out; + avg_out = ((hal.rcout->read(0) + hal.rcout->read(1) + hal.rcout->read(2) + hal.rcout->read(3))/4); + + // display input and output + hal.console->printf_P(PSTR("R:%5d \tP:%5d \tY:%5d \tT:%5d\tMOT1:%5d \tMOT2:%5d \tMOT3:%5d \tMOT4:%5d \t ThrIn/AvgOut:%5d/%5d\n"), + (int)roll_in, + (int)pitch_in, + (int)yaw_in, + (int)throttle_in, + (int)hal.rcout->read(0), + (int)hal.rcout->read(1), + (int)hal.rcout->read(2), + (int)hal.rcout->read(3), + (int)throttle_radio_in, + (int)avg_out); + } + // set all inputs to motor library to zero and disarm motors + motors.set_pitch(0); + motors.set_roll(0); + motors.set_yaw(0); + motors.set_throttle(0); + motors.armed(false); + + hal.console->println("finished test."); } AP_HAL_MAIN(); -- GitLab