mirror of https://github.com/ArduPilot/ardupilot
AP_Motors: example: heli comparison: update functionality and support setting swash type
This commit is contained in:
Iampete1
Randy Mackay
parent
c5c26e89da
commit
b175e7a2f9
|
|
@ -25,7 +25,6 @@ import time
|
|||
# ==============================================================================
|
||||
class DataPoints:
|
||||
|
||||
HEADER_LINE = 3
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# Instantiate the object and parse the data from the provided file
|
||||
|
|
@ -34,27 +33,26 @@ class DataPoints:
|
|||
self.data = {}
|
||||
self.limit_case = []
|
||||
self.init_failed = False
|
||||
self.seen_header = 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:
|
||||
if not self.seen_header:
|
||||
# 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
|
||||
break
|
||||
|
||||
elif line_num == self.HEADER_LINE:
|
||||
# init all dict entries based on the header entries
|
||||
for field in row:
|
||||
self.data[field] = []
|
||||
if (row[0] == 'Roll') and (row[1] == 'Pitch') and (row[2] == 'Yaw') and (row[3] == 'Thr'):
|
||||
self.seen_header = True
|
||||
for field in row:
|
||||
self.data[field] = []
|
||||
|
||||
else:
|
||||
# stow all of the data
|
||||
|
|
@ -67,6 +65,9 @@ class DataPoints:
|
|||
case_is_limited = True
|
||||
self.limit_case.append(case_is_limited)
|
||||
|
||||
if not self.seen_header:
|
||||
self.init_failed = True
|
||||
|
||||
# Make data immutable
|
||||
for field in self.data.keys():
|
||||
self.data[field] = tuple(self.data[field])
|
||||
|
|
@ -94,7 +95,31 @@ class DataPoints:
|
|||
|
||||
# ==============================================================================
|
||||
|
||||
frame_class_lookup = {6:'Single_Heli', 11:'Dual_Heli', 13:'Heli_Quad'}
|
||||
frame_class_lookup = {6:'Single_Heli', 11:'Dual_Heli'}
|
||||
|
||||
swash_type_lookup = {0:'H3',
|
||||
1:'H1',
|
||||
2:'H3_140',
|
||||
3:'H3_120',
|
||||
4:'H4_90',
|
||||
5:'H4_45',}
|
||||
|
||||
# Run sweep over range of types
|
||||
def run_sweep(frame_class, swash_type, dir_name):
|
||||
|
||||
# configure and build the test
|
||||
os.system('./waf configure --board linux')
|
||||
os.system('./waf build --target examples/AP_Motors_test')
|
||||
|
||||
# Run sweep
|
||||
for fc in frame_class:
|
||||
for swash in swash_type:
|
||||
print('Running motors test for frame class = %s (%i), swash = %s (%i)' % (frame_class_lookup[fc], fc, swash_type_lookup[swash], swash))
|
||||
|
||||
filename = '%s_%s_motor_test.csv' % (frame_class_lookup[fc], swash_type_lookup[swash])
|
||||
os.system('./build/linux/examples/AP_Motors_test s frame_class=%d swash=%d > %s/%s' % (fc,swash,dir_name,filename))
|
||||
|
||||
print('Frame class = %s, swash = %s complete\n' % (frame_class_lookup[fc], swash_type_lookup[swash]))
|
||||
|
||||
|
||||
# ==============================================================================
|
||||
|
|
@ -104,162 +129,161 @@ if __name__ == '__main__':
|
|||
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.")
|
||||
parser.add_argument("-H","--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), help="list of frame classes to run comparison on. Defaults to test all helis.")
|
||||
parser.add_argument("-s","--swash-type", type=int, dest='swash_type', nargs="+", default=(0,1,2,3,4,5), help="list of swashplate types to run comparison on. Defaults to test all types.")
|
||||
parser.add_argument("-c","--compare", action='store_true', help='Compare only, do not re-run tests')
|
||||
parser.add_argument("-p","--plot", action='store_true', help='Plot comparison results')
|
||||
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()
|
||||
dir_name = 'motors_comparison'
|
||||
|
||||
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 not args.compare:
|
||||
# Create the new directory
|
||||
if dir_name not in os.listdir('./'):
|
||||
os.mkdir(dir_name)
|
||||
|
||||
# If we have already run this test, delete the old data
|
||||
if dir_name in os.listdir('./'):
|
||||
shutil.rmtree(dir_name)
|
||||
new_name = dir_name + "/new"
|
||||
if "new" not in os.listdir(dir_name):
|
||||
os.mkdir(new_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')
|
||||
|
||||
print('\nRunning motor tests with current changes\n')
|
||||
# run the test
|
||||
run_sweep(args.frame_class, args.swash_type, new_name)
|
||||
|
||||
# run the test
|
||||
if args.head:
|
||||
# rewind head and repeat test
|
||||
if args.head <= 0:
|
||||
print('Number of commits to roll back HEAD must be a positive integer value')
|
||||
quit()
|
||||
|
||||
# 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()
|
||||
|
||||
# Rewind the HEAD by the requested number of commits
|
||||
original_name = dir_name + "/original"
|
||||
if "original" not in os.listdir(dir_name):
|
||||
os.mkdir(original_name)
|
||||
|
||||
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')
|
||||
|
||||
run_sweep(args.frame_class, args.swash_type, original_name)
|
||||
|
||||
# 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))
|
||||
|
||||
# Print comparison
|
||||
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))
|
||||
for sw in args.swash_type:
|
||||
frame = frame_class_lookup[fc]
|
||||
swash = swash_type_lookup[sw]
|
||||
name = frame + ' ' + swash
|
||||
print('%s:' % name)
|
||||
|
||||
# move the csv to the directory for later comparison
|
||||
shutil.move(filename, os.path.join(dir_name, filename))
|
||||
filename = '%s_%s_motor_test.csv' % (frame, swash)
|
||||
new_points = DataPoints(os.path.join(dir_name, 'new/%s' % filename))
|
||||
old_points = DataPoints(os.path.join(dir_name, 'original/%s' % filename))
|
||||
|
||||
print('Frame class = %s complete\n' % frame_class_lookup[fc])
|
||||
if new_points.init_failed:
|
||||
print('\t failed!\n')
|
||||
|
||||
# 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('\tInputs max change:')
|
||||
INPUTS = ['Roll','Pitch','Yaw','Thr']
|
||||
input_diff = {}
|
||||
for field in INPUTS:
|
||||
input_diff[field] = [i-j for i,j in zip(old_points.data[field], new_points.data[field])]
|
||||
print('\t\t%s: %f' % (field, max(map(abs,input_diff[field]))))
|
||||
|
||||
print('Rewinding head by %d commits to: %s\n' % (args.head, base_commit))
|
||||
# Find number of motors
|
||||
num_motors = 0
|
||||
while True:
|
||||
num_motors += 1
|
||||
if 'Mot%i' % (num_motors+1) not in new_points.get_fields():
|
||||
break
|
||||
|
||||
# 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)
|
||||
print('\tOutputs max change:')
|
||||
output_diff = {}
|
||||
for i in range(num_motors):
|
||||
field = 'Mot%i' % (i+1)
|
||||
output_diff[field] = [i-j for i,j in zip(old_points.data[field], new_points.data[field])]
|
||||
print('\t\t%s: %f' % (field, max(map(abs,output_diff[field]))))
|
||||
|
||||
if result.returncode > 0:
|
||||
print('ERROR: Could not rewind HEAD. Exited with error:\n%s\n%s' % (result.stdout, result.stderr))
|
||||
quit()
|
||||
print('\tLimits max change:')
|
||||
LIMITS = ['LimR','LimP','LimY','LimThD','LimThU']
|
||||
limit_diff = {}
|
||||
for field in LIMITS:
|
||||
limit_diff[field] = [i-j for i,j in zip(old_points.data[field], new_points.data[field])]
|
||||
print('\t\t%s: %f' % (field, max(map(abs,limit_diff[field]))))
|
||||
print('\n')
|
||||
|
||||
# Rebuild
|
||||
os.system('./waf clean')
|
||||
os.system('./waf build --target examples/AP_Motors_test')
|
||||
if not args.plot:
|
||||
continue
|
||||
|
||||
# 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])
|
||||
# Plot comparison
|
||||
fig_size = (16, 8)
|
||||
fig, ax = plt.subplots(2, 2, figsize=fig_size)
|
||||
fig.suptitle('%s Input Diff' % name, fontsize=16)
|
||||
ax = ax.flatten()
|
||||
for i, field in enumerate(INPUTS):
|
||||
ax[i].plot(input_diff[field], color=RED)
|
||||
ax[i].set_xlabel('Test Number')
|
||||
ax[i].set_ylabel('%s Old - New' % field)
|
||||
plt.tight_layout(rect=[0, 0.0, 1, 0.95])
|
||||
|
||||
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))
|
||||
fig, ax = plt.subplots(2, num_motors, figsize=fig_size)
|
||||
fig.suptitle('%s Outputs' % name, fontsize=16)
|
||||
for i in range(num_motors):
|
||||
field = 'Mot%i' % (i+1)
|
||||
ax[0,i].plot(old_points.data[field], color=RED)
|
||||
ax[0,i].plot(new_points.data[field], color=BLUE)
|
||||
ax[0,i].set_ylabel(field)
|
||||
ax[0,i].set_xlabel('Test No')
|
||||
ax[1,i].plot(output_diff[field], 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])
|
||||
|
||||
# move the csv to the directory for later comparison
|
||||
shutil.move(filename, os.path.join(dir_name, filename))
|
||||
fig, ax = plt.subplots(2, 5, figsize=fig_size)
|
||||
fig.suptitle(name + ' Limits', fontsize=16)
|
||||
for i, field in enumerate(LIMITS):
|
||||
ax[0,i].plot(old_points.data[field], color=RED)
|
||||
ax[0,i].plot(new_points.data[field], color=BLUE)
|
||||
ax[0,i].set_ylabel(field)
|
||||
ax[0,i].set_xlabel('Test No')
|
||||
ax[1,i].plot(limit_diff[field], 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('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 ***')
|
||||
if args.plot:
|
||||
plt.show()
|
||||
|
|
|
|||
Loading…
Reference in New Issue