mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-01 21:48:28 -04:00
AP_Motors: Allow frame class to be a run-time arg to AP_Motors_test and add all heli frame classes to test
AP_Motors: Example: init each frame type correctly and warn for init fail
This commit is contained in:
Gone4Dirt
Andrew Tridgell
parent
1b3de3acf2
commit
2563edc3c3
@ -10,12 +10,9 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
// Libraries
|
// Libraries
|
||||||
#include <AP_Common/AP_Common.h>
|
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
#include <AP_BattMonitor/AP_BattMonitor.h>
|
#include <AP_BattMonitor/AP_BattMonitor.h>
|
||||||
#include <RC_Channel/RC_Channel.h> // RC Channel Library
|
|
||||||
#include <AP_Motors/AP_Motors.h>
|
#include <AP_Motors/AP_Motors.h>
|
||||||
#include <RC_Channel/RC_Channel.h>
|
|
||||||
#include <SRV_Channel/SRV_Channel.h>
|
#include <SRV_Channel/SRV_Channel.h>
|
||||||
#include <AP_ESC_Telem/AP_ESC_Telem.h>
|
#include <AP_ESC_Telem/AP_ESC_Telem.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
@ -30,9 +27,7 @@ void stability_test();
|
|||||||
void update_motors();
|
void update_motors();
|
||||||
void print_all_motor_matrix();
|
void print_all_motor_matrix();
|
||||||
|
|
||||||
#define HELI_TEST 0 // set to 1 to test helicopters
|
// Instantiate a few classes that will be needed so that the singletons can be called from the motors lib
|
||||||
#define NUM_OUTPUTS 4 // set to 6 for hexacopter, 8 for octacopter and heli
|
|
||||||
|
|
||||||
#if HAL_WITH_ESC_TELEM
|
#if HAL_WITH_ESC_TELEM
|
||||||
AP_ESC_Telem esc_telem;
|
AP_ESC_Telem esc_telem;
|
||||||
#endif
|
#endif
|
||||||
@ -40,95 +35,174 @@ AP_ESC_Telem esc_telem;
|
|||||||
#define VERSION "AP_Motors library test ver 1.1"
|
#define VERSION "AP_Motors library test ver 1.1"
|
||||||
|
|
||||||
SRV_Channels srvs;
|
SRV_Channels srvs;
|
||||||
|
|
||||||
// uncomment the row below depending upon what frame you are using
|
|
||||||
//AP_MotorsTri motors(400);
|
|
||||||
AP_MotorsMatrix motors(400);
|
|
||||||
//AP_MotorsHeli_Single motors(400);
|
|
||||||
//AP_MotorsSingle motors(400);
|
|
||||||
//AP_MotorsCoax motors(400);
|
|
||||||
|
|
||||||
AP_BattMonitor _battmonitor{0, nullptr, nullptr};
|
AP_BattMonitor _battmonitor{0, nullptr, nullptr};
|
||||||
|
|
||||||
|
AP_Motors *motors;
|
||||||
|
AP_MotorsMatrix *motors_matrix;
|
||||||
|
|
||||||
bool thrust_boost = false;
|
bool thrust_boost = false;
|
||||||
|
|
||||||
|
uint8_t num_outputs;
|
||||||
|
|
||||||
// setup
|
// setup
|
||||||
void setup()
|
void setup()
|
||||||
{
|
{
|
||||||
|
|
||||||
// motor initialisation
|
// default to quad frame class, frame class can be changed by argument in parser below
|
||||||
motors.set_dt(1.0/400.0);
|
AP_Motors::motor_frame_class frame_class = AP_Motors::MOTOR_FRAME_QUAD;
|
||||||
motors.set_update_rate(490);
|
|
||||||
|
|
||||||
#if HELI_TEST
|
// Parse the command line arguments
|
||||||
motors.init(AP_Motors::MOTOR_FRAME_HELI, AP_Motors::MOTOR_FRAME_TYPE_PLUS);
|
|
||||||
#else
|
|
||||||
#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
|
|
||||||
#endif // HELI_TEST == 0
|
|
||||||
|
|
||||||
#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;
|
uint8_t argc;
|
||||||
char * const *argv;
|
char * const *argv;
|
||||||
hal.util->commandline_arguments(argc, argv);
|
hal.util->commandline_arguments(argc, argv);
|
||||||
if (argc > 1) {
|
if (argc > 1) {
|
||||||
#if HELI_TEST == 0
|
|
||||||
for (uint8_t i = 2; i < argc; i++) {
|
for (uint8_t i = 2; i < argc; i++) {
|
||||||
const char* arg = argv[i];
|
const char *arg = argv[i];
|
||||||
|
|
||||||
const char *eq = strchr(arg, '=');
|
const char *eq = strchr(arg, '=');
|
||||||
|
|
||||||
if (eq == NULL) {
|
if (eq == NULL) {
|
||||||
::printf("Expected argument with \"=\"\n");
|
::printf("Expected argument with \"=\"\n");
|
||||||
exit(1);
|
exit(1);
|
||||||
}
|
}
|
||||||
|
|
||||||
char cmd[20] {};
|
char cmd[20] {};
|
||||||
strncpy(cmd, arg, eq-arg);
|
strncpy(cmd, arg, eq-arg);
|
||||||
const float value = atof(eq+1);
|
const float value = atof(eq+1);
|
||||||
if (strcmp(cmd,"yaw_headroom") == 0) {
|
if (strcmp(cmd,"yaw_headroom") == 0) {
|
||||||
motors.set_yaw_headroom(value);
|
if (motors_matrix != nullptr) {
|
||||||
|
motors_matrix->set_yaw_headroom(value);
|
||||||
|
} else {
|
||||||
|
::printf("frame_class = %d does not accept yaw_headroom commands\n", frame_class);
|
||||||
|
}
|
||||||
|
|
||||||
} else if (strcmp(cmd,"throttle_avg_max") == 0) {
|
} else if (strcmp(cmd,"throttle_avg_max") == 0) {
|
||||||
motors.set_throttle_avg_max(value);
|
if (motors_matrix != nullptr) {
|
||||||
|
motors_matrix->set_throttle_avg_max(value);
|
||||||
|
} else {
|
||||||
|
::printf("frame_class = %d does not accept throttle_avg_max commands\n", frame_class);
|
||||||
|
}
|
||||||
|
|
||||||
} else if (strcmp(cmd,"thrust_boost") == 0) {
|
} else if (strcmp(cmd,"thrust_boost") == 0) {
|
||||||
thrust_boost = value > 0.0;
|
thrust_boost = value > 0.0;
|
||||||
|
|
||||||
|
} else if (strcmp(cmd,"frame_class") == 0) {
|
||||||
|
// We must have the frame_class argument 2nd as resulting class is used to determine if
|
||||||
|
// we have access to certain functions in the multicopter motors child class
|
||||||
|
if (i != 2) {
|
||||||
|
::printf("frame_class must be second argument\n");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Setup the correct motors object for the frame class to test
|
||||||
|
frame_class = (AP_Motors::motor_frame_class)value;
|
||||||
|
|
||||||
|
switch (frame_class) {
|
||||||
|
|
||||||
|
case AP_Motors::MOTOR_FRAME_QUAD:
|
||||||
|
case AP_Motors::MOTOR_FRAME_HEXA:
|
||||||
|
case AP_Motors::MOTOR_FRAME_OCTA:
|
||||||
|
motors_matrix = new AP_MotorsMatrix(400);
|
||||||
|
motors = motors_matrix;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case AP_Motors::MOTOR_FRAME_HELI:
|
||||||
|
motors = new AP_MotorsHeli_Single(400);
|
||||||
|
num_outputs = 4; // Currently only does default config. In future need to test all types: Mot 1-3 swashplate, mot 4 tail rotor pitch, mot 5 for 4th servo in H4-90 swash
|
||||||
|
break;
|
||||||
|
|
||||||
|
case AP_Motors::MOTOR_FRAME_HELI_DUAL:
|
||||||
|
motors = new AP_MotorsHeli_Dual(400);
|
||||||
|
num_outputs = 6; // Currently only does default config. In future need to test all types: Mot 1-3 swashplate 1, mot 4-6 swashplate 2, mot 7 and 8 for 4th servos on H4-90 swash plates front and back, respectively
|
||||||
|
break;
|
||||||
|
|
||||||
|
case AP_Motors::MOTOR_FRAME_HELI_QUAD:
|
||||||
|
motors = new AP_MotorsHeli_Quad(400);
|
||||||
|
num_outputs = 4; // Only 4 collective servos
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
::printf("ERROR: frame_class=%d not implemented\n", frame_class);
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Init motors
|
||||||
|
motors->init(frame_class, AP_Motors::MOTOR_FRAME_TYPE_X);
|
||||||
|
|
||||||
|
// Get the numper of outputs for a regular copter dynamically. We can't currently do this for heli because the motor mask doesn't tell us the write thing
|
||||||
|
if (motors_matrix != nullptr) {
|
||||||
|
num_outputs = __builtin_popcount(motors->get_motor_mask());
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check that init worked
|
||||||
|
if (!motors->initialised_ok()) {
|
||||||
|
::printf("ERROR: frame_class=%d initialisation failed\n", frame_class);
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
::printf("Expected \"yaw_headroom\" or \"throttle_avg_max\"\n");
|
::printf("Expected \"frame_class\", \"yaw_headroom\" or \"throttle_avg_max\"\n");
|
||||||
exit(1);
|
exit(1);
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
// if we haven't been given a frame class by argument we just assume a quad by default
|
||||||
|
// so that the single first argument s or t still works
|
||||||
|
if (motors == nullptr) {
|
||||||
|
motors_matrix = new AP_MotorsMatrix(400);
|
||||||
|
motors = motors_matrix;
|
||||||
|
motors->init(AP_Motors::MOTOR_FRAME_QUAD, AP_Motors::MOTOR_FRAME_TYPE_X);
|
||||||
|
num_outputs = 4;
|
||||||
|
}
|
||||||
|
|
||||||
|
// motor initialisation
|
||||||
|
motors->set_dt(1.0/400.0);
|
||||||
|
motors->set_update_rate(490);
|
||||||
|
motors->output_min();
|
||||||
|
|
||||||
|
// Start a test type based on the input argument
|
||||||
if (strcmp(argv[1],"t") == 0) {
|
if (strcmp(argv[1],"t") == 0) {
|
||||||
motor_order_test();
|
motor_order_test();
|
||||||
|
|
||||||
} else if (strcmp(argv[1],"s") == 0) {
|
} else if (strcmp(argv[1],"s") == 0) {
|
||||||
stability_test();
|
stability_test();
|
||||||
|
|
||||||
#if HELI_TEST == 0
|
|
||||||
} else if (strcmp(argv[1],"p") == 0) {
|
} else if (strcmp(argv[1],"p") == 0) {
|
||||||
|
if (motors_matrix == nullptr) {
|
||||||
|
::printf("Print only supports motors matrix\n");
|
||||||
|
}
|
||||||
print_all_motor_matrix();
|
print_all_motor_matrix();
|
||||||
#endif
|
|
||||||
} else {
|
} else {
|
||||||
::printf("Expected first argument: 't', 's' or 'p'\n");
|
::printf("Expected first argument: 't', 's' or 'p'\n");
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
hal.scheduler->delay(1000);
|
hal.scheduler->delay(1000);
|
||||||
exit(0);
|
exit(0);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
// We haven't been given a frame class so we just assume a quad frame as default
|
||||||
|
motors_matrix = new AP_MotorsMatrix(400);
|
||||||
|
motors = motors_matrix;
|
||||||
|
motors->init(AP_Motors::MOTOR_FRAME_QUAD, AP_Motors::MOTOR_FRAME_TYPE_X);
|
||||||
|
num_outputs = 4;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// motor initialisation
|
||||||
|
motors->set_dt(1.0/400.0);
|
||||||
|
motors->set_update_rate(490);
|
||||||
|
|
||||||
|
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 (motors_matrix != nullptr) {
|
||||||
|
motors_matrix->update_throttle_range();
|
||||||
|
motors_matrix->set_throttle_avg_max(0.5f);
|
||||||
|
}
|
||||||
|
|
||||||
|
motors->output_min();
|
||||||
|
|
||||||
hal.scheduler->delay(1000);
|
hal.scheduler->delay(1000);
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -176,8 +250,8 @@ void print_all_motor_matrix()
|
|||||||
// Skip the none planar motors types
|
// Skip the none planar motors types
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
motors.init((AP_Motors::motor_frame_class)frame_class, (AP_Motors::motor_frame_type)frame_type);
|
motors_matrix->init((AP_Motors::motor_frame_class)frame_class, (AP_Motors::motor_frame_type)frame_type);
|
||||||
if (motors.initialised_ok()) {
|
if (motors_matrix->initialised_ok()) {
|
||||||
if (!first_layout) {
|
if (!first_layout) {
|
||||||
hal.console->printf(",\n");
|
hal.console->printf(",\n");
|
||||||
}
|
}
|
||||||
@ -185,7 +259,7 @@ void print_all_motor_matrix()
|
|||||||
|
|
||||||
// Grab full frame string and strip "Frame: " and split
|
// Grab full frame string and strip "Frame: " and split
|
||||||
// This is the long way round, motors does have direct getters, but there protected
|
// This is the long way round, motors does have direct getters, but there protected
|
||||||
motors.get_frame_and_type_string(frame_and_type_string, ARRAY_SIZE(frame_and_type_string));
|
motors_matrix->get_frame_and_type_string(frame_and_type_string, ARRAY_SIZE(frame_and_type_string));
|
||||||
char *frame_string = strchr(frame_and_type_string, ':');
|
char *frame_string = strchr(frame_and_type_string, ':');
|
||||||
char *type_string = strchr(frame_and_type_string, '/');
|
char *type_string = strchr(frame_and_type_string, '/');
|
||||||
if (type_string != nullptr) {
|
if (type_string != nullptr) {
|
||||||
@ -202,7 +276,7 @@ void print_all_motor_matrix()
|
|||||||
for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
|
for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
|
||||||
float roll, pitch, yaw, throttle;
|
float roll, pitch, yaw, throttle;
|
||||||
uint8_t testing_order;
|
uint8_t testing_order;
|
||||||
if (motors.get_factors(i, roll, pitch, yaw, throttle, testing_order)) {
|
if (motors_matrix->get_factors(i, roll, pitch, yaw, throttle, testing_order)) {
|
||||||
if (!first_motor) {
|
if (!first_motor) {
|
||||||
hal.console->printf(",\n");
|
hal.console->printf(",\n");
|
||||||
}
|
}
|
||||||
@ -240,15 +314,15 @@ void print_all_motor_matrix()
|
|||||||
void motor_order_test()
|
void motor_order_test()
|
||||||
{
|
{
|
||||||
hal.console->printf("testing motor order\n");
|
hal.console->printf("testing motor order\n");
|
||||||
motors.armed(true);
|
motors->armed(true);
|
||||||
for (int8_t i=1; i <= AP_MOTORS_MAX_NUM_MOTORS; i++) {
|
for (int8_t i=1; i <= num_outputs; i++) {
|
||||||
hal.console->printf("Motor %d\n",(int)i);
|
hal.console->printf("Motor %d\n",(int)i);
|
||||||
motors.output_test_seq(i, 1150);
|
motors->output_test_seq(i, 1150);
|
||||||
hal.scheduler->delay(300);
|
hal.scheduler->delay(300);
|
||||||
motors.output_test_seq(i, 1000);
|
motors->output_test_seq(i, 1000);
|
||||||
hal.scheduler->delay(2000);
|
hal.scheduler->delay(2000);
|
||||||
}
|
}
|
||||||
motors.armed(false);
|
motors->armed(false);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -257,13 +331,14 @@ void stability_test()
|
|||||||
{
|
{
|
||||||
hal.console->printf("%s\n", VERSION);
|
hal.console->printf("%s\n", VERSION);
|
||||||
char frame_and_type_string[30];
|
char frame_and_type_string[30];
|
||||||
motors.get_frame_and_type_string(frame_and_type_string, ARRAY_SIZE(frame_and_type_string));
|
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);
|
hal.console->printf("%s\n", frame_and_type_string);
|
||||||
#if HELI_TEST == 0
|
|
||||||
hal.console->printf("Throttle average max: %0.4f\n", motors.get_throttle_avg_max());
|
if (motors_matrix != nullptr) {
|
||||||
hal.console->printf("Yaw headroom: %i\n", motors.get_yaw_headroom());
|
hal.console->printf("Throttle average max: %0.4f\n", motors_matrix->get_throttle_avg_max());
|
||||||
hal.console->printf("Thrust boost: %s\n", thrust_boost?"True":"False");
|
hal.console->printf("Yaw headroom: %i\n", motors_matrix->get_yaw_headroom());
|
||||||
#endif
|
hal.console->printf("Thrust boost: %s\n", thrust_boost?"True":"False");
|
||||||
|
}
|
||||||
|
|
||||||
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 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 uint8_t throttle_tests_num = ARRAY_SIZE(throttle_tests);
|
||||||
@ -271,19 +346,21 @@ void stability_test()
|
|||||||
const uint8_t rpy_tests_num = ARRAY_SIZE(rpy_tests);
|
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();
|
||||||
|
|
||||||
hal.console->printf("Roll,Pitch,Yaw,Thr,");
|
hal.console->printf("Roll,Pitch,Yaw,Thr,");
|
||||||
for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
|
for (uint8_t i=0; i<num_outputs; i++) {
|
||||||
hal.console->printf("Mot%i,",i+1);
|
hal.console->printf("Mot%i,",i+1);
|
||||||
}
|
}
|
||||||
#if HELI_TEST == 0
|
|
||||||
for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
|
if (motors_matrix != nullptr) {
|
||||||
hal.console->printf("Mot%i_norm,",i+1);
|
for (uint8_t i=0; i<num_outputs; i++) {
|
||||||
|
hal.console->printf("Mot%i_norm,",i+1);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
hal.console->printf("LimR,LimP,LimY,LimThD,LimThU\n");
|
hal.console->printf("LimR,LimP,LimY,LimThD,LimThU\n");
|
||||||
|
|
||||||
// run stability test
|
// run stability test
|
||||||
@ -295,40 +372,43 @@ void stability_test()
|
|||||||
const float pitch_in = rpy_tests[p];
|
const float pitch_in = rpy_tests[p];
|
||||||
const float yaw_in = rpy_tests[y];
|
const float yaw_in = rpy_tests[y];
|
||||||
const float throttle_in = throttle_tests[t];
|
const float throttle_in = throttle_tests[t];
|
||||||
motors.set_roll(roll_in);
|
motors->set_roll(roll_in);
|
||||||
motors.set_pitch(pitch_in);
|
motors->set_pitch(pitch_in);
|
||||||
motors.set_yaw(yaw_in);
|
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();
|
||||||
SRV_Channels::calc_pwm();
|
SRV_Channels::calc_pwm();
|
||||||
|
SRV_Channels::output_ch_all();
|
||||||
// display input and output
|
// display input and output
|
||||||
hal.console->printf("%0.2f,%0.2f,%0.2f,%0.2f,", roll_in, pitch_in, yaw_in, throttle_in);
|
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++) {
|
for (uint8_t i=0; i<num_outputs; i++) {
|
||||||
hal.console->printf("%d,",(int)hal.rcout->read(i));
|
hal.console->printf("%d,",(int)hal.rcout->read(i));
|
||||||
}
|
}
|
||||||
#if HELI_TEST == 0
|
|
||||||
for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
|
if (motors_matrix != nullptr) {
|
||||||
hal.console->printf("%0.4f,", motors.get_thrust_rpyt_out(i));
|
for (uint8_t i=0; i<num_outputs; i++) {
|
||||||
|
hal.console->printf("%0.4f,", motors_matrix->get_thrust_rpyt_out(i));
|
||||||
|
}
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
hal.console->printf("%d,%d,%d,%d,%d\n",
|
hal.console->printf("%d,%d,%d,%d,%d\n",
|
||||||
(int)motors.limit.roll,
|
(int)motors->limit.roll,
|
||||||
(int)motors.limit.pitch,
|
(int)motors->limit.pitch,
|
||||||
(int)motors.limit.yaw,
|
(int)motors->limit.yaw,
|
||||||
(int)motors.limit.throttle_lower,
|
(int)motors->limit.throttle_lower,
|
||||||
(int)motors.limit.throttle_upper);
|
(int)motors->limit.throttle_upper);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// set all inputs to motor library to zero and disarm motors
|
// set all inputs to motor library to zero and disarm motors
|
||||||
motors.set_pitch(0);
|
motors->set_pitch(0);
|
||||||
motors.set_roll(0);
|
motors->set_roll(0);
|
||||||
motors.set_yaw(0);
|
motors->set_yaw(0);
|
||||||
motors.set_throttle(0);
|
motors->set_throttle(0);
|
||||||
motors.armed(false);
|
motors->armed(false);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -336,8 +416,8 @@ void update_motors()
|
|||||||
{
|
{
|
||||||
// call update motors 1000 times to get any ramp limiting complete
|
// call update motors 1000 times to get any ramp limiting complete
|
||||||
for (uint16_t i=0; i<1000; i++) {
|
for (uint16_t i=0; i<1000; i++) {
|
||||||
motors.set_thrust_boost(thrust_boost);
|
motors->set_thrust_boost(thrust_boost);
|
||||||
motors.output();
|
motors->output();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -0,0 +1,265 @@
|
|||||||
|
# Script that automatically runs multipoint output comparison for all AP_Motors_Heli frames
|
||||||
|
# To prove equivalence when doing restructuring PR's
|
||||||
|
#
|
||||||
|
# Run from "ardupilot" directory
|
||||||
|
# Run the command below. The first argument is the number of commits to rewind the HEAD to get the "old"
|
||||||
|
# comparison point. This should rewind back past all of the commits you have added
|
||||||
|
#
|
||||||
|
# If you just want to run a specific frame class, (e.g. dual heli = 11) then add the -f argument giving the
|
||||||
|
# frame class number (e.g. -f 11) to only run that frame class. Default is to run all heli frame classes
|
||||||
|
#
|
||||||
|
# command to run:
|
||||||
|
# ---------------
|
||||||
|
# python3 libraries/AP_Motors/examples/AP_Motors_test/run_heli_comparison.py <INESRT No COMMITS TO REWIND>
|
||||||
|
#
|
||||||
|
# You may have to run "./waf distclean" if failing to build
|
||||||
|
|
||||||
|
import os
|
||||||
|
import subprocess
|
||||||
|
import shutil
|
||||||
|
import csv
|
||||||
|
from matplotlib import pyplot as plt
|
||||||
|
from argparse import ArgumentParser
|
||||||
|
import time
|
||||||
|
|
||||||
|
# ==============================================================================
|
||||||
|
class DataPoints:
|
||||||
|
|
||||||
|
HEADER_LINE = 3
|
||||||
|
|
||||||
|
# --------------------------------------------------------------------------
|
||||||
|
# Instantiate the object and parse the data from the provided file
|
||||||
|
# file: path to the csv file to be parsed
|
||||||
|
def __init__(self, file):
|
||||||
|
self.data = {}
|
||||||
|
self.limit_case = []
|
||||||
|
self.init_failed = False
|
||||||
|
|
||||||
|
with open(file, 'r') as csvfile:
|
||||||
|
# creating a csv reader object
|
||||||
|
csvreader = csv.reader(csvfile)
|
||||||
|
|
||||||
|
# extracting field names through first row
|
||||||
|
line_num = 0
|
||||||
|
for row in csvreader:
|
||||||
|
line_num += 1
|
||||||
|
|
||||||
|
if line_num < self.HEADER_LINE:
|
||||||
|
# Warn the user if the an init failed message has been found in the file
|
||||||
|
if 'initialisation failed' in row[0]:
|
||||||
|
print('\n%s\n' % row[0])
|
||||||
|
self.init_failed = True
|
||||||
|
continue
|
||||||
|
|
||||||
|
elif line_num == self.HEADER_LINE:
|
||||||
|
# init all dict entries based on the header entries
|
||||||
|
for field in row:
|
||||||
|
self.data[field] = []
|
||||||
|
|
||||||
|
else:
|
||||||
|
# stow all of the data
|
||||||
|
case_is_limited = False
|
||||||
|
for field, data in zip(self.data.keys(), row):
|
||||||
|
self.data[field].append(float(data))
|
||||||
|
|
||||||
|
# Keep track of all cases where a limit flag is set
|
||||||
|
if ('lim' in field.lower()) and (float(data) > 0.5):
|
||||||
|
case_is_limited = True
|
||||||
|
self.limit_case.append(case_is_limited)
|
||||||
|
|
||||||
|
# Make data immutable
|
||||||
|
for field in self.data.keys():
|
||||||
|
self.data[field] = tuple(self.data[field])
|
||||||
|
# --------------------------------------------------------------------------
|
||||||
|
|
||||||
|
# --------------------------------------------------------------------------
|
||||||
|
# get the data from a given field
|
||||||
|
# field: dict index, name of field data to be returned
|
||||||
|
# lim_tf: limit bool, return limit cases or not
|
||||||
|
def get_data(self, field, lim_tf):
|
||||||
|
if field not in self.data.keys():
|
||||||
|
raise Exception('%s is not a valid data field' % field)
|
||||||
|
|
||||||
|
ret = []
|
||||||
|
for data, flag in zip(self.data[field], self.limit_case):
|
||||||
|
if (flag == lim_tf):
|
||||||
|
ret.append(data)
|
||||||
|
return ret
|
||||||
|
# --------------------------------------------------------------------------
|
||||||
|
|
||||||
|
# --------------------------------------------------------------------------
|
||||||
|
def get_fields(self):
|
||||||
|
return self.data.keys()
|
||||||
|
# --------------------------------------------------------------------------
|
||||||
|
|
||||||
|
# ==============================================================================
|
||||||
|
|
||||||
|
frame_class_lookup = {6:'Single_Heli', 11:'Dual_Heli', 13:'Heli_Quad'}
|
||||||
|
|
||||||
|
|
||||||
|
# ==============================================================================
|
||||||
|
if __name__ == '__main__':
|
||||||
|
|
||||||
|
BLUE = [0,0,1]
|
||||||
|
RED = [1,0,0]
|
||||||
|
BLACK = [0,0,0]
|
||||||
|
|
||||||
|
dir_name = 'motors_comparison'
|
||||||
|
|
||||||
|
# Build input parser
|
||||||
|
parser = ArgumentParser(description='Find logs in which the input string is found in messages')
|
||||||
|
parser.add_argument("head", type=int, help='number of commits to roll back the head for comparing the work done')
|
||||||
|
parser.add_argument("-f","--frame-class", type=int, dest='frame_class', nargs="+", default=(6,11,13), help="list of frame classes to run comparison on. Defaults to 6 single heli.")
|
||||||
|
args = parser.parse_args()
|
||||||
|
|
||||||
|
# Warn the user that we are about to move things around with git.
|
||||||
|
response = input("WARNING: this tool uses git to checkout older commits. It is safest to do this in a separate test branch.\nDo you wish to continue y/n?:[n]")
|
||||||
|
if response.lower() != 'y':
|
||||||
|
quit()
|
||||||
|
|
||||||
|
if not args.head:
|
||||||
|
print('Number of commits to roll back HEAD must be provided to run comparison. Add --help for more info.')
|
||||||
|
quit()
|
||||||
|
if args.head <= 0:
|
||||||
|
print('Number of commits to roll back HEAD must be a positive integer value')
|
||||||
|
quit()
|
||||||
|
|
||||||
|
# If we have already run this test, delete the old data
|
||||||
|
if dir_name in os.listdir('./'):
|
||||||
|
shutil.rmtree(dir_name)
|
||||||
|
|
||||||
|
# Create the new directory
|
||||||
|
os.mkdir(dir_name)
|
||||||
|
|
||||||
|
# configure and build the test
|
||||||
|
os.system('./waf configure --board linux')
|
||||||
|
os.system('./waf build --target examples/AP_Motors_test')
|
||||||
|
|
||||||
|
print('\nRunning motor tests with current changes\n')
|
||||||
|
|
||||||
|
# run the test
|
||||||
|
for fc in args.frame_class:
|
||||||
|
filename = 'new_%s_motor_test.csv' % frame_class_lookup[fc]
|
||||||
|
os.system('./build/linux/examples/AP_Motors_test s > %s frame_class=%d' % (filename,fc))
|
||||||
|
|
||||||
|
# move the csv to the directory for later comparison
|
||||||
|
shutil.move(filename, os.path.join(dir_name, filename))
|
||||||
|
|
||||||
|
print('Frame class = %s complete\n' % frame_class_lookup[fc])
|
||||||
|
|
||||||
|
# Rewind the HEAD by the requested number of commits
|
||||||
|
cmd = 'git log -%d --format=format:"%%H"' % (args.head+1)
|
||||||
|
result = subprocess.run([cmd], shell=True, capture_output=True, text=True)
|
||||||
|
git_history = result.stdout.split('\n')
|
||||||
|
latest_commit = git_history[0]
|
||||||
|
base_commit = git_history[-1]
|
||||||
|
|
||||||
|
print('Rewinding head by %d commits to: %s\n' % (args.head, base_commit))
|
||||||
|
|
||||||
|
# Checkout to a detached head to test the old code before improvements
|
||||||
|
cmd = 'git checkout %s' % base_commit
|
||||||
|
result = subprocess.run([cmd], shell=True, capture_output=True, text=True)
|
||||||
|
print('\n%s\n' % cmd)
|
||||||
|
|
||||||
|
if result.returncode > 0:
|
||||||
|
print('ERROR: Could not rewind HEAD. Exited with error:\n%s\n%s' % (result.stdout, result.stderr))
|
||||||
|
quit()
|
||||||
|
|
||||||
|
# Rebuild
|
||||||
|
os.system('./waf clean')
|
||||||
|
os.system('./waf build --target examples/AP_Motors_test')
|
||||||
|
|
||||||
|
# Run motors test for "old" comparison point
|
||||||
|
for fc in args.frame_class:
|
||||||
|
print('Running motors test for frame class = %s complete\n' % frame_class_lookup[fc])
|
||||||
|
|
||||||
|
filename = 'original_%s_motor_test.csv' % frame_class_lookup[fc]
|
||||||
|
os.system('./build/linux/examples/AP_Motors_test s > %s frame_class=%d' % (filename,fc))
|
||||||
|
|
||||||
|
# move the csv to the directory for later comparison
|
||||||
|
shutil.move(filename, os.path.join(dir_name, filename))
|
||||||
|
|
||||||
|
print('Frame class = %s, test complete\n' % frame_class_lookup[fc])
|
||||||
|
|
||||||
|
# Move back to active branch
|
||||||
|
print('Returning to original branch, commit = %s\n' % latest_commit)
|
||||||
|
cmd = 'git switch -'
|
||||||
|
result = subprocess.run([cmd], shell=True, capture_output=True, text=True)
|
||||||
|
print('\n%s\n' % cmd)
|
||||||
|
|
||||||
|
if result.returncode > 0:
|
||||||
|
print('WARNING: Could not return head to branch with commit %s. \nError messages:\n%s\n%s' % (latest_commit, result.stdout, result.stderr))
|
||||||
|
|
||||||
|
new_points = {}
|
||||||
|
old_points = {}
|
||||||
|
for fc in args.frame_class:
|
||||||
|
filename = '%s_motor_test.csv' % frame_class_lookup[fc]
|
||||||
|
new_points[frame_class_lookup[fc]] = DataPoints(os.path.join(dir_name, 'new_%s' % filename))
|
||||||
|
old_points[frame_class_lookup[fc]] = DataPoints(os.path.join(dir_name, 'original_%s' % filename))
|
||||||
|
|
||||||
|
# Plot all of the points for correlation comparison
|
||||||
|
for fc in args.frame_class:
|
||||||
|
frame = frame_class_lookup[fc]
|
||||||
|
fig_size = (16, 8)
|
||||||
|
|
||||||
|
# Ensure we didn't get an init fail before proceeding
|
||||||
|
if new_points[frame].init_failed:
|
||||||
|
# Create plot explaining issue to user, as the earlier cmd line warning may have been lost
|
||||||
|
fig, ax = plt.subplots(1, 1, figsize=fig_size)
|
||||||
|
fig.suptitle('%s: INIT FAILED' % frame, fontsize=16)
|
||||||
|
continue
|
||||||
|
|
||||||
|
# Plot inputs
|
||||||
|
fig, ax = plt.subplots(2, 2, figsize=fig_size)
|
||||||
|
fig.suptitle('%s Input Diff' % frame, fontsize=16)
|
||||||
|
ax = ax.flatten()
|
||||||
|
|
||||||
|
plot_index = 0
|
||||||
|
for field in ['Roll','Pitch','Yaw','Thr']:
|
||||||
|
diff = [i-j for i,j in zip(old_points[frame].data[field], new_points[frame].data[field])]
|
||||||
|
ax[plot_index].plot(diff, color=RED)
|
||||||
|
ax[plot_index].set_xlabel('Test Number')
|
||||||
|
ax[plot_index].set_ylabel('%s Old - New' % field)
|
||||||
|
plot_index += 1
|
||||||
|
plt.tight_layout(rect=[0, 0.0, 1, 0.95])
|
||||||
|
|
||||||
|
# Find number of motors
|
||||||
|
num_motors = 0
|
||||||
|
while True:
|
||||||
|
num_motors += 1
|
||||||
|
if 'Mot%i' % (num_motors+1) not in new_points[frame].get_fields():
|
||||||
|
break
|
||||||
|
|
||||||
|
# Plot outputs
|
||||||
|
fig, ax = plt.subplots(2, num_motors, figsize=fig_size)
|
||||||
|
fig.suptitle('%s Outputs' % frame, fontsize=16)
|
||||||
|
for i in range(num_motors):
|
||||||
|
field = 'Mot%i' % (i+1)
|
||||||
|
ax[0,i].plot(old_points[frame].data[field], color=RED)
|
||||||
|
ax[0,i].plot(new_points[frame].data[field], color=BLUE)
|
||||||
|
ax[0,i].set_ylabel(field)
|
||||||
|
ax[0,i].set_xlabel('Test No')
|
||||||
|
|
||||||
|
diff = [i-j for i,j in zip(old_points[frame].data[field], new_points[frame].data[field])]
|
||||||
|
ax[1,i].plot(diff, color=BLACK)
|
||||||
|
ax[1,i].set_ylabel('Change in %s' % field)
|
||||||
|
ax[1,i].set_xlabel('Test No')
|
||||||
|
plt.tight_layout(rect=[0, 0.0, 1, 0.95])
|
||||||
|
|
||||||
|
# Plot limits
|
||||||
|
fig, ax = plt.subplots(2, 5, figsize=fig_size)
|
||||||
|
fig.suptitle(frame + ' Limits', fontsize=16)
|
||||||
|
for i, field in enumerate(['LimR','LimP','LimY','LimThD','LimThU']):
|
||||||
|
ax[0,i].plot(old_points[frame].data[field], color=RED)
|
||||||
|
ax[0,i].plot(new_points[frame].data[field], color=BLUE)
|
||||||
|
ax[0,i].set_ylabel(field)
|
||||||
|
ax[0,i].set_xlabel('Test No')
|
||||||
|
|
||||||
|
diff = [i-j for i,j in zip(old_points[frame].data[field], new_points[frame].data[field])]
|
||||||
|
ax[1,i].plot(diff, color=BLACK)
|
||||||
|
ax[1,i].set_ylabel('Change in %s' % field)
|
||||||
|
ax[1,i].set_xlabel('Test No')
|
||||||
|
plt.tight_layout(rect=[0, 0.0, 1, 0.95])
|
||||||
|
|
||||||
|
print('*** Complete ***')
|
||||||
|
plt.show()
|
||||||
Loading…
Reference in New Issue
Block a user