Update attitude controller

Author: ratheron

drone_controllers/mellinger/control.py

@@ -139,6 +139,7 @@ def attitude2force_torque(
     vel: Array,
     ang_vel: Array,
     cmd: Array,
+    prev_ang_vel: Array | None = None,
     ctrl_errors: tuple[Array, ...] | None = None,
     ctrl_info: tuple[Array, ...] | None = None,
     ctrl_freq: int = 500,
@@ -209,18 +210,13 @@ def attitude2force_torque(
     # Warning: We assume zero desired angular velocity
     ang_vel_des = xp.zeros_like(ang_vel)
     prev_ang_vel_des = xp.zeros_like(ang_vel)
-    ew = ang_vel_des - ang_vel  # if the setpoint is ever != 0 => change sign of setpoint[1]
+    ew = ang_vel_des - ang_vel
     # WARNING: if the setpoint is ever != 0 => change sign of ew.y!
 
-    # ang_vel_d_err likely dampens the system because of measurement noise. This term needs to be
-    # tuned to the sensors of the drone. Since we don't have similar noise properties in the sim, we
-    # set this term to zero. We still keep the calculation in here for completeness.
-    # prev_ang_vel = ang_vel if ctrl_info is None else ctrl_info[0]
-    # Disable the ang_vel_d_err term by setting prev_ang_vel to ang_vel. The other two terms are
-    # already zero.
-    prev_ang_vel = ang_vel
+    # l.259 ff [err_d_rpy]
+    prev_ang_vel = xp.zeros_like(ang_vel) if prev_ang_vel is None else prev_ang_vel
     ang_vel_d_err = ((ang_vel_des - prev_ang_vel_des) - (ang_vel - prev_ang_vel)) / dt
-    # # l.281: No err_d_yaw
+    # l.281: No err_d_yaw
     ang_vel_d_err = xpx.at(ang_vel_d_err)[..., 2].set(0)
 
     # l. 268 ff Integral Error

drone_controllers/mellinger/params.toml

@@ -1,6 +1,6 @@
 [cf2x_L250]
 [cf2x_L250.core]
-mass = 0.033
+mass = 0.029  # The controller is using the wrong mass by default
 mass_thrust = 132000
 pwm_min = 7000
 pwm_max = 65535
@@ -33,7 +33,7 @@ int_err_max = [1.0, 1.0, 1500.0]
 
 [cf2x_P250]
 [cf2x_P250.core]
-mass = 0.033
+mass = 0.029  # The controller is using the wrong mass by default
 mass_thrust = 132000
 pwm_min = 7000
 pwm_max = 65535
@@ -66,7 +66,7 @@ int_err_max = [1.0, 1.0, 1500.0]
 
 [cf2x_T350]
 [cf2x_T350.core]
-mass = 0.03454 # TODO
+mass = 0.0325  # The controller is using the wrong mass by default
 mass_thrust = 132000
 pwm_min = 7000
 pwm_max = 65535
@@ -100,7 +100,7 @@ int_err_max = [1.0, 1.0, 1500.0]
 [cf21B_500]
 [cf21B_500.core]
 gravity_vec = [0.0, 0.0, -9.81]
-mass = 0.04338
+mass = 0.0393  # The controller is using the wrong mass by default
 L = 0.035355
 rpm2thrust = [0.0, -3.133427287299859e-7, 4.407354891648379e-10]
 rpm2torque = [0.0, 1.65886356219615e-9, 2.4693477924534137e-12]