From c4bd14098ec4547e6baebac6245eea380ceb6464 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Fri, 18 Jul 2025 15:34:54 +0200 Subject: [PATCH] Update control effectiveness script (#3501) * update control effectiveness script for new matrix format * add a config file example for a quadcopter - full indi - no G2 vector - new output format --- .../config/quad_full_no_g2.json | 133 ++++++++++++++++++ .../control_effectiveness_estimation.py | 52 +++++-- .../control_effectiveness_utils.py | 11 +- 3 files changed, 177 insertions(+), 19 deletions(-) create mode 100644 sw/tools/control_effectiveness/config/quad_full_no_g2.json diff --git a/sw/tools/control_effectiveness/config/quad_full_no_g2.json b/sw/tools/control_effectiveness/config/quad_full_no_g2.json new file mode 100644 index 0000000000..b4d2e03521 --- /dev/null +++ b/sw/tools/control_effectiveness/config/quad_full_no_g2.json @@ -0,0 +1,133 @@ +{ + "variables" : { + "filt_cutoff" : 4.0, + "act_freq" : 15.0 + }, + "data" : [ + { + "name" : "time", + "type" : "timestamp", + "column" : 0, + "index" : -1, + "format" : "float", + "scale" : 1.0, + "unit" : "s" + }, + { + "name" : "rate.x", + "type" : "input", + "column" : 1, + "index" : 0, + "format" : "bfp", + "resolution" : 12, + "filters" : [["butter", [2, "filt_cutoff"]], ["diff_signal", [2]]] + }, + { + "name" : "rate.y", + "type" : "input", + "column" : 2, + "index" : 1, + "format" : "bfp", + "resolution" : 12, + "filters" : [["butter", [2, "filt_cutoff"]], ["diff_signal", [2]]] + }, + { + "name" : "rate.z", + "type" : "input", + "column" : 3, + "index" : 2, + "format" : "bfp", + "resolution" : 12, + "filters" : [["butter", [2, "filt_cutoff"]], ["diff_signal", [2]]] + }, + { + "name" : "accel.x", + "type" : "input", + "column" : 4, + "index" : -1, + "format" : "bfp", + "resolution" : 10, + "filters" : [] + }, + { + "name" : "accel.y", + "type" : "input", + "column" : 5, + "index" : -1, + "format" : "bfp", + "resolution" : 10, + "filters" : [] + }, + { + "name" : "accel.z", + "type" : "input", + "column" : 6, + "index" : 3, + "format" : "bfp", + "resolution" : 10, + "filters" : [["butter", [2, "filt_cutoff"]], ["diff_signal", [1]]] + }, + { + "name" : "act_1", + "type" : "command", + "column" : 11, + "index" : 0, + "format" : "pprz", + "filters" : [["1st_order", ["act_freq"]], ["butter", [2, "filt_cutoff"]], ["diff_signal", [1]]] + }, + { + "name" : "act_2", + "type" : "command", + "column" : 12, + "index" : 1, + "format" : "pprz", + "filters" : [["1st_order", ["act_freq"]], ["butter", [2, "filt_cutoff"]], ["diff_signal", [1]]] + }, + { + "name" : "act_3", + "type" : "command", + "column" : 13, + "index" : 2, + "format" : "pprz", + "filters" : [["1st_order", ["act_freq"]], ["butter", [2, "filt_cutoff"]], ["diff_signal", [1]]] + }, + { + "name" : "act_4", + "type" : "command", + "column" : 14, + "index" : 3, + "format" : "pprz", + "filters" : [["1st_order", ["act_freq"]], ["butter", [2, "filt_cutoff"]], ["diff_signal", [1]]] + } + ], + "mixing" : [ + [ 1.0, 1.0, 1.0, 1.0 ], + [ 1.0, 1.0, 1.0, 1.0 ], + [ 1.0, 1.0, 1.0, 1.0 ], + [ 1.0, 1.0, 1.0, 1.0 ] + ], + "display" : [ + { + "name" : "G1", + "matrix": [ + [[0,0],[0,1],[0,2],[0,3]], + [[1,0],[1,1],[1,2],[1,3]], + [[2,0],[2,1],[2,2],[2,3]], + [[3,0],[3,1],[3,2],[3,3]] + ], + "scaling" : 1000 + }, + { + "name" : "FILT_CUTOFF", + "coef" : "filt_cutoff" + }, + { + "name" : "FILT_CUTOFF_R", + "coef" : "filt_cutoff" + }, + { + "name" : "ACT_FREQ", + "coef" : ["act_freq","act_freq","act_freq","act_freq"] + } + ] +} diff --git a/sw/tools/control_effectiveness/control_effectiveness_estimation.py b/sw/tools/control_effectiveness/control_effectiveness_estimation.py index f1087a37c3..e29ef48993 100755 --- a/sw/tools/control_effectiveness/control_effectiveness_estimation.py +++ b/sw/tools/control_effectiveness/control_effectiveness_estimation.py @@ -28,7 +28,7 @@ import matplotlib.pyplot as plt import control_effectiveness_utils as ut -def process_data(conf, f_name, start, end, freq=None, act_dyn=None, verbose=False, plot=False): +def process_data(conf, f_name, start, end, freq=None, act_freq=None, verbose=False, plot=False): # Read data from log file data = genfromtxt(f_name, delimiter=',', skip_header=1) @@ -38,8 +38,8 @@ def process_data(conf, f_name, start, end, freq=None, act_dyn=None, verbose=Fals var = {} if 'variables' in conf: var = conf['variables'] - if act_dyn is not None: - var['act_dyn'] = act_dyn # this may overwrite default value + if act_freq is not None: + var['act_freq'] = act_freq # this may overwrite default value # Get number of inputs and outputs mixing = np.array(conf['mixing']) @@ -114,16 +114,40 @@ def process_data(conf, f_name, start, end, freq=None, act_dyn=None, verbose=Fals if disp is not None: print("\nAdd the following lines to your airframe file:\n") + def value_or_default(key, dic, default): + if key in dic: + return dic[key] + else: + return default for d in disp: name = d['name'] - coef = d['coef'] - if isinstance(coef, (int, float, str)): - print(''.format(name, ut.get_param(coef, var))) - elif len(coef) == 2 and isinstance(coef[0], int) and isinstance(coef[1], int): - print(''.format(name, output[coef[0], coef[1]])) - else: - s = ', '.join(["{:.5f}".format(output[e[0], e[1]]) for e in coef]) - print(''.format(name, s)) + coef = value_or_default('coef', d, None) + matrix = value_or_default('matrix', d, None) + scaling = value_or_default('scaling', d, 1.) + if matrix is not None: + print(f'') + for i in range(len(matrix)): + l = [] + for e in matrix[i]: + if isinstance(e, (int, float, str)): + l.append("{:.2f}".format(scaling*ut.get_param(e, var))) + else: + l.append("{:.2f}".format(scaling*output[e[0], e[1]])) + print(f' ') + print('') + elif coef is not None: + if isinstance(coef, (int, float, str)): + print(''.format(name, scaling*ut.get_param(coef, var))) + elif len(coef) == 2 and isinstance(coef[0], int) and isinstance(coef[1], int): + print(''.format(name, scaling*output[coef[0], coef[1]])) + else: + l = [] + for e in coef: + if isinstance(e, (int, float, str)): + l.append("{:.2f}".format(scaling*ut.get_param(e, var))) + else: + l.append("{:.2f}".format(scaling*output[e[0], e[1]])) + print(''.format(name, ', '.join(l))) if plot: plt.show() @@ -136,10 +160,10 @@ def main(): parser = ArgumentParser(description="Control effectiveness estimation tool") parser.add_argument("config", help="JSON configuration file") parser.add_argument("data", help="Log file for parameter estimation") - parser.add_argument("-f", "--freq", dest="freq", + parser.add_argument("-sf", "--sample_freq", dest="freq", help="Sampling frequency, trying auto freq if not set") - parser.add_argument("-d", "--dyn", dest="dyn", - help="First order actuator dynamic (discrete time), 'None' for config file default") + parser.add_argument("-af", "--act_freq", dest="dyn", + help="First order actuator frequency, 'None' for config file default") parser.add_argument("-s", "--start", help="Start time", action="store", dest="start", default="0") diff --git a/sw/tools/control_effectiveness/control_effectiveness_utils.py b/sw/tools/control_effectiveness/control_effectiveness_utils.py index 381affaf59..147dcbd146 100644 --- a/sw/tools/control_effectiveness/control_effectiveness_utils.py +++ b/sw/tools/control_effectiveness/control_effectiveness_utils.py @@ -34,10 +34,11 @@ from matplotlib.pyplot import show # functions for actuators model # -def first_order_model(signal, tau): +def first_order_model(signal, freq, cutoff_freq): ''' - Apply a first order filter with (discrete) time constant tau + Apply a first order filter with cutoff freq ''' + tau = 1. - np.exp(-cutoff_freq / freq) return sp.signal.lfilter([tau], [1, tau-1], signal, axis=0) def rate_limit_model(signal, max_rate): @@ -77,8 +78,8 @@ def apply_filter(filt_name, params, signal, var): apply a filter to an input signal based on the config (name + params) ''' if filt_name == '1st_order': - # params = [tau] - return first_order_model(signal, get_param(params[0], var)) + # params = [freq] + return first_order_model(signal, var['freq'], get_param(params[0], var)) elif filt_name == 'rate_limit': # params = [max_rate] @@ -180,7 +181,7 @@ def print_results(): # def fit_axis(x, y, axis, start, end, verbose=False): - c = np.linalg.lstsq(x[start:end], y[start:end])#, rcond=None) + c = np.linalg.lstsq(x[start:end], y[start:end], rcond=None) if verbose: print("Fit axis", axis) print(c[0]*1000)