From 545e31b1109174a9877f3af520594c3691234c82 Mon Sep 17 00:00:00 2001
From: Dennis Schmidt <dennis.schmidt.0@web.de>
Date: Tue, 24 Oct 2023 11:03:18 +0200
Subject: [PATCH 01/14] dennis test lee success

---
 src/deck/drivers/src/usddeck.c                |   4 +-
 src/modules/interface/controller/controller.h |   1 +
 .../interface/controller/controller_lee.h     |  79 ++++
 src/modules/interface/pptraj.h                |   1 +
 src/modules/interface/stabilizer_types.h      |   2 +
 src/modules/src/controller/Kbuild             |   1 +
 src/modules/src/controller/controller.c       |   2 +
 src/modules/src/controller/controller_lee.c   | 386 ++++++++++++++++++
 src/modules/src/crtp_commander_generic.c      |   3 +-
 src/modules/src/crtp_commander_high_level.c   |   4 +
 src/modules/src/pptraj.c                      |   6 +-
 vendor/libdw1000                              |   2 +-
 12 files changed, 485 insertions(+), 6 deletions(-)
 create mode 100644 src/modules/interface/controller/controller_lee.h
 create mode 100644 src/modules/src/controller/controller_lee.c

diff --git a/src/deck/drivers/src/usddeck.c b/src/deck/drivers/src/usddeck.c
index 8427843ad8..77a9c87b1d 100644
--- a/src/deck/drivers/src/usddeck.c
+++ b/src/deck/drivers/src/usddeck.c
@@ -90,8 +90,8 @@
 #define SPI_END_TRANSACTION     spiEndTransaction
 #endif
 
-#define MAX_USD_LOG_VARIABLES_PER_EVENT   (20)
-#define MAX_USD_LOG_EVENTS                (20)
+#define MAX_USD_LOG_VARIABLES_PER_EVENT   (40)
+#define MAX_USD_LOG_EVENTS                (5)
 #define FIXED_FREQUENCY_EVENT_ID          (0xFFFF)
 #define FIXED_FREQUENCY_EVENT_NAME        "fixedFrequency"
 
diff --git a/src/modules/interface/controller/controller.h b/src/modules/interface/controller/controller.h
index a720b4008a..4b1792a0fc 100644
--- a/src/modules/interface/controller/controller.h
+++ b/src/modules/interface/controller/controller.h
@@ -37,6 +37,7 @@ typedef enum {
 #ifdef CONFIG_CONTROLLER_OOT
   ControllerTypeOot,
 #endif
+  ControllerTypeLee,
   ControllerType_COUNT,
 } ControllerType;
 
diff --git a/src/modules/interface/controller/controller_lee.h b/src/modules/interface/controller/controller_lee.h
new file mode 100644
index 0000000000..9be2ba6cba
--- /dev/null
+++ b/src/modules/interface/controller/controller_lee.h
@@ -0,0 +1,79 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2016 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+#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; // TODO: should be CF global for other modules
+    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 vec qr;
+    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__
diff --git a/src/modules/interface/pptraj.h b/src/modules/interface/pptraj.h
index 568b299be4..22616ff93a 100644
--- a/src/modules/interface/pptraj.h
+++ b/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;
 };
diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h
index d49f852b6a..dff1eb361b 100644
--- a/src/modules/interface/stabilizer_types.h
+++ b/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 {
diff --git a/src/modules/src/controller/Kbuild b/src/modules/src/controller/Kbuild
index 8aebac4e89..33401b23d3 100644
--- a/src/modules/src/controller/Kbuild
+++ b/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
diff --git a/src/modules/src/controller/controller.c b/src/modules/src/controller/controller.c
index 827511c81f..0ef658a8ab 100644
--- a/src/modules/src/controller/controller.c
+++ b/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"},
 };
 
 
diff --git a/src/modules/src/controller/controller_lee.c b/src/modules/src/controller/controller_lee.c
new file mode 100644
index 0000000000..19a3d28911
--- /dev/null
+++ b/src/modules/src/controller/controller_lee.c
@@ -0,0 +1,386 @@
+/*
+The MIT License (MIT)
+
+Copyright (c) 2019 Wolfgang Hoenig
+
+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.
+*/
+
+/*
+This controller is based on the following publication:
+
+Taeyoung Lee, Melvin Leok, and N. Harris McClamroch
+Geometric Tracking Control of a Quadrotor UAV on SE(3)
+CDC 2010
+
+* Difference to Mellinger:
+  * Different angular velocity error
+  * Higher-order terms in attitude controller
+
+TODO:
+  * Switch position controller
+  * consider Omega_d dot (currently assumes this is zero)
+*/
+
+#include <math.h>
+#include <string.h>
+
+#include "math3d.h"
+#include "controller_lee.h"
+
+#define GRAVITY_MAGNITUDE (9.81f)
+
+static controllerLee_t g_self = {
+  .mass = 0.034, // TODO: should be CF global for other modules
+
+  // Inertia matrix (diagonal matrix), see
+  // System Identification of the Crazyflie 2.0 Nano Quadrocopter
+  // BA theses, Julian Foerster, ETHZ
+  // https://polybox.ethz.ch/index.php/s/20dde63ee00ffe7085964393a55a91c7
+  .J = {16.571710e-6, 16.655602e-6, 29.261652e-6}, // kg m^2
+
+  // Position PID
+  .Kpos_P = {9, 9, 9}, // Kp in paper
+  .Kpos_P_limit = 100,
+  .Kpos_D = {7, 7, 7}, // Kv in paper
+  .Kpos_D_limit = 100,
+  .Kpos_I = {5, 5, 5}, // not in paper
+  .Kpos_I_limit = 2,
+
+  // Attitude PID
+  .KR = {0.0055, 0.0055, 0.01},
+  .Komega = {0.0013, 0.0013, 0.002},
+  .KI = {0.012, 0.018, 0.015},
+};
+
+static inline struct vec vclampscl(struct vec value, float min, float max) {
+  return mkvec(
+    clamp(value.x, min, max),
+    clamp(value.y, min, max),
+    clamp(value.z, min, max));
+}
+
+void controllerLeeReset(controllerLee_t* self)
+{
+  self->i_error_pos = vzero();
+  self->i_error_att = vzero();
+}
+
+void controllerLeeInit(controllerLee_t* self)
+{
+  // copy default values (bindings), or NOP (firmware)
+  *self = g_self;
+
+  controllerLeeReset(self);
+}
+
+bool controllerLeeTest(controllerLee_t* self)
+{
+  return true;
+}
+
+void controllerLee(controllerLee_t* self, control_t *control, const setpoint_t *setpoint,
+                                         const sensorData_t *sensors,
+                                         const state_t *state,
+                                         const uint32_t tick)
+{
+
+
+  if (!RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
+    return;
+  }
+
+  // uint64_t startTime = usecTimestamp();
+
+  float dt = (float)(1.0f/ATTITUDE_RATE);
+  // struct vec dessnap = vzero();
+  // Address inconsistency in firmware where we need to compute our own desired yaw angle
+  // Rate-controlled YAW is moving YAW angle setpoint
+  float desiredYaw = 0; //rad
+  if (setpoint->mode.yaw == modeVelocity) {
+    desiredYaw = radians(state->attitude.yaw + setpoint->attitudeRate.yaw * dt);
+  } else if (setpoint->mode.yaw == modeAbs) {
+    desiredYaw = radians(setpoint->attitude.yaw);
+  } else if (setpoint->mode.quat == modeAbs) {
+    struct quat setpoint_quat = mkquat(setpoint->attitudeQuaternion.x, setpoint->attitudeQuaternion.y, setpoint->attitudeQuaternion.z, setpoint->attitudeQuaternion.w);
+    self->rpy_des = quat2rpy(setpoint_quat);
+    desiredYaw = self->rpy_des.z;
+  }
+
+  // Position controller
+  if (   setpoint->mode.x == modeAbs
+      || setpoint->mode.y == modeAbs
+      || setpoint->mode.z == modeAbs) {
+    struct vec pos_d = mkvec(setpoint->position.x, setpoint->position.y, setpoint->position.z);
+    struct vec vel_d = mkvec(setpoint->velocity.x, setpoint->velocity.y, setpoint->velocity.z);
+    struct vec acc_d = mkvec(setpoint->acceleration.x, setpoint->acceleration.y, setpoint->acceleration.z + GRAVITY_MAGNITUDE);
+    struct vec statePos = mkvec(state->position.x, state->position.y, state->position.z);
+    struct vec stateVel = mkvec(state->velocity.x, state->velocity.y, state->velocity.z);
+
+    // errors
+    struct vec pos_e = vclampscl(vsub(pos_d, statePos), -self->Kpos_P_limit, self->Kpos_P_limit);
+    struct vec vel_e = vclampscl(vsub(vel_d, stateVel), -self->Kpos_D_limit, self->Kpos_D_limit);
+    self->i_error_pos = vadd(self->i_error_pos, vscl(dt, pos_e));
+    self->p_error = pos_e;
+    self->v_error = vel_e;
+
+    struct vec F_d = vadd4(
+      acc_d,
+      veltmul(self->Kpos_D, vel_e),
+      veltmul(self->Kpos_P, pos_e),
+      veltmul(self->Kpos_I, self->i_error_pos));
+    
+   
+    struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w);
+    struct mat33 R = quat2rotmat(q);
+    struct vec z  = vbasis(2);
+    control->thrustSi = self->mass*vdot(F_d , mvmul(R, z));
+    self->thrustSi = control->thrustSi;
+    // Reset the accumulated error while on the ground
+    if (control->thrustSi < 0.01f) {
+      controllerLeeReset(self);
+    }
+
+  // Compute Desired Rotation matrix
+    float normFd = control->thrustSi;
+
+    struct vec xdes = vbasis(0);
+    struct vec ydes = vbasis(1);
+    struct vec zdes = vbasis(2);
+   
+    if (normFd > 0) {
+      zdes = vnormalize(F_d);
+    } 
+    struct vec xcdes = mkvec(cosf(desiredYaw), sinf(desiredYaw), 0); 
+    struct vec zcrossx = vcross(zdes, xcdes);
+    float normZX = vmag(zcrossx);
+
+    if (normZX > 0) {
+      ydes = vnormalize(zcrossx);
+    } 
+    xdes = vcross(ydes, zdes);
+    
+    self->R_des = mcolumns(xdes, ydes, zdes);
+
+  } else {
+    if (setpoint->mode.z == modeDisable) {
+      if (setpoint->thrust < 1000) {
+          control->controlMode = controlModeForceTorque;
+          control->thrustSi  = 0;
+          control->torque[0] = 0;
+          control->torque[1] = 0;
+          control->torque[2] = 0;
+          controllerLeeReset(self);
+          return;
+      }
+    }
+    // On CF2, thrust is mapped 65536 <==> 4 * 12 grams
+    const float max_thrust = 70.0f / 1000.0f * 9.81f; // N
+    control->thrustSi = setpoint->thrust / UINT16_MAX * max_thrust;
+
+   self->qr = mkvec(
+      radians(setpoint->attitude.roll),
+      -radians(setpoint->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted
+      desiredYaw);
+  }
+
+  // Attitude controller
+
+  // current rotation [R]
+  struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w);
+  self->rpy = quat2rpy(q);
+  struct mat33 R = quat2rotmat(q);
+
+  // desired rotation [Rdes]
+  struct quat q_des = mat2quat(self->R_des);
+  self->rpy_des = quat2rpy(q_des);
+
+  // rotation error
+  struct mat33 eRM = msub(mmul(mtranspose(self->R_des), R), mmul(mtranspose(R), self->R_des));
+
+  struct vec eR = vscl(0.5f, mkvec(eRM.m[2][1], eRM.m[0][2], eRM.m[1][0]));
+
+  // angular velocity
+  self->omega = mkvec(
+    radians(sensors->gyro.x),
+    radians(sensors->gyro.y),
+    radians(sensors->gyro.z));
+
+  // Compute desired omega
+  struct vec xdes = mcolumn(self->R_des, 0);
+  struct vec ydes = mcolumn(self->R_des, 1);
+  struct vec zdes = mcolumn(self->R_des, 2);
+  struct vec hw = vzero();
+  // Desired Jerk and snap for now are zeros vector
+  struct vec desJerk = mkvec(setpoint->jerk.x, setpoint->jerk.y, setpoint->jerk.z);
+  // struct vec desJerk = vzero();
+
+  if (control->thrustSi != 0) {
+    struct vec tmp = vsub(desJerk, vscl(vdot(zdes, desJerk), zdes));
+    hw = vscl(self->mass/control->thrustSi, tmp);
+  }
+  struct vec z_w = mkvec(0,0,1); 
+  float desiredYawRate = radians(setpoint->attitudeRate.yaw) * vdot(zdes,z_w);
+  struct vec omega_des = mkvec(-vdot(hw,ydes), vdot(hw,xdes), desiredYawRate);
+  
+  self->omega_r = mvmul(mmul(mtranspose(R), self->R_des), omega_des);
+
+  struct vec omega_error = vsub(self->omega, self->omega_r);
+  
+  // Integral part on angle
+  self->i_error_att = vadd(self->i_error_att, vscl(dt, eR));
+
+
+  // compute moments
+  // M = -kR eR - kw ew + w x Jw - J(w x wr)
+  self->u = vadd4(
+    vneg(veltmul(self->KR, eR)),
+    vneg(veltmul(self->Komega, omega_error)),
+    vneg(veltmul(self->KI, self->i_error_att)),
+    vcross(self->omega, veltmul(self->J, self->omega)));
+
+  // if (enableNN > 1) {
+  //   u = vsub(u, tau_a);
+  // }
+
+  control->controlMode = controlModeForceTorque;
+  control->torque[0] = self->u.x;
+  control->torque[1] = self->u.y;
+  control->torque[2] = self->u.z;
+
+  // ticks = usecTimestamp() - startTime;
+}
+
+#ifdef CRAZYFLIE_FW
+
+#include "param.h"
+#include "log.h"
+
+void controllerLeeFirmwareInit(void)
+{
+  controllerLeeInit(&g_self);
+}
+
+bool controllerLeeFirmwareTest(void)
+{
+  return true;
+}
+
+void controllerLeeFirmware(control_t *control, const setpoint_t *setpoint,
+                                         const sensorData_t *sensors,
+                                         const state_t *state,
+                                         const uint32_t tick)
+{
+  controllerLee(&g_self, control, setpoint, sensors, state, tick);
+}
+
+PARAM_GROUP_START(ctrlLee)
+PARAM_ADD(PARAM_FLOAT, KR_x, &g_self.KR.x)
+PARAM_ADD(PARAM_FLOAT, KR_y, &g_self.KR.y)
+PARAM_ADD(PARAM_FLOAT, KR_z, &g_self.KR.z)
+// Attitude I
+PARAM_ADD(PARAM_FLOAT, KI_x, &g_self.KI.x)
+PARAM_ADD(PARAM_FLOAT, KI_y, &g_self.KI.y)
+PARAM_ADD(PARAM_FLOAT, KI_z, &g_self.KI.z)
+// Attitude D
+PARAM_ADD(PARAM_FLOAT, Kw_x, &g_self.Komega.x)
+PARAM_ADD(PARAM_FLOAT, Kw_y, &g_self.Komega.y)
+PARAM_ADD(PARAM_FLOAT, Kw_z, &g_self.Komega.z)
+
+// J
+PARAM_ADD(PARAM_FLOAT, J_x, &g_self.J.x)
+PARAM_ADD(PARAM_FLOAT, J_y, &g_self.J.y)
+PARAM_ADD(PARAM_FLOAT, J_z, &g_self.J.z)
+
+// Position P
+PARAM_ADD(PARAM_FLOAT, Kpos_Px, &g_self.Kpos_P.x)
+PARAM_ADD(PARAM_FLOAT, Kpos_Py, &g_self.Kpos_P.y)
+PARAM_ADD(PARAM_FLOAT, Kpos_Pz, &g_self.Kpos_P.z)
+PARAM_ADD(PARAM_FLOAT, Kpos_P_limit, &g_self.Kpos_P_limit)
+// Position D
+PARAM_ADD(PARAM_FLOAT, Kpos_Dx, &g_self.Kpos_D.x)
+PARAM_ADD(PARAM_FLOAT, Kpos_Dy, &g_self.Kpos_D.y)
+PARAM_ADD(PARAM_FLOAT, Kpos_Dz, &g_self.Kpos_D.z)
+PARAM_ADD(PARAM_FLOAT, Kpos_D_limit, &g_self.Kpos_D_limit)
+// Position I
+PARAM_ADD(PARAM_FLOAT, Kpos_Ix, &g_self.Kpos_I.x)
+PARAM_ADD(PARAM_FLOAT, Kpos_Iy, &g_self.Kpos_I.y)
+PARAM_ADD(PARAM_FLOAT, Kpos_Iz, &g_self.Kpos_I.z)
+PARAM_ADD(PARAM_FLOAT, Kpos_I_limit, &g_self.Kpos_I_limit)
+
+PARAM_ADD(PARAM_FLOAT, mass, &g_self.mass)
+PARAM_GROUP_STOP(ctrlLee)
+
+
+LOG_GROUP_START(ctrlLee)
+
+LOG_ADD(LOG_FLOAT, KR_x, &g_self.KR.x)
+LOG_ADD(LOG_FLOAT, KR_y, &g_self.KR.y)
+LOG_ADD(LOG_FLOAT, KR_z, &g_self.KR.z)
+LOG_ADD(LOG_FLOAT, Kw_x, &g_self.Komega.x)
+LOG_ADD(LOG_FLOAT, Kw_y, &g_self.Komega.y)
+LOG_ADD(LOG_FLOAT, Kw_z, &g_self.Komega.z)
+
+LOG_ADD(LOG_FLOAT,Kpos_Px, &g_self.Kpos_P.x)
+LOG_ADD(LOG_FLOAT,Kpos_Py, &g_self.Kpos_P.y)
+LOG_ADD(LOG_FLOAT,Kpos_Pz, &g_self.Kpos_P.z)
+LOG_ADD(LOG_FLOAT,Kpos_Dx, &g_self.Kpos_D.x)
+LOG_ADD(LOG_FLOAT,Kpos_Dy, &g_self.Kpos_D.y)
+LOG_ADD(LOG_FLOAT,Kpos_Dz, &g_self.Kpos_D.z)
+
+
+LOG_ADD(LOG_FLOAT, thrustSi, &g_self.thrustSi)
+LOG_ADD(LOG_FLOAT, torquex, &g_self.u.x)
+LOG_ADD(LOG_FLOAT, torquey, &g_self.u.y)
+LOG_ADD(LOG_FLOAT, torquez, &g_self.u.z)
+
+// current angles
+LOG_ADD(LOG_FLOAT, rpyx, &g_self.rpy.x)
+LOG_ADD(LOG_FLOAT, rpyy, &g_self.rpy.y)
+LOG_ADD(LOG_FLOAT, rpyz, &g_self.rpy.z)
+
+// desired angles
+LOG_ADD(LOG_FLOAT, rpydx, &g_self.rpy_des.x)
+LOG_ADD(LOG_FLOAT, rpydy, &g_self.rpy_des.y)
+LOG_ADD(LOG_FLOAT, rpydz, &g_self.rpy_des.z)
+
+// errors
+LOG_ADD(LOG_FLOAT, error_posx, &g_self.p_error.x)
+LOG_ADD(LOG_FLOAT, error_posy, &g_self.p_error.y)
+LOG_ADD(LOG_FLOAT, error_posz, &g_self.p_error.z)
+
+LOG_ADD(LOG_FLOAT, error_velx, &g_self.v_error.x)
+LOG_ADD(LOG_FLOAT, error_vely, &g_self.v_error.y)
+LOG_ADD(LOG_FLOAT, error_velz, &g_self.v_error.z)
+
+// omega
+LOG_ADD(LOG_FLOAT, omegax, &g_self.omega.x)
+LOG_ADD(LOG_FLOAT, omegay, &g_self.omega.y)
+LOG_ADD(LOG_FLOAT, omegaz, &g_self.omega.z)
+
+// omega_r
+LOG_ADD(LOG_FLOAT, omegarx, &g_self.omega_r.x)
+LOG_ADD(LOG_FLOAT, omegary, &g_self.omega_r.y)
+LOG_ADD(LOG_FLOAT, omegarz, &g_self.omega_r.z)
+
+// LOG_ADD(LOG_UINT32, ticks, &ticks)
+
+LOG_GROUP_STOP(ctrlLee)
+
+#endif // CRAZYFLIE_FW defined
diff --git a/src/modules/src/crtp_commander_generic.c b/src/modules/src/crtp_commander_generic.c
index 6148d608ae..74e98d4730 100644
--- a/src/modules/src/crtp_commander_generic.c
+++ b/src/modules/src/crtp_commander_generic.c
@@ -353,7 +353,8 @@ static void fullStateDecoder(setpoint_t *setpoint, uint8_t type, const void *dat
   setpoint->position.x = values->x / 1000.0f; \
   setpoint->velocity.x = (values->v ## x) / 1000.0f; \
   setpoint->acceleration.x = (values->a ## x) / 1000.0f; \
-
+  setpoint->jerk.x = 0.0f; \
+  
   UNPACK(x)
   UNPACK(y)
   UNPACK(z)
diff --git a/src/modules/src/crtp_commander_high_level.c b/src/modules/src/crtp_commander_high_level.c
index 83161463f6..dee2ba9e81 100644
--- a/src/modules/src/crtp_commander_high_level.c
+++ b/src/modules/src/crtp_commander_high_level.c
@@ -351,6 +351,10 @@ bool crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *stat
     setpoint->acceleration.x = ev.acc.x;
     setpoint->acceleration.y = ev.acc.y;
     setpoint->acceleration.z = ev.acc.z;
+    setpoint->jerk.x = ev.jerk.x;
+    setpoint->jerk.y = ev.jerk.y;
+    setpoint->jerk.z = ev.jerk.z;
+
 
     // store the last setpoint
     pos = ev.pos;
diff --git a/src/modules/src/pptraj.c b/src/modules/src/pptraj.c
index d24dc2f93c..ea7c4be493 100644
--- a/src/modules/src/pptraj.c
+++ b/src/modules/src/pptraj.c
@@ -309,7 +309,7 @@ struct traj_eval poly4d_eval(struct poly4d const *p, float t)
 
 	// 3rd derivative
 	polyder4d(deriv);
-	struct vec jerk = polyval_xyz(deriv, t);
+	out.jerk = polyval_xyz(deriv, t);
 
 	struct vec thrust = vadd(out.acc, mkvec(0, 0, GRAV));
 	// float thrust_mag = mass * vmag(thrust);
@@ -319,7 +319,7 @@ struct traj_eval poly4d_eval(struct poly4d const *p, float t)
 	struct vec y_body = vnormalize(vcross(z_body, x_world));
 	struct vec x_body = vcross(y_body, z_body);
 
-	struct vec jerk_orth_zbody = vorthunit(jerk, z_body);
+	struct vec jerk_orth_zbody = vorthunit(out.jerk, z_body);
 	struct vec h_w = vscl(1.0f / vmag(thrust), jerk_orth_zbody);
 
 	out.omega.x = -vdot(h_w, y_body);
@@ -356,6 +356,7 @@ struct traj_eval piecewise_eval(
 	ev.pos = vadd(ev.pos, traj->shift);
 	ev.vel = vzero();
 	ev.acc = vzero();
+	ev.jerk = vzero();
 	ev.omega = vzero();
 	return ev;
 }
@@ -386,6 +387,7 @@ struct traj_eval piecewise_eval_reversed(
 	ev.pos = vadd(ev.pos, traj->shift);
 	ev.vel = vzero();
 	ev.acc = vzero();
+	ev.jerk = vzero();
 	ev.omega = vzero();
 	return ev;
 }
diff --git a/vendor/libdw1000 b/vendor/libdw1000
index a77610327f..448a0efde4 160000
--- a/vendor/libdw1000
+++ b/vendor/libdw1000
@@ -1 +1 @@
-Subproject commit a77610327fb875a69543f3a8d19507683b68aff1
+Subproject commit 448a0efde42a258139cc8ca61fe53ff3bb4a9b94

From 0640f1c22c4a540e28c055377b923c0cca04e9fb Mon Sep 17 00:00:00 2001
From: Dennis Schmidt <dennis.schmidt.0@web.de>
Date: Tue, 24 Oct 2023 11:03:18 +0200
Subject: [PATCH 02/14] dennis test lee success

---
 src/deck/drivers/src/usddeck.c                |   4 +-
 src/modules/interface/controller/controller.h |   1 +
 .../interface/controller/controller_lee.h     |  79 ++++
 src/modules/interface/pptraj.h                |   1 +
 src/modules/interface/stabilizer_types.h      |   2 +
 src/modules/src/controller/Kbuild             |   1 +
 src/modules/src/controller/controller.c       |   2 +
 src/modules/src/controller/controller_lee.c   | 386 ++++++++++++++++++
 src/modules/src/crtp_commander_generic.c      |   3 +-
 src/modules/src/crtp_commander_high_level.c   |   4 +
 src/modules/src/pptraj.c                      |   6 +-
 vendor/libdw1000                              |   2 +-
 12 files changed, 485 insertions(+), 6 deletions(-)
 create mode 100644 src/modules/interface/controller/controller_lee.h
 create mode 100644 src/modules/src/controller/controller_lee.c

diff --git a/src/deck/drivers/src/usddeck.c b/src/deck/drivers/src/usddeck.c
index 8427843ad8..77a9c87b1d 100644
--- a/src/deck/drivers/src/usddeck.c
+++ b/src/deck/drivers/src/usddeck.c
@@ -90,8 +90,8 @@
 #define SPI_END_TRANSACTION     spiEndTransaction
 #endif
 
-#define MAX_USD_LOG_VARIABLES_PER_EVENT   (20)
-#define MAX_USD_LOG_EVENTS                (20)
+#define MAX_USD_LOG_VARIABLES_PER_EVENT   (40)
+#define MAX_USD_LOG_EVENTS                (5)
 #define FIXED_FREQUENCY_EVENT_ID          (0xFFFF)
 #define FIXED_FREQUENCY_EVENT_NAME        "fixedFrequency"
 
diff --git a/src/modules/interface/controller/controller.h b/src/modules/interface/controller/controller.h
index a720b4008a..4b1792a0fc 100644
--- a/src/modules/interface/controller/controller.h
+++ b/src/modules/interface/controller/controller.h
@@ -37,6 +37,7 @@ typedef enum {
 #ifdef CONFIG_CONTROLLER_OOT
   ControllerTypeOot,
 #endif
+  ControllerTypeLee,
   ControllerType_COUNT,
 } ControllerType;
 
diff --git a/src/modules/interface/controller/controller_lee.h b/src/modules/interface/controller/controller_lee.h
new file mode 100644
index 0000000000..9be2ba6cba
--- /dev/null
+++ b/src/modules/interface/controller/controller_lee.h
@@ -0,0 +1,79 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2016 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+#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; // TODO: should be CF global for other modules
+    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 vec qr;
+    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__
diff --git a/src/modules/interface/pptraj.h b/src/modules/interface/pptraj.h
index 568b299be4..22616ff93a 100644
--- a/src/modules/interface/pptraj.h
+++ b/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;
 };
diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h
index d49f852b6a..dff1eb361b 100644
--- a/src/modules/interface/stabilizer_types.h
+++ b/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 {
diff --git a/src/modules/src/controller/Kbuild b/src/modules/src/controller/Kbuild
index 8aebac4e89..33401b23d3 100644
--- a/src/modules/src/controller/Kbuild
+++ b/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
diff --git a/src/modules/src/controller/controller.c b/src/modules/src/controller/controller.c
index 827511c81f..0ef658a8ab 100644
--- a/src/modules/src/controller/controller.c
+++ b/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"},
 };
 
 
diff --git a/src/modules/src/controller/controller_lee.c b/src/modules/src/controller/controller_lee.c
new file mode 100644
index 0000000000..19a3d28911
--- /dev/null
+++ b/src/modules/src/controller/controller_lee.c
@@ -0,0 +1,386 @@
+/*
+The MIT License (MIT)
+
+Copyright (c) 2019 Wolfgang Hoenig
+
+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.
+*/
+
+/*
+This controller is based on the following publication:
+
+Taeyoung Lee, Melvin Leok, and N. Harris McClamroch
+Geometric Tracking Control of a Quadrotor UAV on SE(3)
+CDC 2010
+
+* Difference to Mellinger:
+  * Different angular velocity error
+  * Higher-order terms in attitude controller
+
+TODO:
+  * Switch position controller
+  * consider Omega_d dot (currently assumes this is zero)
+*/
+
+#include <math.h>
+#include <string.h>
+
+#include "math3d.h"
+#include "controller_lee.h"
+
+#define GRAVITY_MAGNITUDE (9.81f)
+
+static controllerLee_t g_self = {
+  .mass = 0.034, // TODO: should be CF global for other modules
+
+  // Inertia matrix (diagonal matrix), see
+  // System Identification of the Crazyflie 2.0 Nano Quadrocopter
+  // BA theses, Julian Foerster, ETHZ
+  // https://polybox.ethz.ch/index.php/s/20dde63ee00ffe7085964393a55a91c7
+  .J = {16.571710e-6, 16.655602e-6, 29.261652e-6}, // kg m^2
+
+  // Position PID
+  .Kpos_P = {9, 9, 9}, // Kp in paper
+  .Kpos_P_limit = 100,
+  .Kpos_D = {7, 7, 7}, // Kv in paper
+  .Kpos_D_limit = 100,
+  .Kpos_I = {5, 5, 5}, // not in paper
+  .Kpos_I_limit = 2,
+
+  // Attitude PID
+  .KR = {0.0055, 0.0055, 0.01},
+  .Komega = {0.0013, 0.0013, 0.002},
+  .KI = {0.012, 0.018, 0.015},
+};
+
+static inline struct vec vclampscl(struct vec value, float min, float max) {
+  return mkvec(
+    clamp(value.x, min, max),
+    clamp(value.y, min, max),
+    clamp(value.z, min, max));
+}
+
+void controllerLeeReset(controllerLee_t* self)
+{
+  self->i_error_pos = vzero();
+  self->i_error_att = vzero();
+}
+
+void controllerLeeInit(controllerLee_t* self)
+{
+  // copy default values (bindings), or NOP (firmware)
+  *self = g_self;
+
+  controllerLeeReset(self);
+}
+
+bool controllerLeeTest(controllerLee_t* self)
+{
+  return true;
+}
+
+void controllerLee(controllerLee_t* self, control_t *control, const setpoint_t *setpoint,
+                                         const sensorData_t *sensors,
+                                         const state_t *state,
+                                         const uint32_t tick)
+{
+
+
+  if (!RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
+    return;
+  }
+
+  // uint64_t startTime = usecTimestamp();
+
+  float dt = (float)(1.0f/ATTITUDE_RATE);
+  // struct vec dessnap = vzero();
+  // Address inconsistency in firmware where we need to compute our own desired yaw angle
+  // Rate-controlled YAW is moving YAW angle setpoint
+  float desiredYaw = 0; //rad
+  if (setpoint->mode.yaw == modeVelocity) {
+    desiredYaw = radians(state->attitude.yaw + setpoint->attitudeRate.yaw * dt);
+  } else if (setpoint->mode.yaw == modeAbs) {
+    desiredYaw = radians(setpoint->attitude.yaw);
+  } else if (setpoint->mode.quat == modeAbs) {
+    struct quat setpoint_quat = mkquat(setpoint->attitudeQuaternion.x, setpoint->attitudeQuaternion.y, setpoint->attitudeQuaternion.z, setpoint->attitudeQuaternion.w);
+    self->rpy_des = quat2rpy(setpoint_quat);
+    desiredYaw = self->rpy_des.z;
+  }
+
+  // Position controller
+  if (   setpoint->mode.x == modeAbs
+      || setpoint->mode.y == modeAbs
+      || setpoint->mode.z == modeAbs) {
+    struct vec pos_d = mkvec(setpoint->position.x, setpoint->position.y, setpoint->position.z);
+    struct vec vel_d = mkvec(setpoint->velocity.x, setpoint->velocity.y, setpoint->velocity.z);
+    struct vec acc_d = mkvec(setpoint->acceleration.x, setpoint->acceleration.y, setpoint->acceleration.z + GRAVITY_MAGNITUDE);
+    struct vec statePos = mkvec(state->position.x, state->position.y, state->position.z);
+    struct vec stateVel = mkvec(state->velocity.x, state->velocity.y, state->velocity.z);
+
+    // errors
+    struct vec pos_e = vclampscl(vsub(pos_d, statePos), -self->Kpos_P_limit, self->Kpos_P_limit);
+    struct vec vel_e = vclampscl(vsub(vel_d, stateVel), -self->Kpos_D_limit, self->Kpos_D_limit);
+    self->i_error_pos = vadd(self->i_error_pos, vscl(dt, pos_e));
+    self->p_error = pos_e;
+    self->v_error = vel_e;
+
+    struct vec F_d = vadd4(
+      acc_d,
+      veltmul(self->Kpos_D, vel_e),
+      veltmul(self->Kpos_P, pos_e),
+      veltmul(self->Kpos_I, self->i_error_pos));
+    
+   
+    struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w);
+    struct mat33 R = quat2rotmat(q);
+    struct vec z  = vbasis(2);
+    control->thrustSi = self->mass*vdot(F_d , mvmul(R, z));
+    self->thrustSi = control->thrustSi;
+    // Reset the accumulated error while on the ground
+    if (control->thrustSi < 0.01f) {
+      controllerLeeReset(self);
+    }
+
+  // Compute Desired Rotation matrix
+    float normFd = control->thrustSi;
+
+    struct vec xdes = vbasis(0);
+    struct vec ydes = vbasis(1);
+    struct vec zdes = vbasis(2);
+   
+    if (normFd > 0) {
+      zdes = vnormalize(F_d);
+    } 
+    struct vec xcdes = mkvec(cosf(desiredYaw), sinf(desiredYaw), 0); 
+    struct vec zcrossx = vcross(zdes, xcdes);
+    float normZX = vmag(zcrossx);
+
+    if (normZX > 0) {
+      ydes = vnormalize(zcrossx);
+    } 
+    xdes = vcross(ydes, zdes);
+    
+    self->R_des = mcolumns(xdes, ydes, zdes);
+
+  } else {
+    if (setpoint->mode.z == modeDisable) {
+      if (setpoint->thrust < 1000) {
+          control->controlMode = controlModeForceTorque;
+          control->thrustSi  = 0;
+          control->torque[0] = 0;
+          control->torque[1] = 0;
+          control->torque[2] = 0;
+          controllerLeeReset(self);
+          return;
+      }
+    }
+    // On CF2, thrust is mapped 65536 <==> 4 * 12 grams
+    const float max_thrust = 70.0f / 1000.0f * 9.81f; // N
+    control->thrustSi = setpoint->thrust / UINT16_MAX * max_thrust;
+
+   self->qr = mkvec(
+      radians(setpoint->attitude.roll),
+      -radians(setpoint->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted
+      desiredYaw);
+  }
+
+  // Attitude controller
+
+  // current rotation [R]
+  struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w);
+  self->rpy = quat2rpy(q);
+  struct mat33 R = quat2rotmat(q);
+
+  // desired rotation [Rdes]
+  struct quat q_des = mat2quat(self->R_des);
+  self->rpy_des = quat2rpy(q_des);
+
+  // rotation error
+  struct mat33 eRM = msub(mmul(mtranspose(self->R_des), R), mmul(mtranspose(R), self->R_des));
+
+  struct vec eR = vscl(0.5f, mkvec(eRM.m[2][1], eRM.m[0][2], eRM.m[1][0]));
+
+  // angular velocity
+  self->omega = mkvec(
+    radians(sensors->gyro.x),
+    radians(sensors->gyro.y),
+    radians(sensors->gyro.z));
+
+  // Compute desired omega
+  struct vec xdes = mcolumn(self->R_des, 0);
+  struct vec ydes = mcolumn(self->R_des, 1);
+  struct vec zdes = mcolumn(self->R_des, 2);
+  struct vec hw = vzero();
+  // Desired Jerk and snap for now are zeros vector
+  struct vec desJerk = mkvec(setpoint->jerk.x, setpoint->jerk.y, setpoint->jerk.z);
+  // struct vec desJerk = vzero();
+
+  if (control->thrustSi != 0) {
+    struct vec tmp = vsub(desJerk, vscl(vdot(zdes, desJerk), zdes));
+    hw = vscl(self->mass/control->thrustSi, tmp);
+  }
+  struct vec z_w = mkvec(0,0,1); 
+  float desiredYawRate = radians(setpoint->attitudeRate.yaw) * vdot(zdes,z_w);
+  struct vec omega_des = mkvec(-vdot(hw,ydes), vdot(hw,xdes), desiredYawRate);
+  
+  self->omega_r = mvmul(mmul(mtranspose(R), self->R_des), omega_des);
+
+  struct vec omega_error = vsub(self->omega, self->omega_r);
+  
+  // Integral part on angle
+  self->i_error_att = vadd(self->i_error_att, vscl(dt, eR));
+
+
+  // compute moments
+  // M = -kR eR - kw ew + w x Jw - J(w x wr)
+  self->u = vadd4(
+    vneg(veltmul(self->KR, eR)),
+    vneg(veltmul(self->Komega, omega_error)),
+    vneg(veltmul(self->KI, self->i_error_att)),
+    vcross(self->omega, veltmul(self->J, self->omega)));
+
+  // if (enableNN > 1) {
+  //   u = vsub(u, tau_a);
+  // }
+
+  control->controlMode = controlModeForceTorque;
+  control->torque[0] = self->u.x;
+  control->torque[1] = self->u.y;
+  control->torque[2] = self->u.z;
+
+  // ticks = usecTimestamp() - startTime;
+}
+
+#ifdef CRAZYFLIE_FW
+
+#include "param.h"
+#include "log.h"
+
+void controllerLeeFirmwareInit(void)
+{
+  controllerLeeInit(&g_self);
+}
+
+bool controllerLeeFirmwareTest(void)
+{
+  return true;
+}
+
+void controllerLeeFirmware(control_t *control, const setpoint_t *setpoint,
+                                         const sensorData_t *sensors,
+                                         const state_t *state,
+                                         const uint32_t tick)
+{
+  controllerLee(&g_self, control, setpoint, sensors, state, tick);
+}
+
+PARAM_GROUP_START(ctrlLee)
+PARAM_ADD(PARAM_FLOAT, KR_x, &g_self.KR.x)
+PARAM_ADD(PARAM_FLOAT, KR_y, &g_self.KR.y)
+PARAM_ADD(PARAM_FLOAT, KR_z, &g_self.KR.z)
+// Attitude I
+PARAM_ADD(PARAM_FLOAT, KI_x, &g_self.KI.x)
+PARAM_ADD(PARAM_FLOAT, KI_y, &g_self.KI.y)
+PARAM_ADD(PARAM_FLOAT, KI_z, &g_self.KI.z)
+// Attitude D
+PARAM_ADD(PARAM_FLOAT, Kw_x, &g_self.Komega.x)
+PARAM_ADD(PARAM_FLOAT, Kw_y, &g_self.Komega.y)
+PARAM_ADD(PARAM_FLOAT, Kw_z, &g_self.Komega.z)
+
+// J
+PARAM_ADD(PARAM_FLOAT, J_x, &g_self.J.x)
+PARAM_ADD(PARAM_FLOAT, J_y, &g_self.J.y)
+PARAM_ADD(PARAM_FLOAT, J_z, &g_self.J.z)
+
+// Position P
+PARAM_ADD(PARAM_FLOAT, Kpos_Px, &g_self.Kpos_P.x)
+PARAM_ADD(PARAM_FLOAT, Kpos_Py, &g_self.Kpos_P.y)
+PARAM_ADD(PARAM_FLOAT, Kpos_Pz, &g_self.Kpos_P.z)
+PARAM_ADD(PARAM_FLOAT, Kpos_P_limit, &g_self.Kpos_P_limit)
+// Position D
+PARAM_ADD(PARAM_FLOAT, Kpos_Dx, &g_self.Kpos_D.x)
+PARAM_ADD(PARAM_FLOAT, Kpos_Dy, &g_self.Kpos_D.y)
+PARAM_ADD(PARAM_FLOAT, Kpos_Dz, &g_self.Kpos_D.z)
+PARAM_ADD(PARAM_FLOAT, Kpos_D_limit, &g_self.Kpos_D_limit)
+// Position I
+PARAM_ADD(PARAM_FLOAT, Kpos_Ix, &g_self.Kpos_I.x)
+PARAM_ADD(PARAM_FLOAT, Kpos_Iy, &g_self.Kpos_I.y)
+PARAM_ADD(PARAM_FLOAT, Kpos_Iz, &g_self.Kpos_I.z)
+PARAM_ADD(PARAM_FLOAT, Kpos_I_limit, &g_self.Kpos_I_limit)
+
+PARAM_ADD(PARAM_FLOAT, mass, &g_self.mass)
+PARAM_GROUP_STOP(ctrlLee)
+
+
+LOG_GROUP_START(ctrlLee)
+
+LOG_ADD(LOG_FLOAT, KR_x, &g_self.KR.x)
+LOG_ADD(LOG_FLOAT, KR_y, &g_self.KR.y)
+LOG_ADD(LOG_FLOAT, KR_z, &g_self.KR.z)
+LOG_ADD(LOG_FLOAT, Kw_x, &g_self.Komega.x)
+LOG_ADD(LOG_FLOAT, Kw_y, &g_self.Komega.y)
+LOG_ADD(LOG_FLOAT, Kw_z, &g_self.Komega.z)
+
+LOG_ADD(LOG_FLOAT,Kpos_Px, &g_self.Kpos_P.x)
+LOG_ADD(LOG_FLOAT,Kpos_Py, &g_self.Kpos_P.y)
+LOG_ADD(LOG_FLOAT,Kpos_Pz, &g_self.Kpos_P.z)
+LOG_ADD(LOG_FLOAT,Kpos_Dx, &g_self.Kpos_D.x)
+LOG_ADD(LOG_FLOAT,Kpos_Dy, &g_self.Kpos_D.y)
+LOG_ADD(LOG_FLOAT,Kpos_Dz, &g_self.Kpos_D.z)
+
+
+LOG_ADD(LOG_FLOAT, thrustSi, &g_self.thrustSi)
+LOG_ADD(LOG_FLOAT, torquex, &g_self.u.x)
+LOG_ADD(LOG_FLOAT, torquey, &g_self.u.y)
+LOG_ADD(LOG_FLOAT, torquez, &g_self.u.z)
+
+// current angles
+LOG_ADD(LOG_FLOAT, rpyx, &g_self.rpy.x)
+LOG_ADD(LOG_FLOAT, rpyy, &g_self.rpy.y)
+LOG_ADD(LOG_FLOAT, rpyz, &g_self.rpy.z)
+
+// desired angles
+LOG_ADD(LOG_FLOAT, rpydx, &g_self.rpy_des.x)
+LOG_ADD(LOG_FLOAT, rpydy, &g_self.rpy_des.y)
+LOG_ADD(LOG_FLOAT, rpydz, &g_self.rpy_des.z)
+
+// errors
+LOG_ADD(LOG_FLOAT, error_posx, &g_self.p_error.x)
+LOG_ADD(LOG_FLOAT, error_posy, &g_self.p_error.y)
+LOG_ADD(LOG_FLOAT, error_posz, &g_self.p_error.z)
+
+LOG_ADD(LOG_FLOAT, error_velx, &g_self.v_error.x)
+LOG_ADD(LOG_FLOAT, error_vely, &g_self.v_error.y)
+LOG_ADD(LOG_FLOAT, error_velz, &g_self.v_error.z)
+
+// omega
+LOG_ADD(LOG_FLOAT, omegax, &g_self.omega.x)
+LOG_ADD(LOG_FLOAT, omegay, &g_self.omega.y)
+LOG_ADD(LOG_FLOAT, omegaz, &g_self.omega.z)
+
+// omega_r
+LOG_ADD(LOG_FLOAT, omegarx, &g_self.omega_r.x)
+LOG_ADD(LOG_FLOAT, omegary, &g_self.omega_r.y)
+LOG_ADD(LOG_FLOAT, omegarz, &g_self.omega_r.z)
+
+// LOG_ADD(LOG_UINT32, ticks, &ticks)
+
+LOG_GROUP_STOP(ctrlLee)
+
+#endif // CRAZYFLIE_FW defined
diff --git a/src/modules/src/crtp_commander_generic.c b/src/modules/src/crtp_commander_generic.c
index 6148d608ae..74e98d4730 100644
--- a/src/modules/src/crtp_commander_generic.c
+++ b/src/modules/src/crtp_commander_generic.c
@@ -353,7 +353,8 @@ static void fullStateDecoder(setpoint_t *setpoint, uint8_t type, const void *dat
   setpoint->position.x = values->x / 1000.0f; \
   setpoint->velocity.x = (values->v ## x) / 1000.0f; \
   setpoint->acceleration.x = (values->a ## x) / 1000.0f; \
-
+  setpoint->jerk.x = 0.0f; \
+  
   UNPACK(x)
   UNPACK(y)
   UNPACK(z)
diff --git a/src/modules/src/crtp_commander_high_level.c b/src/modules/src/crtp_commander_high_level.c
index da61d6ba6f..7c0e3f4b9e 100644
--- a/src/modules/src/crtp_commander_high_level.c
+++ b/src/modules/src/crtp_commander_high_level.c
@@ -351,6 +351,10 @@ bool crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *stat
     setpoint->acceleration.x = ev.acc.x;
     setpoint->acceleration.y = ev.acc.y;
     setpoint->acceleration.z = ev.acc.z;
+    setpoint->jerk.x = ev.jerk.x;
+    setpoint->jerk.y = ev.jerk.y;
+    setpoint->jerk.z = ev.jerk.z;
+
 
     // store the last setpoint
     pos = ev.pos;
diff --git a/src/modules/src/pptraj.c b/src/modules/src/pptraj.c
index d24dc2f93c..ea7c4be493 100644
--- a/src/modules/src/pptraj.c
+++ b/src/modules/src/pptraj.c
@@ -309,7 +309,7 @@ struct traj_eval poly4d_eval(struct poly4d const *p, float t)
 
 	// 3rd derivative
 	polyder4d(deriv);
-	struct vec jerk = polyval_xyz(deriv, t);
+	out.jerk = polyval_xyz(deriv, t);
 
 	struct vec thrust = vadd(out.acc, mkvec(0, 0, GRAV));
 	// float thrust_mag = mass * vmag(thrust);
@@ -319,7 +319,7 @@ struct traj_eval poly4d_eval(struct poly4d const *p, float t)
 	struct vec y_body = vnormalize(vcross(z_body, x_world));
 	struct vec x_body = vcross(y_body, z_body);
 
-	struct vec jerk_orth_zbody = vorthunit(jerk, z_body);
+	struct vec jerk_orth_zbody = vorthunit(out.jerk, z_body);
 	struct vec h_w = vscl(1.0f / vmag(thrust), jerk_orth_zbody);
 
 	out.omega.x = -vdot(h_w, y_body);
@@ -356,6 +356,7 @@ struct traj_eval piecewise_eval(
 	ev.pos = vadd(ev.pos, traj->shift);
 	ev.vel = vzero();
 	ev.acc = vzero();
+	ev.jerk = vzero();
 	ev.omega = vzero();
 	return ev;
 }
@@ -386,6 +387,7 @@ struct traj_eval piecewise_eval_reversed(
 	ev.pos = vadd(ev.pos, traj->shift);
 	ev.vel = vzero();
 	ev.acc = vzero();
+	ev.jerk = vzero();
 	ev.omega = vzero();
 	return ev;
 }
diff --git a/vendor/libdw1000 b/vendor/libdw1000
index a77610327f..448a0efde4 160000
--- a/vendor/libdw1000
+++ b/vendor/libdw1000
@@ -1 +1 @@
-Subproject commit a77610327fb875a69543f3a8d19507683b68aff1
+Subproject commit 448a0efde42a258139cc8ca61fe53ff3bb4a9b94

From cb6b75d3a98bed55ea72f19480178eda532ab643 Mon Sep 17 00:00:00 2001
From: Wolfgang Hoenig <hoenig@tu-berlin.de>
Date: Mon, 22 Jan 2024 12:30:53 +0100
Subject: [PATCH 03/14] add Python bindings

---
 bindings/cffirmware.i              |  2 ++
 bindings/setup.py                  |  1 +
 test_python/test_controller_lee.py | 45 ++++++++++++++++++++++++++++++
 3 files changed, 48 insertions(+)
 create mode 100644 test_python/test_controller_lee.py

diff --git a/bindings/cffirmware.i b/bindings/cffirmware.i
index 289706ba69..3b8ea9a8d4 100755
--- a/bindings/cffirmware.i
+++ b/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"
diff --git a/bindings/setup.py b/bindings/setup.py
index a543e31c62..4f67015624 100644
--- a/bindings/setup.py
+++ b/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",
diff --git a/test_python/test_controller_lee.py b/test_python/test_controller_lee.py
new file mode 100644
index 0000000000..eed35560b7
--- /dev/null
+++ b/test_python/test_controller_lee.py
@@ -0,0 +1,45 @@
+#!/usr/bin/env python
+
+import cffirmware
+
+def test_controller_lee():
+
+    ctrl = cffirmware.controllerLee_t()
+
+    cffirmware.controllerLeeInit(ctrl)
+
+    control = cffirmware.control_t()
+    setpoint = cffirmware.setpoint_t()
+    setpoint.mode.z = cffirmware.modeAbs
+    setpoint.position.z = 0
+    setpoint.mode.x = cffirmware.modeVelocity
+    setpoint.velocity.x = 0
+    setpoint.mode.y = cffirmware.modeVelocity
+    setpoint.velocity.y = 0
+    setpoint.mode.yaw = cffirmware.modeVelocity
+    setpoint.attitudeRate.yaw = 0
+
+    state = cffirmware.state_t()
+    state.attitude.roll = 0
+    state.attitude.pitch = -0 # WARNING: This needs to be negated
+    state.attitude.yaw = 0
+    state.position.x = 0
+    state.position.y = 0
+    state.position.z = 0
+    state.velocity.x = 0
+    state.velocity.y = 0
+    state.velocity.z = 0
+
+    sensors = cffirmware.sensorData_t()
+    sensors.gyro.x = 0
+    sensors.gyro.y = 0
+    sensors.gyro.z = 0
+
+    step = 100
+
+    cffirmware.controllerLee(ctrl, control, setpoint,sensors,state,step)
+    assert control.controlMode == cffirmware.controlModeForceTorque
+    # control.thrust will be at a (tuned) hover-state
+    assert control.torqueX == 0
+    assert control.torqueY == 0
+    assert control.torqueZ == 0

From 61b1466553e3a8ae4b0ae67d4ce0daa40b3c6a87 Mon Sep 17 00:00:00 2001
From: Wolfgang Hoenig <hoenig@tu-berlin.de>
Date: Mon, 22 Jan 2024 12:31:21 +0100
Subject: [PATCH 04/14] move enum position for consistent controller ID

---
 src/modules/interface/controller/controller.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/interface/controller/controller.h b/src/modules/interface/controller/controller.h
index 4b1792a0fc..4854009b61 100644
--- a/src/modules/interface/controller/controller.h
+++ b/src/modules/interface/controller/controller.h
@@ -34,10 +34,10 @@ typedef enum {
   ControllerTypeMellinger,
   ControllerTypeINDI,
   ControllerTypeBrescianini,
+  ControllerTypeLee,
 #ifdef CONFIG_CONTROLLER_OOT
   ControllerTypeOot,
 #endif
-  ControllerTypeLee,
   ControllerType_COUNT,
 } ControllerType;
 

From e43c718b53e25fc6b848b31570c52d878df1a0a8 Mon Sep 17 00:00:00 2001
From: Wolfgang Hoenig <hoenig@tu-berlin.de>
Date: Tue, 23 Jan 2024 16:07:51 +0100
Subject: [PATCH 05/14] clean-up - needs flight tests

---
 src/modules/interface/power_distribution.h    |  7 ++++++
 src/modules/src/controller/controller_lee.c   | 23 +++++--------------
 src/modules/src/power_distribution_flapper.c  |  5 ++++
 .../src/power_distribution_quadrotor.c        |  5 ++++
 4 files changed, 23 insertions(+), 17 deletions(-)

diff --git a/src/modules/interface/power_distribution.h b/src/modules/interface/power_distribution.h
index 945d734a7f..07dd866f66 100644
--- a/src/modules/interface/power_distribution.h
+++ b/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__
diff --git a/src/modules/src/controller/controller_lee.c b/src/modules/src/controller/controller_lee.c
index 19a3d28911..0361e92ed8 100644
--- a/src/modules/src/controller/controller_lee.c
+++ b/src/modules/src/controller/controller_lee.c
@@ -1,7 +1,7 @@
 /*
 The MIT License (MIT)
 
-Copyright (c) 2019 Wolfgang Hoenig
+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
@@ -32,10 +32,6 @@ CDC 2010
 * Difference to Mellinger:
   * Different angular velocity error
   * Higher-order terms in attitude controller
-
-TODO:
-  * Switch position controller
-  * consider Omega_d dot (currently assumes this is zero)
 */
 
 #include <math.h>
@@ -43,11 +39,11 @@ CDC 2010
 
 #include "math3d.h"
 #include "controller_lee.h"
-
-#define GRAVITY_MAGNITUDE (9.81f)
+#include "physicalConstants.h"
+#include "power_distribution.h"
 
 static controllerLee_t g_self = {
-  .mass = 0.034, // TODO: should be CF global for other modules
+  .mass = CF_MASS,
 
   // Inertia matrix (diagonal matrix), see
   // System Identification of the Crazyflie 2.0 Nano Quadrocopter
@@ -157,7 +153,7 @@ void controllerLee(controllerLee_t* self, control_t *control, const setpoint_t *
       controllerLeeReset(self);
     }
 
-  // Compute Desired Rotation matrix
+    // Compute Desired Rotation matrix
     float normFd = control->thrustSi;
 
     struct vec xdes = vbasis(0);
@@ -190,8 +186,7 @@ void controllerLee(controllerLee_t* self, control_t *control, const setpoint_t *
           return;
       }
     }
-    // On CF2, thrust is mapped 65536 <==> 4 * 12 grams
-    const float max_thrust = 70.0f / 1000.0f * 9.81f; // N
+    const float max_thrust = powerDistributionGetMaxThrust(); // N
     control->thrustSi = setpoint->thrust / UINT16_MAX * max_thrust;
 
    self->qr = mkvec(
@@ -229,7 +224,6 @@ void controllerLee(controllerLee_t* self, control_t *control, const setpoint_t *
   struct vec hw = vzero();
   // Desired Jerk and snap for now are zeros vector
   struct vec desJerk = mkvec(setpoint->jerk.x, setpoint->jerk.y, setpoint->jerk.z);
-  // struct vec desJerk = vzero();
 
   if (control->thrustSi != 0) {
     struct vec tmp = vsub(desJerk, vscl(vdot(zdes, desJerk), zdes));
@@ -246,7 +240,6 @@ void controllerLee(controllerLee_t* self, control_t *control, const setpoint_t *
   // Integral part on angle
   self->i_error_att = vadd(self->i_error_att, vscl(dt, eR));
 
-
   // compute moments
   // M = -kR eR - kw ew + w x Jw - J(w x wr)
   self->u = vadd4(
@@ -255,10 +248,6 @@ void controllerLee(controllerLee_t* self, control_t *control, const setpoint_t *
     vneg(veltmul(self->KI, self->i_error_att)),
     vcross(self->omega, veltmul(self->J, self->omega)));
 
-  // if (enableNN > 1) {
-  //   u = vsub(u, tau_a);
-  // }
-
   control->controlMode = controlModeForceTorque;
   control->torque[0] = self->u.x;
   control->torque[1] = self->u.y;
diff --git a/src/modules/src/power_distribution_flapper.c b/src/modules/src/power_distribution_flapper.c
index 98529703c0..f21aab5d94 100644
--- a/src/modules/src/power_distribution_flapper.c
+++ b/src/modules/src/power_distribution_flapper.c
@@ -204,6 +204,11 @@ uint32_t powerDistributionGetIdleThrust() {
   return idleThrust;
 }
 
+float powerDistributionGetMaxThrust() {
+  // Unknown
+  ASSERT(false);
+}
+
 /**
  * Power distribution parameters
  */
diff --git a/src/modules/src/power_distribution_quadrotor.c b/src/modules/src/power_distribution_quadrotor.c
index c34de07d41..b16a6ba76f 100644
--- a/src/modules/src/power_distribution_quadrotor.c
+++ b/src/modules/src/power_distribution_quadrotor.c
@@ -170,6 +170,11 @@ uint32_t powerDistributionGetIdleThrust() {
   return idleThrust;
 }
 
+float powerDistributionGetMaxThrust() {
+  const uint16_t pwm = UINT16_MAX;
+  return pwmToThrustA * pwm * pwm + pwmToThrustB * pwm;
+}
+
 /**
  * Power distribution parameters
  */

From 1f167ea0260b982cb2d889da297dbc19b378b295 Mon Sep 17 00:00:00 2001
From: Dennis Schmidt <dennis.schmidt.0@web.de>
Date: Tue, 30 Jan 2024 13:44:31 +0100
Subject: [PATCH 06/14] add Lee controller gains

---
 src/modules/src/controller/controller_lee.c | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/modules/src/controller/controller_lee.c b/src/modules/src/controller/controller_lee.c
index 0361e92ed8..00bff9cd8e 100644
--- a/src/modules/src/controller/controller_lee.c
+++ b/src/modules/src/controller/controller_lee.c
@@ -52,17 +52,17 @@ static controllerLee_t g_self = {
   .J = {16.571710e-6, 16.655602e-6, 29.261652e-6}, // kg m^2
 
   // Position PID
-  .Kpos_P = {9, 9, 9}, // Kp in paper
+  .Kpos_P = {9.5, 9.5, 9.5}, // Kp in paper
   .Kpos_P_limit = 100,
-  .Kpos_D = {7, 7, 7}, // Kv in paper
+  .Kpos_D = {7.0, 7.0, 7.0}, // Kv in paper
   .Kpos_D_limit = 100,
-  .Kpos_I = {5, 5, 5}, // not in paper
+  .Kpos_I = {0.0, 0.0, 0.0}, // not in paper
   .Kpos_I_limit = 2,
 
   // Attitude PID
-  .KR = {0.0055, 0.0055, 0.01},
-  .Komega = {0.0013, 0.0013, 0.002},
-  .KI = {0.012, 0.018, 0.015},
+  .KR = {0.0076, 0.0076, 0.008},
+  .Komega = {0.00115, 0.00115 0.002},
+  .KI = {0.03, 0.03, 0.03},
 };
 
 static inline struct vec vclampscl(struct vec value, float min, float max) {

From f3dc92e4e54d9e423c8cfae8a3656f60db958557 Mon Sep 17 00:00:00 2001
From: Wolfgang Hoenig <hoenig@tu-berlin.de>
Date: Tue, 30 Jan 2024 13:53:44 +0100
Subject: [PATCH 07/14] code cleanup

---
 src/deck/drivers/src/usddeck.c                |  4 +-
 .../interface/controller/controller_lee.h     | 50 +++++++++----------
 src/modules/src/controller/controller_lee.c   |  6 +--
 3 files changed, 28 insertions(+), 32 deletions(-)

diff --git a/src/deck/drivers/src/usddeck.c b/src/deck/drivers/src/usddeck.c
index 77a9c87b1d..8427843ad8 100644
--- a/src/deck/drivers/src/usddeck.c
+++ b/src/deck/drivers/src/usddeck.c
@@ -90,8 +90,8 @@
 #define SPI_END_TRANSACTION     spiEndTransaction
 #endif
 
-#define MAX_USD_LOG_VARIABLES_PER_EVENT   (40)
-#define MAX_USD_LOG_EVENTS                (5)
+#define MAX_USD_LOG_VARIABLES_PER_EVENT   (20)
+#define MAX_USD_LOG_EVENTS                (20)
 #define FIXED_FREQUENCY_EVENT_ID          (0xFFFF)
 #define FIXED_FREQUENCY_EVENT_NAME        "fixedFrequency"
 
diff --git a/src/modules/interface/controller/controller_lee.h b/src/modules/interface/controller/controller_lee.h
index 9be2ba6cba..b044bf2c7b 100644
--- a/src/modules/interface/controller/controller_lee.h
+++ b/src/modules/interface/controller/controller_lee.h
@@ -1,27 +1,26 @@
-/**
- *    ||          ____  _ __
- * +------+      / __ )(_) /_______________ _____  ___
- * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
- * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
- *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
- *
- * Crazyflie control firmware
- *
- * Copyright (C) 2011-2016 Bitcraze AB
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, in version 3.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- */
+/*
+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__
 
@@ -29,7 +28,7 @@
 
 // This structure contains the mutable state and inmutable parameters
 typedef struct controllerLee_s {
-    float mass; // TODO: should be CF global for other modules
+    float mass;
     float thrustSi;
     struct vec J; // Inertia matrix (diagonal matrix); kg m^2
 
@@ -59,7 +58,6 @@ typedef struct controllerLee_s {
 } controllerLee_t;
 
 
-
 void controllerLeeInit(controllerLee_t* self);
 void controllerLeeReset(controllerLee_t* self);
 void controllerLee(controllerLee_t* self, control_t *control, const setpoint_t *setpoint,
diff --git a/src/modules/src/controller/controller_lee.c b/src/modules/src/controller/controller_lee.c
index 00bff9cd8e..9918ed3980 100644
--- a/src/modules/src/controller/controller_lee.c
+++ b/src/modules/src/controller/controller_lee.c
@@ -61,7 +61,7 @@ static controllerLee_t g_self = {
 
   // Attitude PID
   .KR = {0.0076, 0.0076, 0.008},
-  .Komega = {0.00115, 0.00115 0.002},
+  .Komega = {0.00115, 0.00115, 0.002},
   .KI = {0.03, 0.03, 0.03},
 };
 
@@ -97,7 +97,6 @@ void controllerLee(controllerLee_t* self, control_t *control, const setpoint_t *
                                          const uint32_t tick)
 {
 
-
   if (!RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
     return;
   }
@@ -141,8 +140,7 @@ void controllerLee(controllerLee_t* self, control_t *control, const setpoint_t *
       veltmul(self->Kpos_D, vel_e),
       veltmul(self->Kpos_P, pos_e),
       veltmul(self->Kpos_I, self->i_error_pos));
-    
-   
+
     struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w);
     struct mat33 R = quat2rotmat(q);
     struct vec z  = vbasis(2);

From 37cecdc887bd2d5c892268b119cc08d7607e0790 Mon Sep 17 00:00:00 2001
From: Wolfgang Hoenig <hoenig@tu-berlin.de>
Date: Tue, 30 Jan 2024 14:19:15 +0100
Subject: [PATCH 08/14] fix thrust

---
 src/modules/src/controller/controller_lee.c    | 8 ++++----
 src/modules/src/power_distribution_quadrotor.c | 7 +++++--
 src/modules/src/stabilizer.c                   | 2 +-
 3 files changed, 10 insertions(+), 7 deletions(-)

diff --git a/src/modules/src/controller/controller_lee.c b/src/modules/src/controller/controller_lee.c
index 9918ed3980..d5ce6036aa 100644
--- a/src/modules/src/controller/controller_lee.c
+++ b/src/modules/src/controller/controller_lee.c
@@ -187,10 +187,10 @@ void controllerLee(controllerLee_t* self, control_t *control, const setpoint_t *
     const float max_thrust = powerDistributionGetMaxThrust(); // N
     control->thrustSi = setpoint->thrust / UINT16_MAX * max_thrust;
 
-   self->qr = mkvec(
-      radians(setpoint->attitude.roll),
-      -radians(setpoint->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted
-      desiredYaw);
+    self->qr = mkvec(
+        radians(setpoint->attitude.roll),
+        -radians(setpoint->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted
+        desiredYaw);
   }
 
   // Attitude controller
diff --git a/src/modules/src/power_distribution_quadrotor.c b/src/modules/src/power_distribution_quadrotor.c
index b16a6ba76f..c37b7448ed 100644
--- a/src/modules/src/power_distribution_quadrotor.c
+++ b/src/modules/src/power_distribution_quadrotor.c
@@ -45,6 +45,8 @@ static float armLength = 0.046f; // m;
 static float thrustToTorque = 0.005964552f;
 
 // thrust = a * pwm^2 + b * pwm
+//    where PWM is normalized (range 0...1)
+//          thrust is in Newtons (per rotor)
 static float pwmToThrustA = 0.091492681f;
 static float pwmToThrustB = 0.067673604f;
 
@@ -171,8 +173,9 @@ uint32_t powerDistributionGetIdleThrust() {
 }
 
 float powerDistributionGetMaxThrust() {
-  const uint16_t pwm = UINT16_MAX;
-  return pwmToThrustA * pwm * pwm + pwmToThrustB * pwm;
+  // max thrust per rotor occurs if normalized PWM is 1
+  // pwmToThrustA * pwm * pwm + pwmToThrustB * pwm = pwmToThrustA + pwmToThrustB
+  return STABILIZER_NR_OF_MOTORS * (pwmToThrustA + pwmToThrustB);
 }
 
 /**
diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c
index 49d4fd1a08..0a3c1d8cfd 100644
--- a/src/modules/src/stabilizer.c
+++ b/src/modules/src/stabilizer.c
@@ -367,7 +367,7 @@ PARAM_GROUP_START(stabilizer)
  */
 PARAM_ADD_CORE(PARAM_UINT8, estimator, &estimatorType)
 /**
- * @brief Controller type Auto select(0), PID(1), Mellinger(2), INDI(3), Brescianini(4) (Default: 0)
+ * @brief Controller type Auto select(0), PID(1), Mellinger(2), INDI(3), Brescianini(4), Lee(5) (Default: 0)
  */
 PARAM_ADD_CORE(PARAM_UINT8, controller, &controllerType)
 PARAM_GROUP_STOP(stabilizer)

From 82e25d63933424f90e065a21cca1f5a274d452a3 Mon Sep 17 00:00:00 2001
From: Khaledwahba1994 <Khaled.hassan1994@gmail.com>
Date: Tue, 30 Jan 2024 14:34:29 +0100
Subject: [PATCH 09/14] fix attitude manual flight

---
 src/modules/interface/controller/controller_lee.h | 1 -
 src/modules/src/controller/controller_lee.c       | 5 +++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/modules/interface/controller/controller_lee.h b/src/modules/interface/controller/controller_lee.h
index b044bf2c7b..0d64dfac2c 100644
--- a/src/modules/interface/controller/controller_lee.h
+++ b/src/modules/interface/controller/controller_lee.h
@@ -50,7 +50,6 @@ typedef struct controllerLee_s {
     // Logging variables
     struct vec rpy;
     struct vec rpy_des;
-    struct vec qr;
     struct mat33 R_des;
     struct vec omega;
     struct vec omega_r;
diff --git a/src/modules/src/controller/controller_lee.c b/src/modules/src/controller/controller_lee.c
index d5ce6036aa..664813ec57 100644
--- a/src/modules/src/controller/controller_lee.c
+++ b/src/modules/src/controller/controller_lee.c
@@ -187,10 +187,11 @@ void controllerLee(controllerLee_t* self, control_t *control, const setpoint_t *
     const float max_thrust = powerDistributionGetMaxThrust(); // N
     control->thrustSi = setpoint->thrust / UINT16_MAX * max_thrust;
 
-    self->qr = mkvec(
+    struct quat q = rpy2quat(mkvec(
         radians(setpoint->attitude.roll),
         -radians(setpoint->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted
-        desiredYaw);
+        desiredYaw));
+    self->R_des = quat2rotmat(q);
   }
 
   // Attitude controller

From c9ffb0d1ed04d403224f42238ce424313a9e16a5 Mon Sep 17 00:00:00 2001
From: Wolfgang Hoenig <hoenig@tu-berlin.de>
Date: Tue, 30 Jan 2024 15:58:46 +0100
Subject: [PATCH 10/14] Lee: address build error for flapper

---
 src/modules/src/power_distribution_flapper.c | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/modules/src/power_distribution_flapper.c b/src/modules/src/power_distribution_flapper.c
index f21aab5d94..b50868bb55 100644
--- a/src/modules/src/power_distribution_flapper.c
+++ b/src/modules/src/power_distribution_flapper.c
@@ -207,6 +207,7 @@ uint32_t powerDistributionGetIdleThrust() {
 float powerDistributionGetMaxThrust() {
   // Unknown
   ASSERT(false);
+  return 0.0f;
 }
 
 /**

From 39ed21b43f0ba74bd1a71ea566ed919fa900fedd Mon Sep 17 00:00:00 2001
From: Wolfgang Hoenig <hoenig@tu-berlin.de>
Date: Fri, 23 Feb 2024 21:15:31 +0100
Subject: [PATCH 11/14] revert libdw1000 change

---
 vendor/libdw1000 | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/vendor/libdw1000 b/vendor/libdw1000
index 448a0efde4..a77610327f 160000
--- a/vendor/libdw1000
+++ b/vendor/libdw1000
@@ -1 +1 @@
-Subproject commit 448a0efde42a258139cc8ca61fe53ff3bb4a9b94
+Subproject commit a77610327fb875a69543f3a8d19507683b68aff1

From aff418c3e492366fa8150f7261dc515cf7b96529 Mon Sep 17 00:00:00 2001
From: Wolfgang Hoenig <hoenig@tu-berlin.de>
Date: Fri, 23 Feb 2024 21:48:00 +0100
Subject: [PATCH 12/14] update docs

---
 .../sensor-to-control/controllers.md          | 58 +++++++++++++++++--
 1 file changed, 54 insertions(+), 4 deletions(-)

diff --git a/docs/functional-areas/sensor-to-control/controllers.md b/docs/functional-areas/sensor-to-control/controllers.md
index 1608f4a203..e7684186fe 100644
--- a/docs/functional-areas/sensor-to-control/controllers.md
+++ b/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.

From c4db17adcbaa03b2152274d8e6b10fc2d8cab4af Mon Sep 17 00:00:00 2001
From: Wolfgang Hoenig <hoenig@tu-berlin.de>
Date: Fri, 23 Feb 2024 22:02:26 +0100
Subject: [PATCH 13/14] fix build error

---
 src/modules/src/controller/controller_lee.c | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/modules/src/controller/controller_lee.c b/src/modules/src/controller/controller_lee.c
index 664813ec57..8c4d88931b 100644
--- a/src/modules/src/controller/controller_lee.c
+++ b/src/modules/src/controller/controller_lee.c
@@ -41,6 +41,7 @@ CDC 2010
 #include "controller_lee.h"
 #include "physicalConstants.h"
 #include "power_distribution.h"
+#include "platform_defaults.h"
 
 static controllerLee_t g_self = {
   .mass = CF_MASS,

From 82682361960ac9f31f0b48196145571cebfb5b36 Mon Sep 17 00:00:00 2001
From: Wolfgang Hoenig <hoenig@tu-berlin.de>
Date: Tue, 27 Feb 2024 16:30:15 +0100
Subject: [PATCH 14/14] lower gains for Lighthouse

---
 src/modules/src/controller/controller_lee.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/modules/src/controller/controller_lee.c b/src/modules/src/controller/controller_lee.c
index 8c4d88931b..bfde9de410 100644
--- a/src/modules/src/controller/controller_lee.c
+++ b/src/modules/src/controller/controller_lee.c
@@ -53,15 +53,15 @@ static controllerLee_t g_self = {
   .J = {16.571710e-6, 16.655602e-6, 29.261652e-6}, // kg m^2
 
   // Position PID
-  .Kpos_P = {9.5, 9.5, 9.5}, // Kp in paper
+  .Kpos_P = {7.0, 7.0, 7.0}, // Kp in paper
   .Kpos_P_limit = 100,
-  .Kpos_D = {7.0, 7.0, 7.0}, // Kv in paper
+  .Kpos_D = {4.0, 4.0, 4.0}, // Kv in paper
   .Kpos_D_limit = 100,
   .Kpos_I = {0.0, 0.0, 0.0}, // not in paper
   .Kpos_I_limit = 2,
 
   // Attitude PID
-  .KR = {0.0076, 0.0076, 0.008},
+  .KR = {0.007, 0.007, 0.008},
   .Komega = {0.00115, 0.00115, 0.002},
   .KI = {0.03, 0.03, 0.03},
 };