Merge pull request #1343 from IMRCLab/upstream_lee

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",

docs/functional-areas/sensor-to-control/controllers.md

@@ -6,10 +6,12 @@ page_id: controllers
 
 Once the [state estimator](/docs/functional-areas/sensor-to-control/state_estimators.md) have outputed the current (estimated) situation of the crazyflie in position velocity and attitude, it is time for the controllers to keep it that way or to move the crazyflie into a new position based on a setpoint. This is an important part of the stabilization system in the crazyflie.
 
- * [Overview of Control](#overview-of-control)
- * [Cascaded PID controller](#cascaded-pid-controller)
- * Mellinger Controller (TO DO)
- * INDI Controller (TO DO)
+- [Overview of control](#overview-of-control)
+- [Cascaded PID controller](#cascaded-pid-controller)
+- [Mellinger Controller](#mellinger-controller)
+- [INDI Controller](#indi-controller)
+- [Brescianini Controller](#brescianini-controller)
+- [Lee Controller](#lee-controller)
 
 ## Overview of control
 There are four levels to control in the Crazyflie:
@@ -55,3 +57,51 @@ Check the implementation details in `attitude_pid_controller.c` in `attitudeCont
 The most outer-loop of the cascaded PID controller is the position and velocity controller. It receives position or velocity input from a commander which are handled, since it is possible to set in the variable `setpoint_t` which  stabilization mode to use `stab_mode_t` (either position:  `modeAbs` or `modeVelocity`). These can be found in `stabilizer_types.h`. The control loop runs at 100 Hz.
 
 Check the implementation details in `position_controller_pid.c` in `positionController()` and  `velocityController()`.
+
+## Mellinger Controller
+
+Conceptually the Mellinger controller is similar to the cascaded PID controller, i.e. there is an attitude controller (running at 250 Hz) and a position controller (running at 100 Hz). The main difference to the cascaded PID controller is how errors are defined and how the position error is translated into desired attitude setpoints. Like the cascaded PID, this is a reactive geometric controller that uses the mathematical property of differential flatness. Details are given in the following scientific publication:
+
+```
+Daniel Mellinger, and Vijay Kumar
+Minimum snap trajectory generation and control for quadrotors
+IEEE International Conference on Robotics and Automation (ICRA), 2011
+https://doi.org/10.1109/ICRA.2011.5980409
+```
+
+The implementation follows the paper, also for the names of the variables. The main difference is the addition of I-gains and a D-term for the angular velocity.
+
+## INDI Controller
+
+This control algorithm is the Incremental Nonlinear Dynamic Inversion (INDI) controller. Details are given in the following scientific publication:
+
+```
+Ewoud J. J. Smeur, Qiping Chu, and Guido C. H. E. de Croon
+Adaptive Incremental Nonlinear Dynamic Inversion for Attitude Control of Micro Air Vehicles
+JCGD 2015
+https://doi.org/10.2514/1.G001490
+```
+
+## Brescianini Controller
+
+Details of this controller are in the following scientific publication:
+
+```
+Dario Brescianini, Markus Hehn, and Raffaello D'Andrea
+Nonlinear quadrocopter attitude control
+Technical Report ETHZ, 2013
+https://doi.org/10.3929/ethz-a-009970340
+```
+
+## Lee Controller
+
+Conceptually the Lee controller is similar to the cascaded PID controller, i.e. there is an attitude controller (running at 250 Hz) and a position controller (running at 100 Hz). The main difference to the cascaded PID controller is how errors are defined and how the position error is translated into desired attitude setpoints. Like the cascaded PID, this is a reactive geometric controller that uses the mathematical property of differential flatness. Compared to the Mellinger controller, a different angular velocity error and higher-order terms in the attitude controller are used. Details including a stability proof are given in the following scientific publication:
+
+```
+Taeyoung Lee, Melvin Leok, and N. Harris McClamroch
+Geometric Tracking Control of a Quadrotor UAV on SE(3)
+CDC 2010
+https://doi.org/10.1109/CDC.2010.5717652
+```
+
+The implementation follows the paper, also for the names of the variables. The main difference is the addition of I-gains, which are not needed for the theoretical proof, but helpful on the practical system.

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"},
 };