Add Lee controller

bindings/cffirmware.i

@@ -19,6 +19,7 @@
 #include "num.h"
 #include "controller_mellinger.h"
 #include "controller_brescianini.h"
+#include "controller_lee.h"
 #include "power_distribution.h"
 #include "axis3fSubSampler.h"
 #include "outlierFilterTdoa.h"
@@ -35,6 +36,7 @@
 %include "imu_types.h"
 %include "controller_mellinger.h"
 %include "controller_brescianini.h"
+%include "controller_lee.h"
 %include "power_distribution.h"
 %include "axis3fSubSampler.h"
 %include "outlierFilterTdoa.h"

bindings/setup.py

@@ -41,6 +41,7 @@
     "src/modules/src/controller/attitude_pid_controller.c",
     "src/modules/src/controller/controller_mellinger.c",
     "src/modules/src/controller/controller_brescianini.c",
+    "src/modules/src/controller/controller_lee.c",
     "src/utils/src/pid.c",
     "src/utils/src/filter.c",
     "src/utils/src/num.c",

src/modules/interface/controller/controller.h

@@ -34,6 +34,7 @@ typedef enum {
   ControllerTypeMellinger,
   ControllerTypeINDI,
   ControllerTypeBrescianini,
+  ControllerTypeLee,
 #ifdef CONFIG_CONTROLLER_OOT
   ControllerTypeOot,
 #endif

src/modules/interface/controller/controller_lee.h

@@ -0,0 +1,76 @@
+/*
+The MIT License (MIT)
+
+Copyright (c) 2024 Khaled Wahba
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+#ifndef __CONTROLLER_LEE_H__
+#define __CONTROLLER_LEE_H__
+
+#include "stabilizer_types.h"
+
+// This structure contains the mutable state and inmutable parameters
+typedef struct controllerLee_s {
+    float mass;
+    float thrustSi;
+    struct vec J; // Inertia matrix (diagonal matrix); kg m^2
+
+    // Position PID
+    struct vec Kpos_P; // Kp in paper
+    float Kpos_P_limit;
+    struct vec Kpos_D; // Kv in paper
+    float Kpos_D_limit;
+    struct vec Kpos_I; // not in paper
+    float Kpos_I_limit;
+    struct vec i_error_pos;
+    struct vec p_error;
+    struct vec v_error;
+    // Attitude PID
+    struct vec KR;
+    struct vec Komega;
+    struct vec KI;
+    struct vec i_error_att;
+    // Logging variables
+    struct vec rpy;
+    struct vec rpy_des;
+    struct mat33 R_des;
+    struct vec omega;
+    struct vec omega_r;
+    struct vec u;
+} controllerLee_t;
+
+
+void controllerLeeInit(controllerLee_t* self);
+void controllerLeeReset(controllerLee_t* self);
+void controllerLee(controllerLee_t* self, control_t *control, const setpoint_t *setpoint,
+                                         const sensorData_t *sensors,
+                                         const state_t *state,
+                                         const uint32_t tick);
+
+#ifdef CRAZYFLIE_FW
+void controllerLeeFirmwareInit(void);
+bool controllerLeeFirmwareTest(void);
+void controllerLeeFirmware(control_t *control, const setpoint_t *setpoint,
+                                         const sensorData_t *sensors,
+                                         const state_t *state,
+                                         const uint32_t tick);
+#endif // CRAZYFLIE_FW defined
+
+#endif //__CONTROLLER_LEE_H__

src/modules/interface/power_distribution.h

@@ -69,4 +69,11 @@ uint16_t powerDistributionStopRatio(uint32_t id);
  */
 uint32_t powerDistributionGetIdleThrust();
 
+/**
+ * @brief Compute the maximum thrust
+ *
+ * @return float The maximum thrust of the robot [N]
+ */
+float powerDistributionGetMaxThrust();
+
 #endif //__POWER_DISTRIBUTION_H__

src/modules/interface/pptraj.h

@@ -120,6 +120,7 @@ struct traj_eval
 	struct vec pos;
 	struct vec vel;
 	struct vec acc;
+	struct vec jerk;
 	struct vec omega;
 	float yaw;
 };

src/modules/interface/stabilizer_types.h

@@ -65,6 +65,7 @@ typedef struct vec3_s vector_t;
 typedef struct vec3_s point_t;
 typedef struct vec3_s velocity_t;
 typedef struct vec3_s acc_t;
+typedef struct vec3_s jerk_t;
 
 /* Orientation as a quaternion */
 typedef struct quaternion_s {
@@ -257,6 +258,7 @@ typedef struct setpoint_s {
   point_t position;         // m
   velocity_t velocity;      // m/s
   acc_t acceleration;       // m/s^2
+  jerk_t jerk;              // m/s^3
   bool velocity_body;       // true if velocity is given in body frame; false if velocity is given in world frame
 
   struct {

src/modules/src/controller/Kbuild

@@ -6,3 +6,4 @@ obj-y += controller_pid.o
 obj-y += controller_brescianini.o
 obj-y += position_controller_indi.o
 obj-y += position_controller_pid.o
+obj-y += controller_lee.o

src/modules/src/controller/controller.c

@@ -7,6 +7,7 @@
 #include "controller_mellinger.h"
 #include "controller_indi.h"
 #include "controller_brescianini.h"
+#include "controller_lee.h"
 
 #include "autoconf.h"
 
@@ -31,6 +32,7 @@ static ControllerFcns controllerFunctions[] = {
   #ifdef CONFIG_CONTROLLER_OOT
   {.init = controllerOutOfTreeInit, .test = controllerOutOfTreeTest, .update = controllerOutOfTree, .name = "OutOfTree"},
   #endif
+  {.init = controllerLeeFirmwareInit, .test = controllerLeeFirmwareTest, .update = controllerLeeFirmware, .name = "Lee"},
 };