Add B500 params

Author: amacati

drone_controllers/mellinger/params.toml

@@ -28,3 +28,34 @@ 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]
+
+[cf2x_B500.core]
+mass = 0.034
+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.03253
+KF = 3.16e-10  # TODO: Check what the firmware uses
+KM = 7.94e-12
+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]
+]
+gravity_vec = [0.0, 0.0, -9.81]
+
+[cf2x_B500.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]
+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]