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:
parent
1b3de3acf2
commit
2563edc3c3
@ -10,12 +10,9 @@
|
||||
*/
|
||||
|
||||
// Libraries
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_BattMonitor/AP_BattMonitor.h>
|
||||
#include <RC_Channel/RC_Channel.h> // RC Channel Library
|
||||
#include <AP_Motors/AP_Motors.h>
|
||||
#include <RC_Channel/RC_Channel.h>
|
||||
#include <SRV_Channel/SRV_Channel.h>
|
||||
#include <AP_ESC_Telem/AP_ESC_Telem.h>
|
||||
#include <stdio.h>
|
||||
@ -30,9 +27,7 @@ void stability_test();
|
||||
void update_motors();
|
||||
void print_all_motor_matrix();
|
||||
|
||||
#define HELI_TEST 0 // set to 1 to test helicopters
|
||||
#define NUM_OUTPUTS 4 // set to 6 for hexacopter, 8 for octacopter and heli
|
||||
|
||||
// Instantiate a few classes that will be needed so that the singletons can be called from the motors lib
|
||||
#if HAL_WITH_ESC_TELEM
|
||||
AP_ESC_Telem esc_telem;
|
||||
#endif
|
||||
@ -40,95 +35,174 @@ AP_ESC_Telem esc_telem;
|
||||
#define VERSION "AP_Motors library test ver 1.1"
|
||||
|
||||
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_Motors *motors;
|
||||
AP_MotorsMatrix *motors_matrix;
|
||||
|
||||
bool thrust_boost = false;
|
||||
|
||||
uint8_t num_outputs;
|
||||
|
||||
// setup
|
||||
void setup()
|
||||
{
|
||||
|
||||
// motor initialisation
|
||||
motors.set_dt(1.0/400.0);
|
||||
motors.set_update_rate(490);
|
||||
// default to quad frame class, frame class can be changed by argument in parser below
|
||||
AP_Motors::motor_frame_class frame_class = AP_Motors::MOTOR_FRAME_QUAD;
|
||||
|
||||
#if HELI_TEST
|
||||
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
|
||||
// Parse the command line arguments
|
||||
uint8_t argc;
|
||||
char * const *argv;
|
||||
hal.util->commandline_arguments(argc, argv);
|
||||
if (argc > 1) {
|
||||
#if HELI_TEST == 0
|
||||
for (uint8_t i = 2; i < argc; i++) {
|
||||
const char* arg = argv[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);
|
||||
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) {
|
||||
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) {
|
||||
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 {
|
||||
::printf("Expected \"yaw_headroom\" or \"throttle_avg_max\"\n");
|
||||
::printf("Expected \"frame_class\", \"yaw_headroom\" or \"throttle_avg_max\"\n");
|
||||
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) {
|
||||
motor_order_test();
|
||||
|
||||
} else if (strcmp(argv[1],"s") == 0) {
|
||||
stability_test();
|
||||
|
||||
#if HELI_TEST == 0
|
||||
} else if (strcmp(argv[1],"p") == 0) {
|
||||
if (motors_matrix == nullptr) {
|
||||
::printf("Print only supports motors matrix\n");
|
||||
}
|
||||
print_all_motor_matrix();
|
||||
#endif
|
||||
|
||||
} else {
|
||||
::printf("Expected first argument: 't', 's' or 'p'\n");
|
||||
|
||||
}
|
||||
|
||||
hal.scheduler->delay(1000);
|
||||
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);
|
||||
}
|
||||
|
||||
@ -176,8 +250,8 @@ void print_all_motor_matrix()
|
||||
// Skip the none planar motors types
|
||||
continue;
|
||||
}
|
||||
motors.init((AP_Motors::motor_frame_class)frame_class, (AP_Motors::motor_frame_type)frame_type);
|
||||
if (motors.initialised_ok()) {
|
||||
motors_matrix->init((AP_Motors::motor_frame_class)frame_class, (AP_Motors::motor_frame_type)frame_type);
|
||||
if (motors_matrix->initialised_ok()) {
|
||||
if (!first_layout) {
|
||||
hal.console->printf(",\n");
|
||||
}
|
||||
@ -185,7 +259,7 @@ void print_all_motor_matrix()
|
||||
|
||||
// Grab full frame string and strip "Frame: " and split
|
||||
// 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 *type_string = strchr(frame_and_type_string, '/');
|
||||
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++) {
|
||||
float roll, pitch, yaw, throttle;
|
||||
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) {
|
||||
hal.console->printf(",\n");
|
||||
}
|
||||
@ -240,15 +314,15 @@ void print_all_motor_matrix()
|
||||
void motor_order_test()
|
||||
{
|
||||
hal.console->printf("testing motor order\n");
|
||||
motors.armed(true);
|
||||
for (int8_t i=1; i <= AP_MOTORS_MAX_NUM_MOTORS; i++) {
|
||||
motors->armed(true);
|
||||
for (int8_t i=1; i <= num_outputs; 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);
|
||||
motors.output_test_seq(i, 1000);
|
||||
motors->output_test_seq(i, 1000);
|
||||
hal.scheduler->delay(2000);
|
||||
}
|
||||
motors.armed(false);
|
||||
motors->armed(false);
|
||||
|
||||
}
|
||||
|
||||
@ -257,13 +331,14 @@ void stability_test()
|
||||
{
|
||||
hal.console->printf("%s\n", VERSION);
|
||||
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);
|
||||
#if HELI_TEST == 0
|
||||
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());
|
||||
hal.console->printf("Thrust boost: %s\n", thrust_boost?"True":"False");
|
||||
#endif
|
||||
|
||||
if (motors_matrix != nullptr) {
|
||||
hal.console->printf("Throttle average max: %0.4f\n", motors_matrix->get_throttle_avg_max());
|
||||
hal.console->printf("Yaw headroom: %i\n", motors_matrix->get_yaw_headroom());
|
||||
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 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);
|
||||
|
||||
// arm motors
|
||||
motors.armed(true);
|
||||
motors.set_interlock(true);
|
||||
motors->armed(true);
|
||||
motors->set_interlock(true);
|
||||
SRV_Channels::enable_aux_servos();
|
||||
|
||||
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);
|
||||
}
|
||||
#if HELI_TEST == 0
|
||||
for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
|
||||
hal.console->printf("Mot%i_norm,",i+1);
|
||||
|
||||
if (motors_matrix != nullptr) {
|
||||
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");
|
||||
|
||||
// run stability test
|
||||
@ -295,40 +372,43 @@ void stability_test()
|
||||
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);
|
||||
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();
|
||||
SRV_Channels::calc_pwm();
|
||||
SRV_Channels::output_ch_all();
|
||||
// display input and output
|
||||
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));
|
||||
}
|
||||
#if HELI_TEST == 0
|
||||
for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
|
||||
hal.console->printf("%0.4f,", motors.get_thrust_rpyt_out(i));
|
||||
|
||||
if (motors_matrix != nullptr) {
|
||||
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",
|
||||
(int)motors.limit.roll,
|
||||
(int)motors.limit.pitch,
|
||||
(int)motors.limit.yaw,
|
||||
(int)motors.limit.throttle_lower,
|
||||
(int)motors.limit.throttle_upper);
|
||||
(int)motors->limit.roll,
|
||||
(int)motors->limit.pitch,
|
||||
(int)motors->limit.yaw,
|
||||
(int)motors->limit.throttle_lower,
|
||||
(int)motors->limit.throttle_upper);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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);
|
||||
motors->set_pitch(0);
|
||||
motors->set_roll(0);
|
||||
motors->set_yaw(0);
|
||||
motors->set_throttle(0);
|
||||
motors->armed(false);
|
||||
|
||||
}
|
||||
|
||||
@ -336,8 +416,8 @@ void update_motors()
|
||||
{
|
||||
// call update motors 1000 times to get any ramp limiting complete
|
||||
for (uint16_t i=0; i<1000; i++) {
|
||||
motors.set_thrust_boost(thrust_boost);
|
||||
motors.output();
|
||||
motors->set_thrust_boost(thrust_boost);
|
||||
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