Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: add stability patch test to example sketch

Browse Source
This commit is contained in:
Randy Mackay 2014-04-09 21:20:56 +09:00
parent b1b96ec8e9
commit b78e59ea30
1 changed files with 110 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

118
libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde
View File

@ -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();