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

AP_Motors: Example: rework and update take optional arguments

This commit is contained in:
Iampete1 2023-02-18 19:25:24 +00:00 committed by Andrew Tridgell
parent a25aa8d2ac
commit f7a8668c30
1 changed files with 93 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -3,6 +3,12 @@
* Code by Randy Mackay. DIYDrones.com * 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 // Libraries
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
@ -11,6 +17,7 @@
#include <AP_Motors/AP_Motors.h> #include <AP_Motors/AP_Motors.h>
#include <RC_Channel/RC_Channel.h> #include <RC_Channel/RC_Channel.h>
#include <SRV_Channel/SRV_Channel.h> #include <SRV_Channel/SRV_Channel.h>
#include <stdio.h>
const AP_HAL::HAL& hal = AP_HAL::get_HAL(); 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"); hal.console->printf("AP_Motors library test ver 1.0\n");
// motor initialisation // motor initialisation
motors.set_dt(1.0/400.0);
motors.set_update_rate(490); 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); 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 #if HELI_TEST == 0
motors.update_throttle_range(); motors.update_throttle_range();
motors.set_throttle_avg_max(0.5f); motors.set_throttle_avg_max(0.5f);
#endif #endif
motors.output_min(); 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); hal.scheduler->delay(1000);
} }
@ -71,9 +133,11 @@ void loop()
// test motors // test motors
if (value == 't' || value == 'T') { if (value == 't' || value == 'T') {
motor_order_test(); motor_order_test();
hal.console->printf("finished test.\n");
} }
if (value == 's' || value == 'S') { if (value == 's' || value == 'S') {
stability_test(); stability_test();
hal.console->printf("finished test.\n");
} }
} }
@ -90,75 +154,58 @@ void motor_order_test()
hal.scheduler->delay(2000); hal.scheduler->delay(2000);
} }
motors.armed(false); motors.armed(false);
hal.console->printf("finished test.\n");
} }
// stability_test // stability_test
void stability_test() void stability_test()
{ {
int16_t roll_in, pitch_in, yaw_in, avg_out; hal.console->printf("Throttle average max: %0.4f\n", motors.get_throttle_avg_max());
float throttle_in; 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}; 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};
uint8_t throttle_tests_num = sizeof(throttle_tests) / sizeof(int16_t); const uint8_t throttle_tests_num = ARRAY_SIZE(throttle_tests);
int16_t rpy_tests[] = {0, 1000, 2000, 3000, 4500, -1000, -2000, -3000, -4500}; 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};
uint8_t rpy_tests_num = sizeof(rpy_tests) / sizeof(int16_t); const uint8_t rpy_tests_num = ARRAY_SIZE(rpy_tests);
// arm motors // arm motors
motors.armed(true); motors.armed(true);
motors.set_interlock(true); motors.set_interlock(true);
SRV_Channels::enable_aux_servos(); SRV_Channels::enable_aux_servos();
#if NUM_OUTPUTS <= 4 hal.console->printf("Roll,Pitch,Yaw,Thr,");
hal.console->printf("Roll,Pitch,Yaw,Thr,Mot1,Mot2,Mot3,Mot4,AvgOut,LimRP,LimY,LimThD,LimThU\n"); // quad for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
#elif NUM_OUTPUTS <= 6 hal.console->printf("Mot%i,",i+1);
hal.console->printf("Roll,Pitch,Yaw,Thr,Mot1,Mot2,Mot3,Mot4,Mot5,Mot6,AvgOut,LimRP,LimY,LimThD,LimThU\n"); // hexa }
#else for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
hal.console->printf("Roll,Pitch,Yaw,Thr,Mot1,Mot2,Mot3,Mot4,Mot5,Mot6,Mot7,Mot8,AvgOut,LimRP,LimY,LimThD,LimThU\n"); // octa hal.console->printf("Mot%i_norm,",i+1);
#endif }
hal.console->printf("LimR,LimP,LimY,LimThD,LimThU\n");
// run stability test // run stability test
for (uint8_t y=0; y<rpy_tests_num; y++) { for (uint8_t y=0; y<rpy_tests_num; y++) {
for (uint8_t p=0; p<rpy_tests_num; p++) { for (uint8_t p=0; p<rpy_tests_num; p++) {
for (uint8_t r=0; r<rpy_tests_num; r++) { for (uint8_t r=0; r<rpy_tests_num; r++) {
for (uint8_t t=0; t<throttle_tests_num; t++) { for (uint8_t t=0; t<throttle_tests_num; t++) {
roll_in = rpy_tests[r]; const float roll_in = rpy_tests[r];
pitch_in = rpy_tests[p]; const float pitch_in = rpy_tests[p];
yaw_in = rpy_tests[y]; const float yaw_in = rpy_tests[y];
throttle_in = throttle_tests[t]/1000.0f; const float throttle_in = throttle_tests[t];
motors.set_roll(roll_in/4500.0f); motors.set_roll(roll_in);
motors.set_pitch(pitch_in/4500.0f); motors.set_pitch(pitch_in);
motors.set_yaw(yaw_in/4500.0f); motors.set_yaw(yaw_in);
motors.set_throttle(throttle_in); motors.set_throttle(throttle_in);
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
update_motors(); 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 // display input and output
#if NUM_OUTPUTS <= 4 hal.console->printf("%0.2f,%0.2f,%0.2f,%0.2f,", roll_in, pitch_in, yaw_in, throttle_in);
hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // quad for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
#elif NUM_OUTPUTS <= 6 hal.console->printf("%d,",(int)hal.rcout->read(i));
hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // hexa }
#else for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // octa hal.console->printf("%0.4f,", motors.get_thrust_rpyt_out(i));
#endif }
(int)roll_in, hal.console->printf("%d,%d,%d,%d,%d\n",
(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,
(int)motors.limit.roll, (int)motors.limit.roll,
(int)motors.limit.pitch, (int)motors.limit.pitch,
(int)motors.limit.yaw, (int)motors.limit.yaw,
@ -176,7 +223,6 @@ void stability_test()
motors.set_throttle(0); motors.set_throttle(0);
motors.armed(false); motors.armed(false);
hal.console->printf("finished test.\n");
} }
void update_motors() void update_motors()