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

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"]
+    }
+  ]
+}

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('<define name="{}" value="{}"/>'.format(name, ut.get_param(coef, var)))
-            elif len(coef) == 2 and isinstance(coef[0], int) and isinstance(coef[1], int):
-                print('<define name="{}" value="{:.5f}"/>'.format(name, output[coef[0], coef[1]]))
-            else:
-                s = ', '.join(["{:.5f}".format(output[e[0], e[1]]) for e in coef])
-                print('<define name="{}" value="{}" type="float[]"/>'.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'<define name="{name}" type="matrix">')
+                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'  <field value="{l}" type="float[]"/>')
+                print('</define>')
+            elif coef is not None:
+                if isinstance(coef, (int, float, str)):
+                    print('<define name="{}" value="{}"/>'.format(name, scaling*ut.get_param(coef, var)))
+                elif len(coef) == 2 and isinstance(coef[0], int) and isinstance(coef[1], int):
+                    print('<define name="{}" value="{:.2f}"/>'.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('<define name="{}" value="{}" type="float[]"/>'.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")

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)