Replace KF & KM with thrust curves, add B500 model

Author: ratheron

drone_controllers/mellinger/control.py

@@ -153,8 +153,7 @@ def attitude2force_torque(
     pwm_min: float,
     pwm_max: float,
     L: float,
-    KM: float,
-    KF: float,
+    thrust2torque: float,
     mixing_matrix: Array,
 ) -> tuple[Array, Array, Array]:
     """Compute the attitude to desired force-torque part of the Mellinger controller.
@@ -186,8 +185,7 @@ def attitude2force_torque(
         prev_ang_vel: Previous angular velocity in rad/s.
         prev_ang_vel_des: Previous angular velocity command in rad/s.
         L: Distance from the center of the quadrotor to the center of the rotor in m.
-        KM: Torque constant (Nm/RPM).
-        KF: Force constant (N/RPM).
+        thrust2torque: Conversion factor (m).
         mixing_matrix: Mixing matrix for the motor forces with shape (4, 3).
 
     Returns:
@@ -252,7 +250,7 @@ def attitude2force_torque(
     # controllers within drone_models, we still want to return SI forces and torques. We thus need
     # to convert the legacy output to SI units.
     # l. 310 ff
-    torque_des = (mixing_matrix @ motor_forces[..., None])[..., 0] * xp.stack([L, L, KM / KF])
+    torque_des = (mixing_matrix @ motor_forces[..., None])[..., 0] * xp.stack([L, L, thrust2torque])
     force_des = xp.sum(motor_forces, axis=-1)[..., None]
     return force_des, torque_des, r_int_error
 
@@ -270,8 +268,8 @@ def force_torque2rotor_vel(
     thrust_min: float,
     thrust_max: float,
     L: float,
-    KM: float,
-    KF: float,
+    rpm2thrust: Array,
+    thrust2torque: float,
     mixing_matrix: Array,
 ) -> Array:
     """Convert desired collective thrust and torques to rotor speeds.
@@ -293,8 +291,8 @@ def force_torque2rotor_vel(
         thrust_min: Minimum thrust in N.
         thrust_max: Maximum thrust in N.
         L: Distance from the center of the quadrotor to the center of the rotor in m.
-        KM: Torque constant (Nm/RPM).
-        KF: Force constant (N/RPM).
+        rpm2thrust: Force constants (N/RPM, N/RPM**2).
+        thrust2torque: Conversion factor (m).
         mixing_matrix: Mixing matrix for the motor forces with shape (4, 3).
 
     Returns:
@@ -303,9 +301,11 @@ def force_torque2rotor_vel(
     xp = array_namespace(torque)
     assert torque.shape[-1] == 3, f"Torque must have shape (..., 3), but has {torque.shape}"
     assert force.shape[-1] == 1, f"Force must have shape (..., 1), but has {force.shape}"
-    torque_forces = (torque * xp.asarray([1 / L, 1 / L, KF / KM])) @ mixing_matrix
+    torque_forces = (torque * xp.asarray([1 / L, 1 / L, 1 / thrust2torque])) @ mixing_matrix
     motor_forces = (torque_forces + force) / 4
     # Clip motor forces on the thrust instead of PWM level.
     motor_forces = xp.where(xp.all(force == 0), 0.0, xp.clip(motor_forces, thrust_min, thrust_max))
     # Assume perfect battery compensation and calculate the desired motor speeds directly
-    return motor_force2rotor_vel(motor_forces, KF)
+    return motor_force2rotor_vel(
+        motor_forces, rpm2thrust[2]
+    )  # TODO change function to take all rpm2thrust params

drone_controllers/mellinger/params.py

@@ -50,8 +50,7 @@ class AttitudeParams(NamedTuple):
     pwm_min: float
     pwm_max: float
     L: float
-    KF: float
-    KM: float
+    thrust2torque: float
     mixing_matrix: Array
 
     @staticmethod
@@ -72,8 +71,8 @@ class ForceTorqueParams(NamedTuple):
     thrust_min: float
     thrust_max: float
     L: float
-    KF: float
-    KM: float
+    rpm2thrust: Array
+    thrust2torque: float
     mixing_matrix: Array
 
     @staticmethod

drone_controllers/mellinger/params.toml

@@ -7,12 +7,13 @@ pwm_min = 20000.0
 pwm_max = 65535.0
 torque_pwm_max = [32000.0, 32000.0, 32000.0]
 L = 0.03253
-KF = 3.16e-10  # TODO: Check what the firmware uses
-KM = 7.94e-12
+rpm2thrust = [0.0, 0.0, 3.16e-10] # TODO
+rpm2torque = [0.0, 0.0, 7.94e-12] # TODO
+thrust2torque = 0.002513 # TODO
 mixing_matrix = [
-    [-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]
+    [-1.0,  1.0, -1.0,  1.0]
 ]
 gravity_vec = [0.0, 0.0, -9.81]
 
@@ -29,31 +30,64 @@ ki_m = [0.0, 0.0, 500.0]
 kd_omega = [200.0, 200.0, 0.0]
 int_err_max = [1.0, 1.0, 1500.0]
 
-[cf2x_B500.core]
-mass = 0.034
+[cf2x_P250.core]
+mass = 0.033
 mass_thrust = 132000
-thrust_min = 0.03
-thrust_max = 0.18
+thrust_min = 0.02
+thrust_max = 0.1125
 pwm_min = 20000.0
 pwm_max = 65535.0
 torque_pwm_max = [32000.0, 32000.0, 32000.0]
 L = 0.03253
-KF = 3.16e-10  # TODO: Check what the firmware uses
-KM = 7.94e-12
+rpm2thrust = [0.0, 0.0, 5.792654323957372e-10] # TODO
+rpm2torque = [0.0, 0.0, 7.94e-12] # TODO
+thrust2torque = 0.001371 # TODO
 mixing_matrix = [
-    [-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]
+    [-1.0,  1.0, -1.0,  1.0]
 ]
 gravity_vec = [0.0, 0.0, -9.81]
 
-[cf2x_B500.state2attitude]
+[cf2x_P250.state2attitude]
+kp = [0.4, 0.4, 1.25]
+kd = [0.2, 0.2, 0.5]
+ki = [0.05, 0.05, 0.05]
+int_err_max = [2.0, 2.0, 0.4]
+
+[cf2x_P250.attitude2force_torque]
+kR = [70000.0, 70000.0, 60000.0]
+kw = [20000.0, 20000.0, 12000.0]
+ki_m = [0.0, 0.0, 500.0]
+kd_omega = [200.0, 200.0, 0.0]
+int_err_max = [1.0, 1.0, 1500.0]
+
+[cf21B_500.core]
+mass = 0.04338
+gravity_vec = [0.0, 0.0, -9.81]
+mass_thrust = 132000
+thrust_min = 0.03
+thrust_max = 0.18
+pwm_min = 20000.0
+pwm_max = 65535.0
+torque_pwm_max = [32000.0, 32000.0, 32000.0]
+L = 0.035355
+rpm2thrust = [0.0, -8.357406697944084e-07, 4.716909960828207e-10]
+rpm2torque = [0.0, 2.435151264350409e-09, 2.424629050055297e-12]
+thrust2torque = 0.00593893393599368
+mixing_matrix = [
+    [-1.0, -1.0,  1.0,  1.0],
+    [-1.0,  1.0,  1.0, -1.0],
+    [-1.0,  1.0, -1.0,  1.0]
+]
+
+[cf21B_500.state2attitude]
 kp = [0.4, 0.4, 1.25]
 kd = [0.2, 0.2, 0.5]
 ki = [0.05, 0.05, 0.05]
 int_err_max = [2.0, 2.0, 0.4]
 
-[cf2x_B500.attitude2force_torque]
+[cf21B_500.attitude2force_torque]
 kR = [70000.0, 70000.0, 60000.0]
 kw = [20000.0, 20000.0, 12000.0]
 ki_m = [0.0, 0.0, 500.0]