Moving angular acceleration differentiation to after resampling

Author: manumerous

Tools/parametric_model/configs/quadrotor_model.yaml

@@ -1,4 +1,3 @@
----
 # general information
 model_name: "Gazebo Standard Multirotor"
 model_type: "Standard Multirotor"
@@ -84,15 +83,15 @@ dynamics_model_config:
       vertical_c_m_leaver_quad: !!python/tuple [0.0, 2.0]
       vertical_c_m_rolling: !!python/tuple [0.0, 2.0]
       vertical_rot_drag_lin: !!python/tuple [0.0, 2.0]
-      vertical_rot_thrust_lin: !!python/tuple [-1.0, 0.0]
-      vertical_rot_thrust_quad: !!python/tuple [0.0, 10.0]
+      vertical_rot_thrust_lin: !!python/tuple [-5.0, 0.0]
+      vertical_rot_thrust_quad: !!python/tuple [0.0, 50.0]
   estimate_forces: True
   estimate_moments: True
-  resample_freq: 100.0
+  resample_freq: 250.0
   estimate_angular_acceleration: True
   data:
     required_ulog_topics:
-      # Use with simulator logs only, disable estimate_angular_acceleration when using
+      #Use with simulator logs only, disable estimate_angular_acceleration when using
       # vehicle_angular_acceleration:
       #   ulog_name:
       #     - "timestamp"

Tools/parametric_model/src/tools/data_handler.py

@@ -158,23 +158,42 @@ def compute_resampled_dataframe(self, ulog):
                 curr_df.columns = topic_dict["dataframe_name"]
             topic_type_bar.next()
             if (topic_type == "vehicle_angular_velocity" and self.estimate_angular_acceleration):
-                print(curr_df)
-                ang_vel_np = curr_df[["ang_vel_x",
+                ang_vel_mat = curr_df[["ang_vel_x",
                                       "ang_vel_y",  "ang_vel_z"]].to_numpy()
                 time_in_secods_np = (curr_df[["timestamp"]].to_numpy()/1000000)
                 time_in_secods_np = time_in_secods_np.flatten()
-                print(ang_vel_np)
-                ang_acc_np = np.gradient(ang_vel_np, time_in_secods_np, axis=0)
-                print(ang_acc_np)
+                ang_acc_np = np.gradient(
+                    ang_vel_mat, time_in_secods_np, axis=0)
                 topic_type_bar.next()
                 curr_df[["ang_acc_b_x", "ang_acc_b_y",
                          "ang_acc_b_z"]] = ang_acc_np
+
             df_list.append(curr_df)
 
         topic_type_bar.finish()
-        print(df_list)
         resampled_df = resample_dataframe_list(
             df_list, fts, self.resample_freq)
+        if (self.estimate_angular_acceleration):
+            ang_vel_mat = resampled_df[["ang_vel_x",
+                                       "ang_vel_y",  "ang_vel_z"]].to_numpy()
+            for i in range(3):
+                ang_vel_mat[:, i] = np.convolve(
+                    ang_vel_mat[:, i], np.ones(33), mode='same') / 33
+
+            # Alternate forward differentiation version
+            # ang_vel_mat_1 = np.roll(ang_vel_mat, -1, axis=0)
+            # diff_angular_acc_mat = (
+            #     ang_vel_mat_1 - ang_vel_mat) * self.resample_freq
+            # resampled_df[["ang_acc_b_x", "ang_acc_b_y",
+            #               "ang_acc_b_z"]] = diff_angular_acc_mat
+
+            time_in_secods_np = (
+                resampled_df[["timestamp"]].to_numpy()/1000000)
+            time_in_secods_np = time_in_secods_np.flatten()
+            ang_acc_np = np.gradient(ang_vel_mat, time_in_secods_np, axis=0)
+            topic_type_bar.next()
+            resampled_df[["ang_acc_b_x", "ang_acc_b_y",
+                          "ang_acc_b_z"]] = ang_acc_np
         return resampled_df.dropna()
 
     def visually_select_data(self, plot_config_dict=None):

Tools/parametric_model/src/tools/dataframe_tools.py

@@ -42,7 +42,7 @@
 from src.tools.quat_utils import slerp
 
 # pre normalization thresholds
-PWM_THRESHOLD = 1500
+PWM_THRESHOLD = 1050
 ACTUATOR_CONTROLS_THRESHOLD = -0.2
 
 
@@ -63,7 +63,7 @@ def compute_flight_time(ulog, pwm_threshold=None, control_threshold=None):
     if "actuator_outputs" in topic_type_list:
         act_df = pandas_from_topic(ulog, ["actuator_outputs"])
         # choose first actuator data
-        act_df_crp = act_df[act_df.iloc[:, 4] > pwm_threshold]
+        act_df_crp = act_df[act_df.iloc[:, 2] > pwm_threshold]
 
     # special case for aero mini tilt wing for asl
     elif "actuator_controls_0" in topic_type_list:
@@ -74,7 +74,6 @@ def compute_flight_time(ulog, pwm_threshold=None, control_threshold=None):
     else:
         print("could not select flight time due to missing actuator topic")
         exit(1)
-
         # set start and end time of flight duration
     t_start = act_df_crp.iloc[1, 0]
     t_end = act_df_crp.iloc[(act_df_crp.shape[0]-1), 0]