mirror of https://github.com/ArduPilot/ardupilot
AP_Motors: Example: rework and update take optional arguments
This commit is contained in:
Iampete1
Andrew Tridgell
parent
a25aa8d2ac
commit
f7a8668c30
|
|
@ -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 <AP_Common/AP_Common.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
|
@ -11,6 +17,7 @@
|
|||
#include <AP_Motors/AP_Motors.h>
|
||||
#include <RC_Channel/RC_Channel.h>
|
||||
#include <SRV_Channel/SRV_Channel.h>
|
||||
#include <stdio.h>
|
||||
|
||||
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; i<NUM_OUTPUTS; i++) {
|
||||
hal.console->printf("Mot%i,",i+1);
|
||||
}
|
||||
for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
|
||||
hal.console->printf("Mot%i_norm,",i+1);
|
||||
}
|
||||
hal.console->printf("LimR,LimP,LimY,LimThD,LimThU\n");
|
||||
|
||||
// run stability test
|
||||
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_roll(roll_in/4500.0f);
|
||||
motors.set_pitch(pitch_in/4500.0f);
|
||||
motors.set_yaw(yaw_in/4500.0f);
|
||||
const float roll_in = rpy_tests[r];
|
||||
const float pitch_in = rpy_tests[p];
|
||||
const float yaw_in = rpy_tests[y];
|
||||
const float throttle_in = throttle_tests[t];
|
||||
motors.set_roll(roll_in);
|
||||
motors.set_pitch(pitch_in);
|
||||
motors.set_yaw(yaw_in);
|
||||
motors.set_throttle(throttle_in);
|
||||
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
||||
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
|
||||
#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; i<NUM_OUTPUTS; i++) {
|
||||
hal.console->printf("%d,",(int)hal.rcout->read(i));
|
||||
}
|
||||
for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
|
||||
hal.console->printf("%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()
|
||||
|
|
|
|||
Loading…
Reference in New Issue