Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: example sketch tests stab patch more thoroughly

This commit is contained in:
Leonard Hall 2015-12-07 17:23:48 +09:00 committed by Randy Mackay
parent cef3f42df5
commit 90b3d7ca39
1 changed files with 57 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

134
libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp
View File

@ -29,6 +29,7 @@
#include <AP_NavEKF/AP_NavEKF.h>
#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <AP_Scheduler/AP_Scheduler.h>
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
@ -42,11 +43,11 @@ void update_motors();
RC_Channel rc1(0), rc2(1), rc3(2), rc4(3);
// uncomment the row below depending upon what frame you are using
//AP_MotorsTri motors(400);
//AP_MotorsTri motors(400);
AP_MotorsQuad motors(400);
//AP_MotorsHexa motors(400);
//AP_MotorsY6 motors(400);
//AP_MotorsOcta motors(400);
//AP_MotorsHexa motors(400);
//AP_MotorsY6 motors(400);
//AP_MotorsOcta motors(400);
//AP_MotorsOctaQuad motors(400);
//AP_MotorsHeli motors(rc1, rc2, rc3, rc4, 400);
@ -66,14 +67,9 @@ void setup()
motors.output_min();
// setup radio
if (rc3.radio_min == 0) {
// cope with AP_Param not being loaded
rc3.radio_min = 1000;
}
if (rc3.radio_max == 0) {
// cope with AP_Param not being loaded
rc3.radio_max = 2000;
}
rc3.radio_min = 1000;
rc3.radio_max = 2000;
// set rc channel ranges
rc1.set_angle(4500);
rc2.set_angle(4500);
@ -128,78 +124,62 @@ void motor_order_test()
// stability_test
void stability_test()
{
int16_t roll_in, pitch_in, yaw_in, throttle_in, avg_out;
int16_t roll_in, pitch_in, yaw_in, avg_out;
float throttle_in;
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;
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);
hal.console->printf("\nTesting stability patch\nThrottle Min:%d Max:%d\n",(int)rc3.radio_min,(int)rc3.radio_max);
// arm motors
motors.armed(true);
motors.set_interlock(true);
motors.set_stabilizing(true);
//hal.console->printf("Roll,Pitch,Yaw,Thr,Mot1,Mot2,Mot3,Mot4,AvgOut\n"); // quad
//hal.console->printf("Roll,Pitch,Yaw,Thr,Mot1,Mot2,Mot3,Mot4,Mot5,Mot6,AvgOut\n"); // hexa
//hal.console->printf("Roll,Pitch,Yaw,Thr,Mot1,Mot2,Mot3,Mot4,Mot5,Mot6,Mot7,Mot8,AvgOut\n"); // octa
// run stability test
for (uint16_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);
update_motors();
// calc average output
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("R:%5d \tP:%5d \tY:%5d \tT:%5d\tMOT1:%5d \tMOT2:%5d \tMOT3:%5d \tMOT4:%5d \t AvgOut:%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)avg_out);
for (uint8_t y=0; y<rpy_tests_num; y++) {
for (uint8_t p=0; p<rpy_tests_num; p++) {
for (uint8_t r=0; r<rpy_tests_num; r++) {
for (uint8_t t=0; t<throttle_tests_num; t++) {
roll_in = rpy_tests[r];
pitch_in = rpy_tests[p];
yaw_in = rpy_tests[y];
throttle_in = throttle_tests[t]/1000.0f;
motors.set_pitch(roll_in);
motors.set_roll(pitch_in);
motors.set_yaw(yaw_in);
motors.set_throttle(throttle_in);
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_THROTTLE_UNLIMITED);
motors.set_throttle_mix_mid();
update_motors();
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("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // octa
//hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d\n", // hexa
hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d\n", // quad
(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),
//(int)hal.rcout->read(4),
//(int)hal.rcout->read(5),
//(int)hal.rcout->read(6),
//(int)hal.rcout->read(7),
(int)avg_out);
}
}
}
}
// set all inputs to motor library to zero and disarm motors
motors.set_pitch(0);
motors.set_roll(0);
@ -212,8 +192,8 @@ void stability_test()
void update_motors()
{
// call update motors 1000 times to get any ramp limiting completed
for (uint16_t i=0; i<1000; i++) {
// call update motors 1000 times to get any ramp limiting complete
for (uint16_t i=0; i<4000; i++) {
motors.output();
}
}