WIP: use new CA topic instead of actuator_outputs/controls

Signed-off-by: Silvan Fuhrer <[email protected]>

Author: sfuhrer

Tools/parametric_model/configs/standardplane_model.yaml

@@ -17,7 +17,7 @@ model_config:
         - rotor_4:
           description: "puller rotor"
           rotor_type: "RotorModel"
-          dataframe_name: "u4"
+          dataframe_name: "u_motor"
           rotor_axis:
             - 1
             - 0
@@ -32,19 +32,19 @@ model_config:
       wing_:
         - control_surface_0:
           description: "aileron_right"
-          dataframe_name: "u6"
+          dataframe_name: "u_aileron"
 
         - control_surface_1:
           description: "aileron_left"
-          dataframe_name: "u5"
+          dataframe_name: "u_aileron"
 
         - control_surface_2:
           description: "elevator"
-          dataframe_name: "u7"
+          dataframe_name: "u_elevator"
 
         - control_surface_3:
           description: "rudder"
-          dataframe_name: "u2"
+          dataframe_name: "u_rudder"
 
         # - control_surface_4:
         #   description: "flaps"
@@ -110,31 +110,61 @@ dynamics_model_config:
   estimate_angular_acceleration: False
   data:
     required_ulog_topics:
-      actuator_outputs:
+      vehicle_thrust_setpoint:
         ulog_name:
           - "timestamp"
-          - "output[2]" # Rudder
-          # - "output[3]" # Flaps
-          - "output[4]" # motor
-          - "output[5]" # left aileron
-          - "output[6]" # right aileron
-          - "output[7]" # elevator
+          - "xyz[0]" # thrust in x
         dataframe_name:
           - "timestamp"
-          - "u2"
-          # - "u3"
-          - "u4"
-          - "u5"
-          - "u6"
-          - "u7"
+          - "u_motor"
         actuator_type:
           - "timestamp"
-          - "control_surface"
-          # - "control_surface"
           - "motor"
+      vehicle_torque_setpoint:
+        ulog_name:
+          - "timestamp"
+          - "xyz[0]" # torque in x
+          - "xyz[1]" # torque in y
+          - "xyz[2]" # torque in z
+        dataframe_name:
+          - "timestamp"
+          - "u_aileron"
+          - "u_elevator"
+          - "u_rudder"
+        actuator_type:
+          - "timestamp"
           - "control_surface"
           - "control_surface"
           - "control_surface"
+      # actuator_motors:
+      #   ulog_name:
+      #     - "timestamp"
+      #     - "control[0]"
+      #   dataframe_name:
+      #     - "timestamp"
+      #     - "u_motor"
+      #   actuator_type:
+      #     - "timestamp"
+      #     - "motor"
+      # actuator_servos:
+      #   ulog_name:
+      #     - "timestamp"
+      #     - "control[0]"
+      #     - "control[1]"
+      #     - "control[2]"
+      #     - "control[3]"
+      #   dataframe_name:
+      #     - "timestamp"
+      #     - "u_aileron_left"
+      #     - "u_aileron_right"
+      #     - "u_elevator_new"
+      #     - "u_rudder_new"
+      #   actuator_type:
+      #     - "timestamp"
+      #     - "control_surface"
+      #     - "control_surface"
+      #     - "control_surface"
+      #     - "control_surface"
       vehicle_local_position:
         ulog_name:
           - "timestamp"
@@ -182,3 +212,10 @@ dynamics_model_config:
           - "ang_acc_b_x"
           - "ang_acc_b_y"
           - "ang_acc_b_z"
+      vehicle_land_detected:
+        ulog_name:
+          - "timestamp"
+          - "landed"
+        dataframe_name:
+          - "timestamp"
+          - "landed"

Tools/parametric_model/src/models/dynamics_model.py

@@ -86,7 +86,7 @@ def __init__(self, config_dict):
 
     def prepare_regression_matrices(self):
         if "V_air_body_x" not in self.data_df:
-            self.normalize_actuators()
+            # self.normalize_actuators()
             self.compute_airspeed_from_groundspeed(["vx", "vy", "vz"])
 
         # Rotor features

Tools/parametric_model/src/tools/data_handler.py

@@ -162,9 +162,7 @@ def compute_resampled_dataframe(self, ulog):
             else:
                 curr_df = pandas_from_topic(ulog, [topic_type])
 
-            if topic_type == "actuator_outputs":
-                fts = compute_flight_time(curr_df)
-            elif topic_type == "actuator_controls_0":
+            if topic_type == "vehicle_land_detected":
                 fts = compute_flight_time(curr_df)
 
             curr_df = curr_df[topic_dict["ulog_name"]]

Tools/parametric_model/src/tools/dataframe_tools.py

@@ -41,22 +41,22 @@
 from src.tools.ulog_tools import pandas_from_topic
 from src.tools.quat_utils import slerp
 
-# pre normalization thresholds
-PWM_THRESHOLD = 1000
-ACTUATOR_CONTROLS_THRESHOLD = -0.2
-
 
 def compute_flight_time(act_df, pwm_threshold=None, control_threshold=None):
-    """This function computes the flight time by a simple thresholding of actuator outputs or control values. 
-    This works usually well for logs from the simulator or mission flights. But in some cases the assumption of an actuator output staying higher than the trsehhold for the hole flight might not be valid."""
+    """This function computes the flight time by:
+    Option 1: listen to vehicle_land_detected/landed
+    Option 2: user defined start/end time stamp (micro seconds) 
+    """
 
-    if pwm_threshold is None:
-        pwm_threshold = PWM_THRESHOLD
+    # # Option 1:
+    # print("act_df: ", act_df)
+    # act_df_crp = act_df[act_df.iloc[:, 4] < 1] # take part where landed is 0
+    # print("act_df_crp after selection: ", act_df_crp)
 
-    if control_threshold is None:
-        control_threshold = ACTUATOR_CONTROLS_THRESHOLD
-    act_df_crp = act_df[act_df.iloc[:, 2] > pwm_threshold]
-    act_df_crp = act_df[act_df.iloc[:, 4] > pwm_threshold]
+    # print("act_df: ", act_df)
+    act_df_crp = act_df[act_df.iloc[:, 0] > 405000000]
+    act_df_crp = act_df_crp[act_df_crp.iloc[:, 0] < 430000000]
+    # print("act_df_crp after selection: ", act_df_crp)
 
     t_start = act_df_crp.iloc[1, 0]
     t_end = act_df_crp.iloc[(act_df_crp.shape[0]-1), 0]