From 55242445b20330bcba4e1835b672d20188aca115 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Mon, 30 Oct 2023 22:21:44 +0000 Subject: [PATCH] AP_Motors: example: support setting and testing DUAL_MODE --- .../AP_Motors_test/AP_Motors_test.cpp | 9 + .../AP_Motors_test/run_heli_comparison.py | 203 ++++++++++-------- 2 files changed, 126 insertions(+), 86 deletions(-) diff --git a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp index 48eb729434..d65c940cd8 100644 --- a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp +++ b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp @@ -120,6 +120,15 @@ void setup() exit(1); } + } else if (strcmp(cmd,"dual_mode") == 0) { + if (frame_class != AP_Motors::MOTOR_FRAME_HELI_DUAL) { + ::printf("dual_mode only supported by dual heli frame type (%i), got %i\n", AP_Motors::MOTOR_FRAME_HELI_DUAL, frame_class); + exit(1); + } + + // look away now, more dodgy param access. + AP_Int8 *dual_mode = (AP_Int8*)motors + AP_MotorsHeli_Dual::var_info[1].offset; + dual_mode->set(value); } else if (strcmp(cmd,"frame_class") == 0) { // We must have the frame_class argument 2nd as resulting class is used to determine if diff --git a/libraries/AP_Motors/examples/AP_Motors_test/run_heli_comparison.py b/libraries/AP_Motors/examples/AP_Motors_test/run_heli_comparison.py index 69bdea9a6b..118a5de193 100644 --- a/libraries/AP_Motors/examples/AP_Motors_test/run_heli_comparison.py +++ b/libraries/AP_Motors/examples/AP_Motors_test/run_heli_comparison.py @@ -96,9 +96,12 @@ swash_type_lookup = {0: 'H3', 4: 'H4_90', 5: 'H4_45'} +dual_mode_lookup = {0: 'Longitudinal', + 1: 'Transverse', + 2: 'Intermeshing_or_Coaxial'} # Run sweep over range of types -def run_sweep(frame_class, swash_type, dir_name): +def run_sweep(frame_class, swash_type, dual_mode, dir_name): # configure and build the test os.system('./waf configure --board linux') @@ -107,19 +110,29 @@ def run_sweep(frame_class, swash_type, dir_name): # Run sweep for fc in frame_class: for swash in swash_type: - if swash is not None: - name = '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]) - swash_cmd = 'swash=%d' % swash + for mode in dual_mode: + if swash is not None: + swash_cmd = 'swash=%d' % swash - else: - name = 'frame class = %s (%i)' % (frame_class_lookup[fc], fc) - filename = '%s_motor_test.csv' % (frame_class_lookup[fc]) - swash_cmd = '' + if mode is not None: + name = 'frame class = %s (%i), swash = %s (%i), dual mode = %s (%i)' % (frame_class_lookup[fc], fc, swash_type_lookup[swash], swash, dual_mode_lookup[mode], mode) + filename = '%s_%s_%s_motor_test.csv' % (frame_class_lookup[fc], swash_type_lookup[swash], dual_mode_lookup[mode]) + mode_cmd = 'dual_mode=%d' % mode - print('Running motors test for %s' % name) - os.system('./build/linux/examples/AP_Motors_test s frame_class=%d %s > %s/%s' % (fc, swash_cmd, dir_name, filename)) - print('%s complete\n' % name) + else: + name = '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]) + mode_cmd = '' + + else: + name = 'frame class = %s (%i)' % (frame_class_lookup[fc], fc) + filename = '%s_motor_test.csv' % (frame_class_lookup[fc]) + swash_cmd = '' + mode_cmd = '' + + print('Running motors test for %s' % name) + os.system('./build/linux/examples/AP_Motors_test s frame_class=%d %s %s > %s/%s' % (fc, swash_cmd, mode_cmd, dir_name, filename)) + print('%s complete\n' % name) if __name__ == '__main__': @@ -135,6 +148,7 @@ if __name__ == '__main__': parser.add_argument("-s", "--swash-type", type=int, dest='swash_type', nargs="+", help="list of swashplate types to run comparison on. Defaults to test all types. Invalid for heli quad") 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') + parser.add_argument("-m", "--dual_mode", type=int, dest='dual_mode', help='Set DUAL_MODE, 0:Longitudinal, 1:Transverse, 2:Intermeshing/Coaxial, default:run all') args = parser.parse_args() if 13 in args.frame_class: @@ -147,6 +161,16 @@ if __name__ == '__main__': if not args.swash_type: args.swash_type = (0, 1, 2, 3, 4, 5) + if (args.frame_class != [11]) and (args.dual_mode is not None): + print('Only Frame %s (%i) supports dual_mode' % (frame_class_lookup[11], 11)) + quit() + + if args.frame_class == [11]: + if args.dual_mode is None: + args.dual_mode = (0, 1, 2) + else: + args.dual_mode = [None] + dir_name = 'motors_comparison' if not args.compare: @@ -161,7 +185,7 @@ if __name__ == '__main__': print('\nRunning motor tests with current changes\n') # run the test - run_sweep(args.frame_class, args.swash_type, new_name) + run_sweep(args.frame_class, args.swash_type, args.dual_mode, new_name) if args.head: # rewind head and repeat test @@ -213,91 +237,98 @@ if __name__ == '__main__': # Print comparison for fc in args.frame_class: for sw in args.swash_type: - frame = frame_class_lookup[fc] - if sw is not None: - swash = swash_type_lookup[sw] - name = frame + ' ' + swash - filename = '%s_%s_motor_test.csv' % (frame, swash) + for dm in args.dual_mode: + frame = frame_class_lookup[fc] + if sw is not None: + swash = swash_type_lookup[sw] + if dm is not None: + mode = dual_mode_lookup[dm] + name = frame + ' ' + swash + ' ' + mode + filename = '%s_%s_%s_motor_test.csv' % (frame, swash, mode) - else: - name = frame - filename = '%s_motor_test.csv' % (frame) + else: + name = frame + ' ' + swash + filename = '%s_%s_motor_test.csv' % (frame, swash) - print('%s:' % name) + else: + name = frame + filename = '%s_motor_test.csv' % (frame) - new_points = DataPoints(os.path.join(dir_name, 'new/%s' % filename)) - old_points = DataPoints(os.path.join(dir_name, 'original/%s' % filename)) + print('%s:' % name) - if new_points.init_failed: - print('\t failed!\n') + new_points = DataPoints(os.path.join(dir_name, 'new/%s' % filename)) + old_points = DataPoints(os.path.join(dir_name, 'original/%s' % filename)) - 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])))) + if new_points.init_failed: + print('\t failed!\n') - # 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 + 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('\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])))) + # 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 - 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') + 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 not args.plot: - continue + 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') - # 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]) + if not args.plot: + continue - 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]) + # 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]) - 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]) + 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]) + + 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]) if args.plot: plt.show()