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 2023-06-10 04:31:49 +01:00 committed by Andrew Tridgell
parent 1b3de3acf2
commit 2563edc3c3
2 changed files with 435 additions and 90 deletions

View File

@ -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();
}
}

View File

@ -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()