diff --git a/.vscode/settings.json b/.vscode/settings.json
index 605386e7e4..34f4c41133 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -5,6 +5,8 @@
         "**/bower_components": true
     },
     "files.associations": {
-        "bootloader.h": "c"
-    }
+        "bootloader.h": "c",
+        "controller_lee.h": "c"
+    },
+    "C_Cpp.errorSquiggles": "Disabled"
 }
diff --git a/bindings/cffirmware.i b/bindings/cffirmware.i
index 667769fa41..743307ace4 100755
--- a/bindings/cffirmware.i
+++ b/bindings/cffirmware.i
@@ -1,6 +1,46 @@
 %module cffirmware
 %include <stdint.i>
 
+%include "cpointer.i"
+%include "carrays.i"
+// %typemap(in) float[ANY] (float temp[$1_dim0]) {
+//   int i;
+//   if (!PySequence_Check($input)) {
+//     PyErr_SetString(PyExc_ValueError, "Expected a sequence");
+//     SWIG_fail;
+//   }
+//   if (PySequence_Length($input) != $1_dim0) {
+//     PyErr_SetString(PyExc_ValueError, "Size mismatch. Expected $1_dim0 elements");
+//     SWIG_fail;
+//   }
+//   for (i = 0; i < $1_dim0; i++) {
+//     PyObject *o = PySequence_GetItem($input, i);
+//     if (PyNumber_Check(o)) {
+//       temp[i] = (float) PyFloat_AsDouble(o);
+//     } else {
+//       PyErr_SetString(PyExc_ValueError, "Sequence elements must be numbers");      
+//       SWIG_fail;
+//     }
+//   }
+//   $1 = temp;
+// }
+
+%typemap(memberin) float [ANY] {
+  int i;
+  for (i = 0; i < $1_dim0; i++) {
+      $1[i] = $input[i];
+  }
+}
+
+%typemap(out) float [ANY] {
+  int i;
+  $result = PyList_New($1_dim0);
+  for (i = 0; i < $1_dim0; i++) {
+    PyObject *o = PyFloat_FromDouble((double) $1[i]);
+    PyList_SetItem($result,i,o);
+  }
+}
+
 // ignore GNU specific compiler attributes
 #define __attribute__(x)
 
@@ -19,6 +59,9 @@
 #include "num.h"
 #include "controller_mellinger.h"
 #include "power_distribution.h"
+#include "controller_sjc.h"
+#include "controller_lee.h"
+#include "controller_lee_payload.h"
 %}
 
 %include "math3d.h"
@@ -30,6 +73,9 @@
 %include "imu_types.h"
 %include "controller_mellinger.h"
 %include "power_distribution.h"
+%include "controller_sjc.h"
+%include "controller_lee.h"
+%include "controller_lee_payload.h"
 
 %inline %{
 struct poly4d* piecewise_get(struct piecewise_traj *pp, int i)
diff --git a/bindings/setup.py b/bindings/setup.py
index 6b7c47e50b..42824f2102 100644
--- a/bindings/setup.py
+++ b/bindings/setup.py
@@ -29,6 +29,9 @@
     "src/modules/src/controller_mellinger.c",
     "src/modules/src/power_distribution_quadrotor.c",
     # "src/modules/src/power_distribution_flapper.c",
+    "src/modules/src/controller_sjc.c",
+    "src/modules/src/controller_lee.c",
+    "src/modules/src/controller_lee_payload.c",
 ]
 
 cffirmware = Extension(
diff --git a/configs/cf2_defconfig b/configs/cf2_defconfig
index a8ff4d6600..2edbe72037 100644
--- a/configs/cf2_defconfig
+++ b/configs/cf2_defconfig
@@ -1 +1,4 @@
 CONFIG_PLATFORM_CF2=y
+CONFIG_APP_ENABLE=y
+CONFIG_APP_PRIORITY=1
+CONFIG_APP_STACKSIZE=350
diff --git a/src/deck/drivers/src/acs37800.c b/src/deck/drivers/src/acs37800.c
new file mode 100644
index 0000000000..19ebd0ecc4
--- /dev/null
+++ b/src/deck/drivers/src/acs37800.c
@@ -0,0 +1,347 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2021 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/>.
+ *
+ * asc37800.c - Deck driver for ACS37800 power monitoring IC
+ */
+
+#include <stdint.h>
+#include <stdlib.h>
+#include "stm32fxxx.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+#include "timers.h"
+
+#include "debug.h"
+#include "system.h"
+#include "deck.h"
+#include "param.h"
+#include "log.h"
+#include "sleepus.h"
+#include "i2cdev.h"
+
+
+#define I_RANGE           30.0f // Sensor amp range
+#define V_DIVIDER         92  // (91k + 1k) / 1k
+#define ADC_RANGE         0.250f
+#define CODES_HALF_RANGE  27500.0f
+#define CODES_FULL_RANGE  55000.0f
+#define MILLW_TO_WATT     1000.0f
+#define LSB_PER_MILLWATT  3.08f
+
+// EEPROM
+#define ACSREG_E_TRIM         0x0B
+#define ACSREG_E_OFFS_AVG     0x0C
+#define ACSREG_E_FAULT        0x0D
+#define ACSREG_E_UNDR_OVR_V   0x0E
+#define ACSREG_E_IO_SEL       0x0F
+
+// SHADOW
+#define ACSREG_S_TRIM         0x1B
+#define ACSREG_S_OFFS_AVG     0x1C
+#define ACSREG_S_FAULT        0x1D
+#define ACSREG_S_UNDR_OVR_V   0x1E
+#define ACSREG_S_IO_SEL       0x1F
+
+#define ACSREG_I_V_RMS      0x20
+#define ACSREG_P_ACTIVE     0x21
+#define ACSREG_P_APPARENT   0x22
+#define ACSREG_NUM_PTS_OUT  0x25
+#define ACSREG_VRMS_AVGS    0x26
+#define ACSREG_VRMS_AVGM    0x27
+#define ACSREG_P_ACT_AVGS   0x28
+#define ACSREG_P_ACT_AVGM   0x29
+#define ACSREG_I_V_CODES    0x2A
+#define ACSREG_P_INSTANT    0x2C
+#define ACSREG_DSP_STATUS   0x2D
+#define ACSREG_ACCESS_CODE  0x2F
+
+#define ACS_I2C_ADDR        0x7F
+#define ACS_ACCESS_CODE     0x4F70656E
+
+
+static bool isInit;
+static uint32_t viBatRaw;
+static uint32_t viBatRMS;
+static uint32_t vavgBatRMS;
+static float vBat;
+static float vBatRMS;
+//static float vavgBat;
+static float iBat;
+static float iBatRMS;
+static float iavgBat;
+static uint32_t pBatRaw;
+static uint32_t pavgBatRaw;
+static float pBat;
+static float pavgBat;
+// "milli" representation to save logging bits
+static int16_t vBatMV;
+static int16_t iBatMA;
+static int16_t pBatMW;
+
+
+static uint16_t currZtrim, currZtrimOld;
+static uint8_t writeTrim;
+
+static void asc37800Task(void* prm);
+
+/*
+ * Sign extend a bitfield which if right justified
+ *
+ *    data        - the bitfield to be sign extended
+ *    width       - the width of the bitfield
+ *    returns     - the sign extended bitfield
+ */
+int32_t signExtendBitfield(uint32_t data, uint16_t width)
+{
+  // If the bitfield is the width of the variable, don't bother trying to sign extend (it already is)
+    if (width == 32)
+    {
+        return (int32_t)data;
+    }
+
+    int32_t x = (int32_t)data;
+    int32_t mask = 1L << (width - 1);
+
+    x = x & ((1 << width) - 1); // make sure the upper bits are zero
+
+    return (int32_t)((x ^ mask) - mask);
+}
+
+/*
+ * Convert an unsigned bitfield which is right justified, into a floating point number
+ *
+ *    data        - the bitfield to be converted
+ *    binaryPoint - the binary point (the bit to the left of the binary point)
+ *    width       - the width of the bitfield
+ *    returns     - the floating point number
+ */
+float convertUnsignedFixedPoint(uint32_t inputValue, uint16_t binaryPoint, uint16_t width)
+{
+    uint32_t mask;
+
+    if (width == 32)
+    {
+        mask = 0xFFFFFFFF;
+    }
+    else
+    {
+        mask = (1UL << width) - 1UL;
+    }
+
+    return (float)(inputValue & mask) / (float)(1L << binaryPoint);
+}
+
+/*
+ * Convert a signed bitfield which is right justified, into a floating point number
+ *
+ *    data        - the bitfield to be sign extended then converted
+ *    binaryPoint - the binary point (the bit to the left of the binary point)
+ *    width       - the width of the bitfield
+ *    returns     - the floating point number
+ */
+float convertSignedFixedPoint(uint32_t inputValue, uint16_t binaryPoint, uint16_t width)
+{
+    int32_t signedValue = signExtendBitfield(inputValue, width);
+    return (float)signedValue / (float)(1L << binaryPoint);
+}
+
+static bool asc37800Read32(uint8_t reg, uint32_t *data32)
+{
+  return i2cdevReadReg8(I2C1_DEV, ACS_I2C_ADDR, reg, 4, (uint8_t *)data32);
+}
+
+static bool asc37800Write32(uint8_t reg, uint32_t data32)
+{
+  return i2cdevWriteReg8(I2C1_DEV, ACS_I2C_ADDR, reg, 4, (uint8_t *)&data32);
+}
+
+static void ascFindAndSetAddress(void)
+{
+  uint8_t startAddr;
+  uint32_t dummy = 0;
+
+  for (startAddr = 96; startAddr <= 110; startAddr++)
+  {
+    bool isReplying = i2cdevWrite(I2C1_DEV, startAddr, 1, (uint8_t *)&dummy);
+
+    if (isReplying)
+    {
+      // Unlock EEPROM
+      dummy = ACS_ACCESS_CODE;
+      i2cdevWriteReg8(I2C1_DEV, startAddr, ACSREG_ACCESS_CODE, 4, (uint8_t *)&dummy);
+      // EEPROM: write and lock i2c address to 0x7F;
+      i2cdevReadReg8(I2C1_DEV, startAddr, ACSREG_E_IO_SEL, 4, (uint8_t *)&dummy);
+      DEBUG_PRINT("ACS37800 A:0x%.2X R:0x%.2X:%X\n", (unsigned int)startAddr, (unsigned int)ACSREG_E_IO_SEL, (unsigned int)dummy);
+
+      dummy = (0x7F << 2) | (0x01 << 9);
+      i2cdevWriteReg8(I2C1_DEV, startAddr, ACSREG_E_IO_SEL, 4, (uint8_t *)&dummy);
+      vTaskDelay(M2T(10));
+
+      DEBUG_PRINT("ACS37800 found on: %d. Setting address to 0x7E. Power cycle needed!\n", startAddr);
+    }
+  }
+}
+
+
+static void asc37800Init(DeckInfo *info)
+{
+  uint8_t dummy;
+  uint32_t val;
+
+  if (isInit) {
+    return;
+  }
+
+  if (i2cdevWrite(I2C1_DEV, ACS_I2C_ADDR, 1, (uint8_t *)&dummy))
+  {
+    asc37800Write32(ACSREG_ACCESS_CODE, ACS_ACCESS_CODE);
+    DEBUG_PRINT("ACS37800 I2C [OK]\n");
+  }
+  else
+  {
+    ascFindAndSetAddress();
+  }
+
+  // Enable bypass in shadow reg and set N to 32.
+  asc37800Write32(ACSREG_S_IO_SEL, (1 << 24) | (32 << 14) | (0x01 << 9) |  (0x7F << 2));
+  // Set current and power for averaging and keep device specific (trim) settings.
+  asc37800Read32(ACSREG_S_TRIM, &val);
+  currZtrim = currZtrimOld = val & 0xFF;
+  asc37800Write32(ACSREG_S_TRIM, (val | (1 << 23) | (1 << 22)));
+  // Set average to 10 samples. (ASC37800 sample rate 1khz)
+  asc37800Write32(ACSREG_S_OFFS_AVG, ((10 << 7) | (10 << 0)));
+
+  DEBUG_PRINT("---------------\n");
+  for (int reg = 0x0B; reg <= 0x0F; reg++)
+  {
+    bool res;
+
+    res = asc37800Read32(reg, &val);
+    DEBUG_PRINT("%d:R:0x%.2X:%X\n", (unsigned int)res, (unsigned int)reg, (unsigned int)val);
+    res = asc37800Read32(reg+0x10, &val);
+    DEBUG_PRINT("%d:R:0x%.2X:%X\n\n", (unsigned int)res, (unsigned int)reg+0x10, (unsigned int)val);
+  }
+
+
+  xTaskCreate(asc37800Task, "asc37800",
+              2*configMINIMAL_STACK_SIZE, NULL,
+              /*priority*/2, NULL);
+
+  isInit = true;
+}
+
+static void asc37800Task(void* prm)
+{
+  systemWaitStart();
+
+  TickType_t lastWakeTime = xTaskGetTickCount();
+
+  while(1) {
+    vTaskDelayUntil(&lastWakeTime, M2T(10));
+
+    asc37800Read32(ACSREG_I_V_CODES, &viBatRaw);
+    asc37800Read32(ACSREG_I_V_RMS, &viBatRMS);
+    asc37800Read32(ACSREG_VRMS_AVGS, &vavgBatRMS);
+    asc37800Read32(ACSREG_P_INSTANT, &pBatRaw);
+    asc37800Read32(ACSREG_P_ACT_AVGS, &pavgBatRaw);
+
+    vBat = (int16_t)(viBatRaw & 0xFFFF) / CODES_HALF_RANGE * ADC_RANGE * V_DIVIDER;
+    iBat = (int16_t)(viBatRaw >> 16 & 0xFFFF) / CODES_HALF_RANGE * I_RANGE;
+    vBatRMS = (uint16_t)(viBatRMS & 0xFFFF) / CODES_FULL_RANGE * ADC_RANGE * V_DIVIDER;
+    iBatRMS = (uint16_t)(viBatRMS >> 16 & 0xFFFF) / CODES_FULL_RANGE * I_RANGE;
+//    vavgBat = (uint16_t)(vavgBatRMS & 0xFFFF) / CODES_FULL_RANGE * ADC_RANGE * V_DIVIDER;
+    iavgBat = (uint16_t)(vavgBatRMS >> 16 & 0xFFFF) / CODES_FULL_RANGE * I_RANGE;
+    pBat = (int16_t)(pBatRaw & 0xFFFF) / LSB_PER_MILLWATT * V_DIVIDER / MILLW_TO_WATT;
+    pavgBat = (int16_t)(pavgBatRaw & 0xFFFF) / LSB_PER_MILLWATT * V_DIVIDER / MILLW_TO_WATT;
+
+    vBatMV = (int16_t)(vBatRMS * 1000);
+    iBatMA = (int16_t)(iavgBat * 1000);
+    pBatMW = (int16_t)(pavgBat * 1000);
+
+    // Code so tuning can be done through cfclient parameters
+    if (currZtrimOld != currZtrim)
+    {
+      uint32_t val;
+      asc37800Read32(ACSREG_S_TRIM, &val);
+      DEBUG_PRINT("%X ->", (unsigned int)val);
+      val = (val & 0xFFFFFE00) | currZtrim;
+      DEBUG_PRINT("%X\n ", (unsigned int)val);
+      asc37800Write32(ACSREG_S_TRIM, val);
+      currZtrimOld = currZtrim;
+    }
+
+    if (writeTrim != 0)
+    {
+      uint32_t val;
+      writeTrim = 0;
+      // Enable EEPROM Writing
+      asc37800Write32(ACSREG_ACCESS_CODE, ACS_ACCESS_CODE);
+      asc37800Read32(ACSREG_S_TRIM, &val);
+      DEBUG_PRINT("EEPROM write: %X", (unsigned int)val);
+      // Write trim reg to EEPROM
+      asc37800Write32(ACSREG_E_TRIM, val);
+      vTaskDelay(M2T(10));
+      // Disable access
+      asc37800Write32(ACSREG_ACCESS_CODE, 0);
+    }
+
+//    DEBUG_PRINT("V: %.3f I: %.3f P: %.3f\n", vBat, iBat, pBat);
+//    DEBUG_PRINT("V: %f\tI: %f\tP: %f\n", vBat, iBat, pBat);
+//    DEBUG_PRINT("Vc: %f\tIc: %f\tV: %f\tI: %f\tP: %f\n", vavgBat, iavgBat, vBat, iBat, pBat);
+  }
+}
+
+static const DeckDriver asc37800_deck = {
+  .vid = 0x00,
+  .pid = 0x00,
+  .name = "bcACS37800",
+
+  .usedGpio = DECK_USING_I2C,
+
+  .init = asc37800Init,
+};
+
+DECK_DRIVER(asc37800_deck);
+
+PARAM_GROUP_START(deck)
+PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcACS37800, &isInit)
+PARAM_GROUP_STOP(deck)
+
+PARAM_GROUP_START(asc37800)
+PARAM_ADD(PARAM_UINT16, currZtrim, &currZtrim)
+PARAM_ADD(PARAM_UINT8, writeTrim, &writeTrim)
+PARAM_GROUP_STOP(asc37800)
+
+LOG_GROUP_START(asc37800)
+LOG_ADD(LOG_FLOAT, v, &vBat)
+LOG_ADD(LOG_FLOAT, vRMS, &vBatRMS)
+LOG_ADD(LOG_INT16, v_mV, &vBatMV)
+LOG_ADD(LOG_FLOAT, i, &iBat)
+LOG_ADD(LOG_FLOAT, iRMS, &iBatRMS)
+LOG_ADD(LOG_FLOAT, i_avg, &iavgBat)
+LOG_ADD(LOG_INT16, i_mA, &iBatMA)
+LOG_ADD(LOG_FLOAT, p, &pBat)
+LOG_ADD(LOG_FLOAT, p_avg, &pavgBat)
+LOG_ADD(LOG_INT16, p_mW, &pBatMW)
+LOG_GROUP_STOP(asc37800)
diff --git a/src/deck/drivers/src/loadcell.c b/src/deck/drivers/src/loadcell.c
new file mode 100644
index 0000000000..dd0114128d
--- /dev/null
+++ b/src/deck/drivers/src/loadcell.c
@@ -0,0 +1,209 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2021 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/>.
+ *
+ * loadcell.c - Deck driver for HX711 load cell
+ * See 
+ *  * https://learn.sparkfun.com/tutorials/load-cell-amplifier-hx711-breakout-hookup-guide
+ *  * Code based on https://github.com/bogde/HX711
+ */
+
+#include <stdint.h>
+#include <stdlib.h>
+#include "stm32fxxx.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+#include "timers.h"
+
+#include "deck.h"
+#include "param.h"
+#include "log.h"
+#include "sleepus.h"
+
+// Hardware defines (also update deck driver below!)
+#define CLK_PIN    DECK_GPIO_IO1
+#define DAT_PIN    DECK_GPIO_IO4
+
+static bool isInit;
+static int32_t rawWeight;
+static float weight;
+static bool enable = true;
+
+static float a = 4.22852802e-05;
+static float b = -2.21784688e+01;
+
+static void loadcellTask(void* prm);
+
+static enum hx711_gain
+{
+  GAIN128 = 1,
+  GAIN64 = 3,
+  GAIN32 = 2,
+} gain;
+
+static void hx711_init(void);
+static bool hx711_is_ready(void);
+// static void hx711_set_gain(void);
+static int32_t hx711_read(void);
+// static void hx711_power_down(void);
+static void hx711_power_up(void);
+
+static void hx711_init(void)
+{
+  pinMode(CLK_PIN, OUTPUT);
+  pinMode(DAT_PIN, INPUT);
+  // hx711_set_gain();
+  hx711_power_up();
+}
+
+static bool hx711_is_ready(void)
+{
+  return digitalRead(DAT_PIN) == LOW;
+}
+
+// static void hx711_set_gain(void)
+// {
+//   digitalWrite(CLK_PIN, LOW);
+//   hx711_read();
+// }
+
+static int32_t hx711_read(void)
+{
+  // Wait for the chip to become ready.
+  // while (!hx711_is_ready());
+
+  // if (!hx711_is_ready()) {
+  //   return 0;
+  // }
+
+  // Define structures for reading data into.
+  int32_t value = 0;
+
+  // Protect the read sequence from system interrupts.  If an interrupt occurs during
+  // the time the PD_SCK signal is high it will stretch the length of the clock pulse.
+  // If the total pulse time exceeds 60 uSec this will cause the HX711 to enter
+  // power down mode during the middle of the read sequence.  While the device will
+  // wake up when PD_SCK goes low again, the reset starts a new conversion cycle which
+  // forces DOUT high until that cycle is completed.  
+  //
+  // The result is that all subsequent bits read by shiftIn() will read back as 1,
+  // corrupting the value returned by read().
+
+  // Since we use a high-priority task, we do not need to actually disable the interrupt
+
+  // portDISABLE_INTERRUPTS();
+
+  // Pulse the clock pin 24 times to read the data.
+  for (int i = 0; i < 24; ++i) {
+    digitalWrite(CLK_PIN, HIGH);
+    value |= digitalRead(DAT_PIN) << (24 - i);
+    digitalWrite(CLK_PIN, LOW);
+    sleepus(1);
+  }
+
+  // Set the channel and the gain factor for the next reading using the clock pin.
+  for (int i = 0; i < gain; ++i) {
+    digitalWrite(CLK_PIN, HIGH);
+    sleepus(1);
+    digitalWrite(CLK_PIN, LOW);
+    sleepus(1);
+  }
+
+  // portENABLE_INTERRUPTS();
+
+  // Replicate the most significant bit to pad out a 32-bit signed integer
+  if (value & (1<<24)) {
+    value = 0xFF000000 | value;
+  }
+  return value;
+}
+
+// static void hx711_power_down(void)
+// {
+//   digitalWrite(CLK_PIN, LOW);
+//   digitalWrite(CLK_PIN, HIGH);
+// }
+
+static void hx711_power_up(void)
+{
+  digitalWrite(CLK_PIN, LOW);
+}
+
+static void loadcellInit(DeckInfo *info)
+{
+  if (isInit) {
+    return;
+  }
+
+  gain = GAIN128;
+  hx711_init();
+
+  // Create a task with very high priority to reduce risk that we get
+  // invalid data
+  xTaskCreate(loadcellTask, "LOADCELL",
+              configMINIMAL_STACK_SIZE, NULL,
+              /*priority*/6, NULL);
+
+  isInit = true;
+}
+
+static void loadcellTask(void* prm)
+{
+  TickType_t lastWakeTime = xTaskGetTickCount();
+
+  while(1) {
+    vTaskDelayUntil(&lastWakeTime, F2T(100));
+
+    if (enable && hx711_is_ready()) {
+      rawWeight = hx711_read();
+      weight = a * rawWeight + b;
+    }
+  }
+}
+
+static const DeckDriver loadcell_deck = {
+  .vid = 0x00,
+  .pid = 0x00,
+  .name = "bcLoadcell",
+
+  .usedGpio = DECK_USING_IO_1 | DECK_USING_IO_4,
+
+  .init = loadcellInit,
+};
+
+DECK_DRIVER(loadcell_deck);
+
+PARAM_GROUP_START(deck)
+PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcLoadcell, &isInit)
+PARAM_GROUP_STOP(deck)
+
+PARAM_GROUP_START(loadcell)
+PARAM_ADD(PARAM_UINT8, enable, &enable)
+PARAM_ADD(PARAM_FLOAT, a, &a)
+PARAM_ADD(PARAM_FLOAT, b, &b)
+PARAM_GROUP_STOP(loadcell)
+
+LOG_GROUP_START(loadcell)
+LOG_ADD(LOG_INT32, rawWeight, &rawWeight)
+LOG_ADD(LOG_FLOAT, weight, &weight)
+LOG_GROUP_STOP(loadcell)
diff --git a/src/deck/drivers/src/loadcell_nau7802.c b/src/deck/drivers/src/loadcell_nau7802.c
new file mode 100644
index 0000000000..717aa780d5
--- /dev/null
+++ b/src/deck/drivers/src/loadcell_nau7802.c
@@ -0,0 +1,567 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2021 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/>.
+ *
+ * loadcell.c - Deck driver for NAU7802 load cell
+ * See 
+ *  * https://learn.sparkfun.com/tutorials/qwiic-scale-hookup-guide
+ *  * Code based on https://github.com/sparkfun/SparkFun_Qwiic_Scale_NAU7802_Arduino_Library
+ */
+
+#include <stdint.h>
+#include <stdlib.h>
+#include "nvicconf.h"
+#include "stm32fxxx.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+#include "timers.h"
+
+#include "deck.h"
+#include "param.h"
+#include "log.h"
+#include "i2cdev.h"
+#include "sleepus.h"
+#include "debug.h"
+#include "statsCnt.h"
+
+// Hardware defines (also update deck driver below!)
+#define DECK_I2C_ADDRESS 0x2A
+#define DATA_READY_PIN DECK_GPIO_IO1
+
+static bool isInit;
+static int32_t rawWeight;
+static float weight;
+
+static float a[2];
+static float b[2];
+// static uint8_t currentChannel = 0;
+
+static uint8_t sampleRateDesired = 0;
+static uint8_t sampleRate = 0;
+static uint8_t channelDesired = 0;
+static uint8_t channel = 0;
+
+static STATS_CNT_RATE_DEFINE(rate, 1000);
+
+static xSemaphoreHandle dataReady;
+static StaticSemaphore_t dataReadyBuffer;
+
+static void loadcellTask(void* prm);
+
+////////////////////////////
+
+typedef struct
+{
+  uint8_t pu_ctrl;
+  uint8_t ctrl1;
+  uint8_t ctrl2;
+} nau7802_t;
+static nau7802_t nau7802;
+
+//Register Map
+typedef enum
+{
+  NAU7802_PU_CTRL = 0x00,
+  NAU7802_CTRL1,
+  NAU7802_CTRL2,
+  NAU7802_OCAL1_B2,
+  NAU7802_OCAL1_B1,
+  NAU7802_OCAL1_B0,
+  NAU7802_GCAL1_B3,
+  NAU7802_GCAL1_B2,
+  NAU7802_GCAL1_B1,
+  NAU7802_GCAL1_B0,
+  NAU7802_OCAL2_B2,
+  NAU7802_OCAL2_B1,
+  NAU7802_OCAL2_B0,
+  NAU7802_GCAL2_B3,
+  NAU7802_GCAL2_B2,
+  NAU7802_GCAL2_B1,
+  NAU7802_GCAL2_B0,
+  NAU7802_I2C_CONTROL,
+  NAU7802_ADCO_B2,
+  NAU7802_ADCO_B1,
+  NAU7802_ADCO_B0,
+  NAU7802_ADC = 0x15, //Shared ADC and OTP 32:24
+  NAU7802_OTP_B1,     //OTP 23:16 or 7:0?
+  NAU7802_OTP_B0,     //OTP 15:8
+  NAU7802_PGA = 0x1B,
+  NAU7802_PGA_PWR = 0x1C,
+  NAU7802_DEVICE_REV = 0x1F,
+} Scale_Registers;
+
+//Bits within the PU_CTRL register
+typedef enum
+{
+  NAU7802_PU_CTRL_RR = 0,
+  NAU7802_PU_CTRL_PUD,
+  NAU7802_PU_CTRL_PUA,
+  NAU7802_PU_CTRL_PUR,
+  NAU7802_PU_CTRL_CS,
+  NAU7802_PU_CTRL_CR,
+  NAU7802_PU_CTRL_OSCS,
+  NAU7802_PU_CTRL_AVDDS,
+} PU_CTRL_Bits;
+
+//Bits within the CTRL1 register
+typedef enum
+{
+  NAU7802_CTRL1_GAIN = 2,
+  NAU7802_CTRL1_VLDO = 5,
+  NAU7802_CTRL1_DRDY_SEL = 6,
+  NAU7802_CTRL1_CRP = 7,
+} CTRL1_Bits;
+
+//Bits within the CTRL2 register
+typedef enum
+{
+  NAU7802_CTRL2_CALMOD = 0,
+  NAU7802_CTRL2_CALS = 2,
+  NAU7802_CTRL2_CAL_ERROR = 3,
+  NAU7802_CTRL2_CRS = 4,
+  NAU7802_CTRL2_CHS = 7,
+} CTRL2_Bits;
+
+//Bits within the PGA register
+typedef enum
+{
+  NAU7802_PGA_CHP_DIS = 0,
+  NAU7802_PGA_INV = 3,
+  NAU7802_PGA_BYPASS_EN,
+  NAU7802_PGA_OUT_EN,
+  NAU7802_PGA_LDOMODE,
+  NAU7802_PGA_RD_OTP_SEL,
+} PGA_Bits;
+
+//Bits within the PGA PWR register
+typedef enum
+{
+  NAU7802_PGA_PWR_PGA_CURR = 0,
+  NAU7802_PGA_PWR_ADC_CURR = 2,
+  NAU7802_PGA_PWR_MSTR_BIAS_CURR = 4,
+  NAU7802_PGA_PWR_PGA_CAP_EN = 7,
+} PGA_PWR_Bits;
+
+//Allowed Low drop out regulator voltages
+typedef enum
+{
+  NAU7802_LDO_2V4 = 0b111,
+  NAU7802_LDO_2V7 = 0b110,
+  NAU7802_LDO_3V0 = 0b101,
+  NAU7802_LDO_3V3 = 0b100,
+  NAU7802_LDO_3V6 = 0b011,
+  NAU7802_LDO_3V9 = 0b010,
+  NAU7802_LDO_4V2 = 0b001,
+  NAU7802_LDO_4V5 = 0b000,
+} NAU7802_LDO_Values;
+
+//Allowed gains
+typedef enum
+{
+  NAU7802_GAIN_128 = 0b111,
+  NAU7802_GAIN_64 = 0b110,
+  NAU7802_GAIN_32 = 0b101,
+  NAU7802_GAIN_16 = 0b100,
+  NAU7802_GAIN_8 = 0b011,
+  NAU7802_GAIN_4 = 0b010,
+  NAU7802_GAIN_2 = 0b001,
+  NAU7802_GAIN_1 = 0b000,
+} NAU7802_Gain_Values;
+
+//Allowed samples per second
+typedef enum
+{
+  NAU7802_SPS_320 = 0b111,
+  NAU7802_SPS_80 = 0b011,
+  NAU7802_SPS_40 = 0b010,
+  NAU7802_SPS_20 = 0b001,
+  NAU7802_SPS_10 = 0b000,
+} NAU7802_SPS_Values;
+
+//Select between channel values
+typedef enum
+{
+  NAU7802_CHANNEL_1 = 0,
+  NAU7802_CHANNEL_2 = 1,
+} NAU7802_Channels;
+
+//Calibration state
+typedef enum
+{
+  NAU7802_CAL_SUCCESS = 0,
+  NAU7802_CAL_IN_PROGRESS = 1,
+  NAU7802_CAL_FAILURE = 2,
+} NAU7802_Cal_Status;
+
+void nau7802_init(nau7802_t* ctx)
+{
+  ctx->pu_ctrl = 0;
+  ctx->ctrl1 = 0;
+  ctx->ctrl2 = 0;
+}
+
+// //Resets all registers to Power Of Defaults
+// bool nau7802_reset(nau7802_t *ctx)
+// {
+//   bool result = true;
+//   result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_RR, 1);
+//   sleepus(1);
+//   result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_RR, 0);
+//   return result;
+// }
+
+//Power up digital and analog sections of scale
+bool nau7802_powerUp(nau7802_t *ctx)
+{
+  bool result = true;
+
+  ctx->pu_ctrl = 0x06; //(PU analog and PU digital)
+  result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, ctx->pu_ctrl);
+  DEBUG_PRINT("pu %d\n", result);
+
+//  result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_PUD, 1);
+//  result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_PUA, 1);
+
+  //Wait for Power Up bit to be set - takes approximately 200us
+  for (int i = 0; i < 200; ++i) {
+    uint8_t bit;
+    result &= i2cdevReadBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_PUR, &bit);
+    if (result && bit) {
+      return true;
+    }
+    sleepus(1);
+  }
+  return false;
+}
+
+//Set the onboard Low-Drop-Out voltage regulator to a given value
+bool nau7802_setLDO(nau7802_t *ctx, NAU7802_LDO_Values ldoValue)
+{
+  bool result = true;
+  //Set the value of the LDO
+  ctx->ctrl1 &= 0b11000111;    //Clear LDO bits
+  ctx->ctrl1 |= ldoValue << 3; //Mask in new LDO bits
+  result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL1, ctx->ctrl1);
+  //Enable the internal LDO
+  DEBUG_PRINT("l1 %d\n", ctx->pu_ctrl);
+  ctx->pu_ctrl |= (1 << NAU7802_PU_CTRL_AVDDS);
+  result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, ctx->pu_ctrl);
+  DEBUG_PRINT("l2 %d\n", ctx->pu_ctrl);
+  return result;
+}
+
+//Set the gain
+bool nau7802_setGain(nau7802_t *ctx, NAU7802_Gain_Values gainValue)
+{
+  bool result = true;
+  ctx->ctrl1 &= 0b11111000; //Clear gain bits
+  ctx->ctrl1 |= gainValue;  //Mask in new bits
+  result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL1, ctx->ctrl1);
+  return result;
+}
+
+//Set the readings per second
+bool nau7802_setSampleRate(nau7802_t* ctx, NAU7802_SPS_Values rate)
+{
+  bool result = true;
+  ctx->ctrl2 &= 0b10001111; // Clear CRS bits
+  ctx->ctrl2 |= rate << 4;  //Mask in new CRS bits
+  result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, ctx->ctrl2);
+  return result;
+}
+
+//Set channel
+bool nau7802_setChannel(nau7802_t *ctx, NAU7802_Channels channel)
+{
+  bool result = true;
+  ctx->ctrl2 &= 0b01111111; // Clear CHS bits
+  ctx->ctrl2 |= channel << 7;  //Mask in new CHS bits
+  result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, ctx->ctrl2);
+  return result;
+}
+
+//Check calibration status.
+NAU7802_Cal_Status nau7802_calAFEStatus(nau7802_t *ctx)
+{
+  bool result = true;
+  uint8_t bit;
+  result &= i2cdevReadBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, NAU7802_CTRL2_CALS, &bit);
+  if (result && bit) {
+    return NAU7802_CAL_IN_PROGRESS;
+  }
+  result &= i2cdevReadBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, NAU7802_CTRL2_CAL_ERROR, &bit);
+  if (result && bit) {
+    return NAU7802_CAL_FAILURE;
+  }
+
+  if (result) {
+    // Calibration passed
+    return NAU7802_CAL_SUCCESS;
+  }
+  return NAU7802_CAL_FAILURE;
+}
+
+//Calibrate analog front end of system. Returns true if CAL_ERR bit is 0 (no error)
+//Takes approximately 344ms to calibrate; wait up to 1000ms.
+//It is recommended that the AFE be re-calibrated any time the gain, SPS, or channel number is changed.
+bool nau7802_calibrateAFE(nau7802_t *ctx)
+{
+  bool result = true;
+  //Begin asynchronous calibration of the analog front end.
+  result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, NAU7802_CTRL2_CALS, 1);
+
+  for (int i = 0; i < 1000; ++i) {
+    if (nau7802_calAFEStatus(ctx) == NAU7802_CAL_SUCCESS) {
+      return true;
+    }
+    vTaskDelay(M2T(1));
+  }
+  return false;
+}
+
+//Returns true if Cycle Ready bit is set (conversion is complete)
+bool nau7802_hasMeasurement(nau7802_t *ctx)
+{
+  bool result = true;
+  uint8_t bit;
+  result &= i2cdevReadBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_CR, &bit);
+  return result && bit;
+}
+
+//Returns 24-bit reading
+//Assumes CR Cycle Ready bit (ADC conversion complete) has been checked to be 1
+bool nau7802_getMeasurement(nau7802_t *ctx, int32_t *measurement)
+{
+  bool result = true;
+
+  uint8_t data[3];
+  result &= i2cdevReadReg8(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_ADCO_B2, 3, (uint8_t*)&data);
+  // DEBUG_PRINT("M: %d %d, %d, %d\n", result, data[0], data[1], data[2]);
+
+  int32_t valueRaw = ((int32_t)data[0] << 16) |
+                      ((int32_t)data[1] << 8) |
+                      ((int32_t)data[2] << 0);
+  // recover the sign
+  int32_t valueShifted = (valueRaw << 8);
+  int32_t value = (valueShifted >> 8);
+
+  *measurement = value;
+
+  return result;
+}
+
+bool nau7802_revision(nau7802_t *ctx, uint8_t *revision)
+{
+  bool result = true;
+  result &= i2cdevReadByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_DEVICE_REV, revision);
+  *revision = *revision & 0x0F;
+  return result;
+}
+
+/////////////////////////////
+
+static void setupDataReadyInterrupt(void)
+{
+  pinMode(DATA_READY_PIN, INPUT);
+
+  dataReady = xSemaphoreCreateBinaryStatic(&dataReadyBuffer);
+
+  // TODO: EXTI_PortSourceGPIOB and EXTI_PinSource8 should be part of deckGPIOMapping!
+  SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOB, EXTI_PinSource8);
+
+  EXTI_InitTypeDef EXTI_InitStructure;
+  EXTI_InitStructure.EXTI_Line = EXTI_Line8;
+  EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
+  EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
+  EXTI_InitStructure.EXTI_LineCmd = ENABLE;
+  // portDISABLE_INTERRUPTS();
+  EXTI_Init(&EXTI_InitStructure);
+  // EXTI_ClearITPendingBit(EXTI_Line15);
+  // portENABLE_INTERRUPTS();
+}
+
+void __attribute__((used)) EXTI8_Callback(void)
+{
+  portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
+  xSemaphoreGiveFromISR(dataReady, &xHigherPriorityTaskWoken);
+  if (xHigherPriorityTaskWoken) {
+    portYIELD();
+  }
+}
+
+static void loadcellInit(DeckInfo *info)
+{
+  // if (isInit) {
+    // return;
+  // }
+
+  isInit = true;
+
+  // sleepus(1000 * 1000);
+  // isInit &= nau7802_reset();
+
+  if (true) {
+
+    nau7802_init(&nau7802);
+    // isInit &= nau7802_reset();
+    // DEBUG_PRINT("Reset [%d]\n", isInit);
+
+    isInit &= nau7802_powerUp(&nau7802);
+    DEBUG_PRINT("Power up [%d]\n", isInit);
+
+    uint8_t revision;
+    isInit &= nau7802_revision(&nau7802, & revision);
+    DEBUG_PRINT("revision [%d]: %d\n", isInit, revision);
+
+    isInit &= nau7802_setLDO(&nau7802, NAU7802_LDO_2V7);
+    DEBUG_PRINT("LDO [%d]\n", isInit);
+    isInit &= nau7802_setGain(&nau7802, NAU7802_GAIN_128);
+    DEBUG_PRINT("Gain [%d]\n", isInit);
+    isInit &= nau7802_setSampleRate(&nau7802, NAU7802_SPS_10);
+    DEBUG_PRINT("Sample rate [%d]\n", isInit);
+    // Turn off CLK_CHP. From 9.1 power on sequencing.
+    isInit &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_ADC, 0x30);
+    DEBUG_PRINT("CLK_CHP [%d]\n", isInit);
+    // Enable 330pF decoupling cap on chan 2. From 9.14 application circuit note.
+    isInit &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PGA_PWR, NAU7802_PGA_PWR_PGA_CAP_EN, 1);
+    DEBUG_PRINT("CAP [%d]\n", isInit);
+    // calibrate
+    isInit &= nau7802_calibrateAFE(&nau7802);
+    DEBUG_PRINT("Cal [%d]\n", isInit);
+
+    uint8_t v;
+    isInit &= i2cdevReadByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, &v);
+    DEBUG_PRINT("puctrl: %d\n", v);
+    isInit &= i2cdevReadByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL1, &v);
+    DEBUG_PRINT("ctrl1: %d\n", v);
+    isInit &= i2cdevReadByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, &v);
+    DEBUG_PRINT("ctrl2: %d\n", v);
+  }
+
+  // Set up Interrupt
+  setupDataReadyInterrupt();
+
+  if (1) {
+    // Create a task
+    xTaskCreate(loadcellTask, "LOADCELL",
+                configMINIMAL_STACK_SIZE, NULL,
+                /*priority*/1, NULL);
+  }
+}
+
+static void loadcellTask(void* prm)
+{
+  while (1) {
+    BaseType_t semResult = xSemaphoreTake(dataReady, M2T(500));
+    if (semResult == pdTRUE || digitalRead(DATA_READY_PIN))
+    {
+      int32_t measurement;
+      bool result = nau7802_getMeasurement(&nau7802, &measurement);
+      if (result) {
+        rawWeight = measurement;
+        weight = a[channel] * rawWeight + b[channel];
+        // currentChannel = (currentChannel + 1) % 2;
+        // nau7802_setChannel(&nau7802, currentChannel);
+
+        // nau7802.pu_ctrl |= (1 << NAU7802_PU_CTRL_CS);
+        // i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, nau7802.pu_ctrl);
+
+        // nau7802.ctrl2 &= 0b11101111; // Clear CS bit
+        // i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, nau7802.pu_ctrl);
+
+        STATS_CNT_RATE_EVENT(&rate);
+      }
+    }
+    if (sampleRate != sampleRateDesired) {
+      switch (sampleRateDesired) {
+        case NAU7802_SPS_320:
+        case NAU7802_SPS_80:
+        case NAU7802_SPS_40:
+        case NAU7802_SPS_20:
+        case NAU7802_SPS_10:
+          {
+            bool result = nau7802_setSampleRate(&nau7802, sampleRateDesired);
+            if (result) {
+              DEBUG_PRINT("switched sample rate!\n");
+            }
+          }
+          break;
+        default:
+          DEBUG_PRINT("Unknown sample rate!\n");
+          break;
+      }
+      sampleRate = sampleRateDesired;
+    }
+    if (channel != channelDesired) {
+      switch (channelDesired) {
+        case NAU7802_CHANNEL_1:
+        case NAU7802_CHANNEL_2:
+          {
+            bool result = nau7802_setChannel(&nau7802, channelDesired);
+            result &= nau7802_calibrateAFE(&nau7802);
+            if (result)
+            {
+              DEBUG_PRINT("switched and calibrated channel!\n");
+            }
+          }
+          break;
+        default:
+          DEBUG_PRINT("Unknown channel!\n");
+          break;
+      }
+      channel = channelDesired;
+    }
+
+
+  }
+}
+
+static const DeckDriver loadcell_deck = {
+    .vid = 0x00,
+    .pid = 0x00,
+    .name = "bcLoadcell",
+
+    .usedGpio = DECK_USING_IO_1,
+
+    .init = loadcellInit,
+};
+
+DECK_DRIVER(loadcell_deck);
+
+PARAM_GROUP_START(deck)
+PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcLoadcell, &isInit)
+PARAM_GROUP_STOP(deck)
+
+PARAM_GROUP_START(loadcell)
+PARAM_ADD(PARAM_FLOAT, a, &a[0])
+PARAM_ADD(PARAM_FLOAT, b, &b[0])
+PARAM_ADD(PARAM_UINT8, sampleRate, &sampleRateDesired)
+PARAM_ADD(PARAM_UINT8, channel, &channelDesired)
+PARAM_GROUP_STOP(loadcell)
+
+LOG_GROUP_START(loadcell)
+LOG_ADD(LOG_INT32, rawWeight, &rawWeight)
+LOG_ADD(LOG_FLOAT, weight, &weight)
+
+STATS_CNT_RATE_LOG_ADD(rate, &rate)
+LOG_GROUP_STOP(loadcell)
diff --git a/src/deck/drivers/src/rpm.c b/src/deck/drivers/src/rpm.c
new file mode 100644
index 0000000000..4c9eb19f44
--- /dev/null
+++ b/src/deck/drivers/src/rpm.c
@@ -0,0 +1,328 @@
+/*
+ *    ||          ____  _ __                           
+ * +------+      / __ )(_) /_______________ _____  ___ 
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2012 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/>.
+ *
+ * rpm.c - Deck that measure the motor RPM using QRD1114 IR reflector-sensor.
+ */
+#define DEBUG_MODULE "RPM"
+
+#include <stdint.h>
+#include <string.h>
+
+#include "stm32fxxx.h"
+#include "config.h"
+#include "deck.h"
+#include "debug.h"
+#include "log.h"
+
+//Hardware configuration
+#define ET_GPIO_PERIF   (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOC)
+
+#define ET_GPIO_PORT_TX2  GPIOA
+#define ET_GPIO_PIN_TX2   GPIO_Pin_2
+#define ET_GPIO_PORT_RX2  GPIOA
+#define ET_GPIO_PIN_RX2   GPIO_Pin_3
+#define ET_GPIO_PORT_IO2  GPIOB
+#define ET_GPIO_PIN_IO2   GPIO_Pin_5
+#define ET_GPIO_PORT_IO3  GPIOB
+#define ET_GPIO_PIN_IO3   GPIO_Pin_4
+
+#define ER_NBR_PINS         4
+
+typedef struct _etGpio
+{
+  GPIO_TypeDef     *port;
+  uint16_t          pin;
+  char              name[6];
+} EtGpio;
+
+EtGpio erGpio[ER_NBR_PINS] =
+{
+    {ET_GPIO_PORT_TX2,  ET_GPIO_PIN_TX2, "TX2"},
+    {ET_GPIO_PORT_RX2,  ET_GPIO_PIN_RX2, "RX2"},
+    {ET_GPIO_PORT_IO2,  ET_GPIO_PIN_IO2, "IO2"},
+    {ET_GPIO_PORT_IO3,  ET_GPIO_PIN_IO3, "IO3"},
+};
+
+static bool isInit;
+static uint16_t lastcc1Val;
+static uint16_t lastcc2Val;
+static uint16_t lastcc3Val;
+static uint16_t lastcc4Val;
+static uint16_t m1Time[2];
+static uint16_t m2Time[2];
+static uint16_t m3Time[2];
+static uint16_t m4Time[2];
+static int m1cnt;
+static int m2cnt;
+static int m3cnt;
+static int m4cnt;
+static uint16_t m1rpm;
+static uint16_t m2rpm;
+static uint16_t m3rpm;
+static uint16_t m4rpm;
+
+
+static void rpmInit(DeckInfo *info)
+{
+  int i;
+  isInit = true;
+
+  GPIO_InitTypeDef GPIO_InitStructure;
+  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
+  TIM_ICInitTypeDef  TIM_ICInitStructure;
+  NVIC_InitTypeDef NVIC_InitStructure;
+
+  // Enable Clocks
+  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB, ENABLE);
+  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM5, ENABLE);
+
+  // Configure optical switch input pins
+  for (i = 0; i < ER_NBR_PINS; i++)
+  {
+    GPIO_InitStructure.GPIO_Pin = erGpio[i].pin;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
+    GPIO_InitStructure.GPIO_Speed = GPIO_High_Speed;
+    GPIO_Init(erGpio[i].port, &GPIO_InitStructure);
+  }
+
+   // Map timer to alternate functions
+  GPIO_PinAFConfig(GPIOA, GPIO_PinSource2, GPIO_AF_TIM5);
+  GPIO_PinAFConfig(GPIOA, GPIO_PinSource3, GPIO_AF_TIM5);
+  GPIO_PinAFConfig(GPIOB, GPIO_PinSource5, GPIO_AF_TIM3);
+  GPIO_PinAFConfig(GPIOB, GPIO_PinSource4, GPIO_AF_TIM3);
+
+  /* Time base configuration */
+  TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
+  TIM_TimeBaseStructure.TIM_Prescaler = (84 - 1); // 84 / 84M = 1uS
+  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+  TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
+  TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure);
+  TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
+
+  TIM_ICInitStructure.TIM_Channel = TIM_Channel_3;
+  TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
+  TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
+  TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
+  TIM_ICInitStructure.TIM_ICFilter = 0x0;
+  TIM_ICInit(TIM5, &TIM_ICInitStructure);
+  TIM_ICInitStructure.TIM_Channel = TIM_Channel_4;
+  TIM_ICInit(TIM5, &TIM_ICInitStructure);
+  TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
+  TIM_ICInit(TIM3, &TIM_ICInitStructure);
+  TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
+  TIM_ICInit(TIM3, &TIM_ICInitStructure);
+
+  /* Enable the TIM5 and TIM3 global Interrupt */
+  NVIC_InitStructure.NVIC_IRQChannel = TIM5_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 10;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+  NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
+  NVIC_Init(&NVIC_InitStructure);
+
+  /* TIM enable counter */
+  TIM_Cmd(TIM5, ENABLE);
+  TIM_Cmd(TIM3, ENABLE);
+  TIM_ITConfig(TIM5, TIM_IT_CC3, ENABLE);
+  TIM_ITConfig(TIM5, TIM_IT_CC4, ENABLE);
+  TIM_ITConfig(TIM5, TIM_IT_Update, ENABLE);
+  TIM_ITConfig(TIM3, TIM_IT_CC1, ENABLE);
+  TIM_ITConfig(TIM3, TIM_IT_CC2, ENABLE);
+  TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);
+}
+
+static uint16_t calcRPM(uint32_t t1, uint32_t t2)
+{
+  uint16_t rpm;
+  // (timer resolution * sec/min) / (time for one revolution)
+  rpm = (1000000 * 60) / (t1 + t2);
+
+  return rpm;
+}
+
+void __attribute__((used)) TIM5_IRQHandler(void)
+{
+  static int updateM1Cnt = 2;
+  static int updateM4Cnt = 2;
+  uint16_t ccVal;
+
+  //Motor1
+  if(TIM_GetITStatus(TIM5, TIM_IT_CC3) == SET)
+  {
+    /* Clear TIM5 Capture compare interrupt pending bit */
+    TIM_ClearITPendingBit(TIM5, TIM_IT_CC3);
+    ccVal = TIM_GetCapture3(TIM5);
+
+    if (TIM_GetFlagStatus(TIM5, TIM_FLAG_CC3OF))
+    {
+      // Overflow
+      lastcc3Val = ccVal;
+    }
+
+    if (ccVal > lastcc3Val)
+      m1Time[m1cnt & 0x01] = ccVal - lastcc3Val;
+    else
+      m1Time[m1cnt & 0x01] = (uint16_t)((0xFFFF + (uint32_t)ccVal) - lastcc3Val);
+
+    lastcc3Val = ccVal;
+    m1cnt++;
+    m1rpm = calcRPM(m1Time[0], m1Time[1]);
+    updateM1Cnt = 2;
+  }
+
+  //Motor4
+  if(TIM_GetITStatus(TIM5, TIM_IT_CC4) == SET)
+  {
+    /* Clear TIM5 Capture compare interrupt pending bit */
+    TIM_ClearITPendingBit(TIM5, TIM_IT_CC4);
+    ccVal = TIM_GetCapture4(TIM5);
+
+    if (TIM_GetFlagStatus(TIM5, TIM_FLAG_CC4OF))
+    {
+      // Overflow
+      lastcc4Val = ccVal;
+    }
+
+    if (ccVal > lastcc4Val)
+      m4Time[m4cnt & 0x01] = ccVal - lastcc4Val;
+    else
+      m4Time[m4cnt & 0x01] = (uint16_t)((0xFFFF + (uint32_t)ccVal) - lastcc4Val);
+
+    lastcc4Val = ccVal;
+    m4cnt++;
+    m4rpm = calcRPM(m4Time[0], m4Time[1]);
+    updateM4Cnt = 2;
+  }
+
+  if(TIM_GetITStatus(TIM5, TIM_IT_Update) == SET)
+  {
+    TIM_ClearITPendingBit(TIM5, TIM_IT_Update);
+    if (--updateM1Cnt < 0)
+    {
+      m1rpm = 0;
+    }
+    if (--updateM4Cnt < 0)
+    {
+      m4rpm = 0;
+    }
+  }
+}
+
+void __attribute__((used)) TIM3_IRQHandler(void)
+{
+  static int updateM2Cnt = 2;
+  static int updateM3Cnt = 2;
+  uint16_t ccVal;
+
+  //Motor2
+  if(TIM_GetITStatus(TIM3, TIM_IT_CC1) == SET)
+  {
+    /* Clear TIM3 Capture compare interrupt pending bit */
+    TIM_ClearITPendingBit(TIM3, TIM_IT_CC1);
+    ccVal = TIM_GetCapture1(TIM3);
+
+    if (TIM_GetFlagStatus(TIM3, TIM_FLAG_CC1OF))
+    {
+      // Overflow
+      lastcc1Val = ccVal;
+    }
+
+    if (ccVal > lastcc1Val)
+      m2Time[m2cnt & 0x01] = ccVal - lastcc1Val;
+    else
+      m2Time[m2cnt & 0x01] = (uint16_t)((0xFFFF + (uint32_t)ccVal) - lastcc1Val);
+
+    lastcc1Val = ccVal;
+    m2cnt++;
+    m2rpm = calcRPM(m2Time[0], m2Time[1]);
+    updateM2Cnt = 2;
+
+//    rpmPutchar((m2Time >> 8) & 0x00FF);
+//    rpmPutchar(m2Time & 0x00FF);
+  }
+
+  //Motor3
+  if(TIM_GetITStatus(TIM3, TIM_IT_CC2) == SET)
+  {
+    /* Clear TIM3 Capture compare interrupt pending bit */
+    TIM_ClearITPendingBit(TIM3, TIM_IT_CC2);
+    ccVal = TIM_GetCapture2(TIM3);
+
+    if (TIM_GetFlagStatus(TIM3, TIM_FLAG_CC2OF))
+    {
+      // Overflow
+      lastcc2Val = ccVal;
+    }
+
+    if (ccVal > lastcc2Val)
+      m3Time[m3cnt & 0x01] = ccVal - lastcc2Val;
+    else
+      m3Time[m3cnt & 0x01] = (uint16_t)((0xFFFF + (uint32_t)ccVal) - lastcc2Val);
+
+    lastcc2Val = ccVal;
+    m3cnt++;
+    m3rpm = calcRPM(m3Time[0], m3Time[1]);
+    updateM3Cnt = 2;
+
+//    rpmPutchar((m3rpm >> 8) & 0x00FF);
+//    rpmPutchar(m3rpm & 0x00FF);
+  }
+
+  if(TIM_GetITStatus(TIM3, TIM_IT_Update) == SET)
+  {
+    TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
+    if (--updateM2Cnt < 0)
+    {
+      m2rpm = 0;
+    }
+    if (--updateM3Cnt < 0)
+    {
+      m3rpm = 0;
+    }
+  }
+}
+
+static const DeckDriver rpm_deck = {
+  .vid = 0x00,
+  .pid = 0x00,
+  .name = "bcRpm",
+
+  .usedGpio = DECK_USING_IO_2 | DECK_USING_IO_3 | DECK_USING_PA2 | DECK_USING_PA3,
+
+  .init = rpmInit,
+};
+
+DECK_DRIVER(rpm_deck);
+
+
+LOG_GROUP_START(rpm)
+LOG_ADD(LOG_UINT16, m1, &m1rpm)
+LOG_ADD(LOG_UINT16, m2, &m2rpm)
+LOG_ADD(LOG_UINT16, m3, &m3rpm)
+LOG_ADD(LOG_UINT16, m4, &m4rpm)
+LOG_GROUP_STOP(rpm)
+
diff --git a/src/deck/drivers/src/usddeck.c b/src/deck/drivers/src/usddeck.c
index 35506e3071..3c9f8abbfe 100644
--- a/src/deck/drivers/src/usddeck.c
+++ b/src/deck/drivers/src/usddeck.c
@@ -80,7 +80,8 @@
 
 #else
 #include "deck_spi.h"
-#define USD_CS_PIN    DECK_GPIO_IO4
+#define USD_CS_PIN              DECK_GPIO_IO3
+#define USD_CS_PIN_USED         DECK_USING_IO_3
 
 #define SPI_BEGIN               spiBegin
 #define USD_SPI_BAUDRATE_2MHZ   SPI_BAUDRATE_2MHZ
@@ -90,8 +91,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   (54)
+#define MAX_USD_LOG_EVENTS                (5)
 #define FIXED_FREQUENCY_EVENT_ID          (0xFFFF)
 #define FIXED_FREQUENCY_EVENT_NAME        "fixedFrequency"
 
@@ -460,7 +461,8 @@ static void usdInit(DeckInfo *info)
     shutdownMutex = xSemaphoreCreateMutex();
 
     /* try to mount drives before creating the tasks */
-    if (f_mount(&FatFs, "", 1) == FR_OK) {
+    FRESULT r = f_mount(&FatFs, "", 1);
+    if (r == FR_OK) {
       DEBUG_PRINT("mount SD-Card [OK].\n");
 
       /* create usd-log task */
@@ -468,7 +470,7 @@ static void usdInit(DeckInfo *info)
                   USDLOG_TASK_STACKSIZE, NULL,
                   USDLOG_TASK_PRI, NULL);
     } else {
-      DEBUG_PRINT("mount SD-Card [FAIL].\n");
+      DEBUG_PRINT("mount SD-Card [FAIL] (%d).\n", r);
     }
   }
   isInit = true;
@@ -1067,7 +1069,7 @@ static const DeckDriver usd_deck = {
     .vid = 0xBC,
     .pid = 0x08,
     .name = "bcUSD",
-    .usedGpio = DECK_USING_IO_4,
+    .usedGpio = USD_CS_PIN_USED,
     .usedPeriph = DECK_USING_SPI,
     .init = usdInit,
     .test = usdTest,
diff --git a/src/drivers/interface/motors.h b/src/drivers/interface/motors.h
index 1604be9c54..0b05a33529 100644
--- a/src/drivers/interface/motors.h
+++ b/src/drivers/interface/motors.h
@@ -58,11 +58,6 @@
 #define MOTORS_TIM_DBG_CFG        DBGMCU_APB2PeriphConfig
 #define MOTORS_GPIO_AF_CFG(a,b,c) GPIO_PinAFConfig(a,b,c)
 
-// Compensate thrust depending on battery voltage so it will produce about the same
-// amount of thrust independent of the battery voltage. Based on thrust measurement.
-// Not applied for brushless motor setup.
-#define ENABLE_THRUST_BAT_COMPENSATED
-
 #ifdef CONFIG_MOTORS_ESC_PROTOCOL_ONESHOT125
 /**
  * *WARNING* Make sure the brushless driver is configured correctly as on the Crazyflie with normal
@@ -324,6 +319,12 @@ void motorsBurstDshot();
  */
 void motorsSetRatio(uint32_t id, uint16_t ratio);
 
+// computes maximum thrust in grams given the current battery state
+float motorsGetMaxThrust();
+
+// set thrust for motor (in grams)
+void motorsSetThrust(uint32_t id, float thrust);
+
 /**
  * Get the PWM ratio of the motor 'id'. Return -1 if wrong ID.
  */
diff --git a/src/drivers/src/motors.c b/src/drivers/src/motors.c
index 410571f36e..12beaedecf 100644
--- a/src/drivers/src/motors.c
+++ b/src/drivers/src/motors.c
@@ -45,6 +45,21 @@
 //Logging includes
 #include "log.h"
 #include "param.h"
+#include "math3d.h"
+
+// Data for CF14
+static float d00 = 0.5543364748044269;
+static float d10 = 0.11442589787133063;
+static float d01 = -0.5067031467944692;
+static float d20 = -0.002283966554392003;
+static float d11 = -0.03255320005438393;
+
+static float e00 = -10.291152501242268;
+static float e10 = 3.926415845326646;
+static float e01 = 26.077196474667165;
+
+static float maxThrust;
+static bool new_thrust_comp = false;
 
 static bool motorSetEnable = false;
 static uint32_t motorPower[] = {0, 0, 0, 0};    // user-requested PWM signals
@@ -93,6 +108,12 @@ const MotorHealthTestDef unknownMotorHealthTestSettings = {
 };
 
 static bool isInit = false;
+
+// Compensate thrust depending on battery voltage so it will produce about the same
+// amount of thrust independent of the battery voltage. Based on thrust measurement.
+// Not applied for brushless motor setup.
+static uint8_t batCompensation = true;
+
 static uint64_t lastCycleTime;
 static uint32_t cycleTime;
 
@@ -169,6 +190,7 @@ GPIO_InitTypeDef GPIO_PassthroughOutput =
 //
 // And to get the PWM as a percentage we would need to divide the
 // Voltage needed with the Supply voltage.
+
 static uint16_t motorsCompensateBatteryVoltage(uint16_t ithrust)
 {
   float supply_voltage = pmGetBatteryVoltage();
@@ -180,6 +202,24 @@ static uint16_t motorsCompensateBatteryVoltage(uint16_t ithrust)
    * under 2V. That would suggest a damaged battery. This protects against
    * rushing the motors on bugs and invalid voltage levels.
    */
+  if (new_thrust_comp) {
+    if (ithrust > 0) {
+      // desired thrust in grams
+      float maxThrust = motorsGetMaxThrust();
+      float maxNewton = maxThrust / 1000.0f * 9.81f;
+      float thrustNewton = ((float)ithrust / 65535.0f) * maxNewton;
+      float thrustGram = thrustNewton / 9.81f * 1000.0f;
+      // normalized voltage
+      float v = supply_voltage / 4.2f;
+      // normalized pwm:
+      float pwm = d00 + d10 * thrustGram + d01 * v + d20 * thrustGram * thrustGram + d11 * thrustGram * v;
+
+      return pwm * UINT16_MAX;
+    } else {
+      return 0;
+    }
+  }
+
   if (supply_voltage < 2.0f)
   {
     return ithrust;
@@ -299,25 +339,25 @@ void motorsDeInit(const MotorPerifDef** motorMapSelect)
 
 bool motorsTest(void)
 {
-  int i;
-
-  for (i = 0; i < sizeof(MOTORS) / sizeof(*MOTORS); i++)
-  {
-    if (motorMap[i]->drvType == BRUSHED)
-    {
-#ifdef ACTIVATE_STARTUP_SOUND
-      motorsBeep(MOTORS[i], true, testsound[i], (uint16_t)(MOTORS_TIM_BEEP_CLK_FREQ / A4)/ 20);
-      vTaskDelay(M2T(MOTORS_TEST_ON_TIME_MS));
-      motorsBeep(MOTORS[i], false, 0, 0);
-      vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME_MS));
-#else
-      motorsSetRatio(MOTORS[i], MOTORS_TEST_RATIO);
-      vTaskDelay(M2T(MOTORS_TEST_ON_TIME_MS));
-      motorsSetRatio(MOTORS[i], 0);
-      vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME_MS));
-#endif
-    }
-  }
+  // int i;
+
+//   for (i = 0; i < sizeof(MOTORS) / sizeof(*MOTORS); i++)
+//   {
+//     if (motorMap[i]->drvType == BRUSHED)
+//     {
+// #ifdef ACTIVATE_STARTUP_SOUND
+//       motorsBeep(MOTORS[i], true, testsound[i], (uint16_t)(MOTORS_TIM_BEEP_CLK_FREQ / A4)/ 20);
+//       vTaskDelay(M2T(MOTORS_TEST_ON_TIME_MS));
+//       motorsBeep(MOTORS[i], false, 0, 0);
+//       vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME_MS));
+// #else
+//       motorsSetRatio(MOTORS[i], MOTORS_TEST_RATIO);
+//       vTaskDelay(M2T(MOTORS_TEST_ON_TIME_MS));
+//       motorsSetRatio(MOTORS[i], 0);
+//       vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME_MS));
+// #endif
+//     }
+//   }
 
   return isInit;
 }
@@ -449,7 +489,7 @@ void motorsBurstDshot()
 #endif
 
 
-// Ithrust is thrust mapped for 65536 <==> 60 grams
+// Ithrust is thrust mapped for 65536 <==> 15 grams (per rotor)
 void motorsSetRatio(uint32_t id, uint16_t ithrust)
 {
   if (isInit) {
@@ -459,15 +499,16 @@ void motorsSetRatio(uint32_t id, uint16_t ithrust)
 
     motorPower[id] = ithrust;
 
-#ifdef ENABLE_THRUST_BAT_COMPENSATED
-    if (motorMap[id]->drvType == BRUSHED)
+    if (batCompensation)
     {
-      // To make sure we provide the correct PWM given current supply voltage
-      // from the battery, we do calculations based on measurements of PWM,
-      // voltage and thrust. See comment at function definition for details.
-      ratio = motorsCompensateBatteryVoltage(ithrust);
+      if (motorMap[id]->drvType == BRUSHED)
+      {
+        // To make sure we provide the correct PWM given current supply voltage
+        // from the battery, we do calculations based on measurements of PWM,
+        // voltage and thrust. See comment at function definition for details.
+        ratio = motorsCompensateBatteryVoltage(ithrust);
+      }
     }
-#endif
 
     motor_ratios[id] = ratio;
     if (motorSetEnable) {
@@ -569,6 +610,42 @@ int motorsGetRatio(uint32_t id)
   return motor_ratios[id];
 }
 
+
+// computes maximum thrust in grams given the current battery state
+float motorsGetMaxThrust()
+{
+  // normalized voltage
+  float v = pmGetBatteryVoltage() / 4.2f;
+  // normalized pwm
+  float pwm = (motor_ratios[0] + motor_ratios[1] + motor_ratios[2] + motor_ratios[3]) / 4.0f / UINT16_MAX;
+
+  maxThrust = clamp(e00 + e10 * pwm + e01 * v, 8, 50);
+
+  return maxThrust;
+}
+
+// set thrust for motor (in grams)
+void motorsSetThrust(uint32_t id, float thrustGram)
+{
+  if (motorMap[id]->drvType == BRUSHED)
+  {
+    if (thrustGram > 0) {
+      // normalized voltage
+      float v = pmGetBatteryVoltage() / 4.2f;
+      // normalized pwm:
+      float pwm = d00 + d10 * thrustGram + d01 * v + d20 * thrustGram * thrustGram + d11 * thrustGram * v;
+
+      motor_ratios[id] = pwm * UINT16_MAX;
+    } else {
+      motor_ratios[id] = 0;
+    }
+
+    motorMap[id]->setCompare(motorMap[id]->tim, motorsConv16ToBits(motor_ratios[id]));
+  } else {
+    ASSERT(false);
+  }
+}
+
 void motorsBeep(int id, bool enable, uint16_t frequency, uint16_t ratio)
 {
   TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
@@ -740,21 +817,40 @@ LOG_GROUP_START(pwm)
 /**
  * @brief Current motor 1 PWM output
  */
-LOG_ADD(LOG_UINT32, m1_pwm, &motor_ratios[0])
+LOG_ADD(LOG_UINT16, m1_pwm, &motor_ratios[0])
 /**
  * @brief Current motor 2 PWM output
  */
-LOG_ADD(LOG_UINT32, m2_pwm, &motor_ratios[1])
+LOG_ADD(LOG_UINT16, m2_pwm, &motor_ratios[1])
 /**
  * @brief Current motor 3 PWM output
  */
-LOG_ADD(LOG_UINT32, m3_pwm, &motor_ratios[2])
+LOG_ADD(LOG_UINT16, m3_pwm, &motor_ratios[2])
 /**
  * @brief Current motor 4 PWM output
  */
-LOG_ADD(LOG_UINT32, m4_pwm, &motor_ratios[3])
+LOG_ADD(LOG_UINT16, m4_pwm, &motor_ratios[3])
 /**
  * @brief Cycle time of M1 output in microseconds
  */
 LOG_ADD(LOG_UINT32, cycletime, &cycleTime)
+
+LOG_ADD(LOG_FLOAT, maxThrust, &maxThrust)
 LOG_GROUP_STOP(pwm)
+
+PARAM_GROUP_START(pwm)
+
+  PARAM_ADD(PARAM_FLOAT, d00, &d00)
+  PARAM_ADD(PARAM_FLOAT, d10, &d10)
+  PARAM_ADD(PARAM_FLOAT, d01, &d01)
+  PARAM_ADD(PARAM_FLOAT, d20, &d20)
+  PARAM_ADD(PARAM_FLOAT, d11, &d11)
+
+  PARAM_ADD(PARAM_FLOAT, e00, &e00)
+  PARAM_ADD(PARAM_FLOAT, e10, &e10)
+  PARAM_ADD(PARAM_FLOAT, e01, &e01)
+  
+  PARAM_ADD(PARAM_UINT8, new_thrust_comp, &new_thrust_comp)
+
+
+PARAM_GROUP_STOP(pwm)
\ No newline at end of file
diff --git a/src/drivers/src/uart1.c b/src/drivers/src/uart1.c
index 51477ee4f5..d0663adaa0 100644
--- a/src/drivers/src/uart1.c
+++ b/src/drivers/src/uart1.c
@@ -42,6 +42,7 @@
 #include "config.h"
 #include "nvicconf.h"
 #include "static_mem.h"
+#include "debug.h"
 
 /** This uart is conflicting with SPI2 DMA used in sensors_bmi088_spi_bmp388.c
  *  which is used in CF-Bolt. So for other products this can be enabled.
@@ -205,10 +206,10 @@ bool uart1GetDataWithDefaultTimeout(uint8_t *c)
 void uart1SendData(uint32_t size, uint8_t* data)
 {
   uint32_t i;
-
-  if (!isInit)
-    return;
-
+  if (!isInit){
+    DEBUG_PRINT("DO NOT WORK \n");
+    return; 
+  }
   for(i = 0; i < size; i++)
   {
     while (!(UART1_TYPE->SR & USART_FLAG_TXE));
diff --git a/src/hal/interface/usec_time.h b/src/hal/interface/usec_time.h
index 8a48ea28ff..6c8160e934 100644
--- a/src/hal/interface/usec_time.h
+++ b/src/hal/interface/usec_time.h
@@ -32,6 +32,11 @@
  */
 void initUsecTimer(void);
 
+/**
+ * Reset the microsecond-resolution timer to 0.
+ */
+void usecTimerReset(void);
+
 /**
  * Get microsecond-resolution timestamp.
  */
diff --git a/src/hal/src/usec_time.c b/src/hal/src/usec_time.c
index 6971d308ea..66215a13ee 100644
--- a/src/hal/src/usec_time.c
+++ b/src/hal/src/usec_time.c
@@ -27,11 +27,13 @@
 #include <stdbool.h>
 #include "usec_time.h"
 #include "cfassert.h"
+#include "param.h"
 
 #include "nvicconf.h"
 #include "stm32fxxx.h"
 
 static bool isInit = false;
+static uint8_t reset = 0;
 static uint32_t usecTimerHighCount;
 
 void initUsecTimer(void)
@@ -71,6 +73,16 @@ void initUsecTimer(void)
   isInit = true;
 }
 
+void usecTimerReset(void)
+{
+  IF_DEBUG_ASSERT(isInit);
+
+  const uint32_t zero = 0;
+  __atomic_store(&usecTimerHighCount, &zero, __ATOMIC_SEQ_CST);
+
+  TIM7->CNT = 0;
+}
+
 uint64_t usecTimestamp(void)
 {
   IF_DEBUG_ASSERT(isInit);
@@ -96,3 +108,20 @@ void __attribute__((used)) TIM7_IRQHandler(void)
 
   __sync_fetch_and_add(&usecTimerHighCount, 1);
 }
+
+/**
+ * Parameters to synchronize time
+ * */
+
+static void resetParamCallback(void)
+{
+  if (reset) {
+    usecTimerReset();
+    reset = 0;
+  }
+}
+
+
+PARAM_GROUP_START(usec)
+PARAM_ADD_WITH_CALLBACK(PARAM_UINT8, reset, &reset, &resetParamCallback)
+PARAM_GROUP_STOP(usec)
\ No newline at end of file
diff --git a/src/modules/interface/controller.h b/src/modules/interface/controller.h
index a5042c00b8..8fec622140 100644
--- a/src/modules/interface/controller.h
+++ b/src/modules/interface/controller.h
@@ -33,6 +33,10 @@ typedef enum {
   ControllerTypePID,
   ControllerTypeMellinger,
   ControllerTypeINDI,
+  ControllerTypeSJC,
+  ControllerTypeMellingerSI,
+  ControllerTypeLee,
+  ControllerTypeLeePayload,
   ControllerType_COUNT,
 } ControllerType;
 
diff --git a/src/modules/interface/controller_lee.h b/src/modules/interface/controller_lee.h
new file mode 100644
index 0000000000..d61bd50409
--- /dev/null
+++ b/src/modules/interface/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, 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, 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/controller_lee_payload.h b/src/modules/interface/controller_lee_payload.h
new file mode 100644
index 0000000000..1f664b0dfe
--- /dev/null
+++ b/src/modules/interface/controller_lee_payload.h
@@ -0,0 +1,98 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 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_PAYLOAD_H__
+#define __CONTROLLER_LEE_PAYLOAD_H__
+
+#include "stabilizer_types.h"
+
+// This structure contains the mutable state and inmutable parameters
+typedef struct controllerLeePayload_s {
+    float mass; // TODO: should be CF global for other modules
+    float mp; // mass payload
+    float l; // length of cable;
+    float thrustSI;
+    struct vec J; // Inertia matrix (diagonal matrix); kg m^2
+    float offset; // offset for reference
+    //Position PID
+    struct vec Kpos_P;
+    float Kpos_P_limit;
+    struct vec Kpos_D;
+    float Kpos_D_limit;
+    struct vec Kpos_I;
+    float Kpos_I_limit;
+
+    struct vec i_error_pos;
+    struct vec i_error_att;
+
+   // Cable PD 
+    struct vec K_q;
+    struct vec K_w;
+
+   // Cable errors
+    struct vec plp_error;
+    struct vec plv_error;
+
+   //Attitude PID
+    struct vec KR;
+    struct vec Komega;
+    struct vec KI;
+
+    // Logging variables
+    struct vec qi_prev;
+    struct vec qdi_prev;
+    struct vec payload_vel_prev;
+    struct vec rpy;
+    struct vec rpy_des;
+    struct vec qr;
+    struct mat33 R_des;
+    struct quat q;
+    struct mat33 R;
+    struct vec omega;
+    struct vec omega_r;
+    struct vec u;
+    struct vec u_i;
+    struct vec qidot_prev;
+    struct vec acc_prev;
+
+} controllerLeePayload_t;
+
+
+void controllerLeePayloadInit(controllerLeePayload_t* self);
+void controllerLeePayloadReset(controllerLeePayload_t* self);
+void controllerLeePayload(controllerLeePayload_t* self, control_t *control, setpoint_t *setpoint,
+                                         const sensorData_t *sensors,
+                                         const state_t *state,
+                                         const uint32_t tick);
+
+#ifdef CRAZYFLIE_FW
+void controllerLeePayloadFirmwareInit(void);
+bool controllerLeePayloadFirmwareTest(void);
+void controllerLeePayloadFirmware(control_t *control, setpoint_t *setpoint,
+                                         const sensorData_t *sensors,
+                                         const state_t *state,
+                                         const uint32_t tick);
+#endif //   CRAZYFLIE_FW defined
+
+#endif //__CONTROLLER_LEE_PAYLOAD_H__
diff --git a/src/modules/interface/controller_mellingerSI.h b/src/modules/interface/controller_mellingerSI.h
new file mode 100644
index 0000000000..d53fcb619f
--- /dev/null
+++ b/src/modules/interface/controller_mellingerSI.h
@@ -0,0 +1,38 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 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/>.
+ *
+ * controller_mellingerSI.h - Mellinger Controller Interface
+ */
+#ifndef __CONTROLLER_MELLINGER_SI_H__
+#define __CONTROLLER_MELLINGER_SI_H__
+
+#include "stabilizer_types.h"
+
+void controllerMellingerSIInit(void);
+bool controllerMellingerSITest(void);
+void controllerMellingerSI(control_t *control, setpoint_t *setpoint,
+                                         const sensorData_t *sensors,
+                                         const state_t *state,
+                                         const uint32_t tick);
+
+#endif //__CONTROLLER_MELLINGER_SI_H__
diff --git a/src/modules/interface/controller_sjc.h b/src/modules/interface/controller_sjc.h
new file mode 100644
index 0000000000..7451597f9c
--- /dev/null
+++ b/src/modules/interface/controller_sjc.h
@@ -0,0 +1,16 @@
+/**
+ * controller_sjc.h - Controller Interface for nonlinear controller
+ */
+#ifndef __CONTROLLER_SJC_H__
+#define __CONTROLLER_SJC_H__
+
+#include "stabilizer_types.h"
+
+void controllerSJCInit(void);
+bool controllerSJCTest(void);
+void controllerSJC(control_t *control, setpoint_t *setpoint,
+                    const sensorData_t *sensors,
+                    const state_t *state,
+                    const uint32_t tick);
+
+#endif //__CONTROLLER_SJC_H__
diff --git a/src/modules/interface/math3d.h b/src/modules/interface/math3d.h
index 260fe3ff62..3aa96872dc 100644
--- a/src/modules/interface/math3d.h
+++ b/src/modules/interface/math3d.h
@@ -293,6 +293,11 @@ static inline struct vec vadd4(struct vec a, struct vec b, struct vec c, struct
 	// TODO: make sure it compiles to optimal code
 	return vadd(vadd(a, b), vadd(c, d));
 }
+// add 5 vectors.
+static inline struct vec vadd5(struct vec a, struct vec b, struct vec c, struct vec d, struct vec e) {
+	// TODO: make sure it compiles to optimal code
+	return vadd(vadd(vadd(vadd(a, b), c), d), e);
+}
 // subtract b and c from a.
 static inline struct vec vsub2(struct vec a, struct vec b, struct vec c) {
 	return vadd3(a, vneg(b), vneg(c));
@@ -377,6 +382,24 @@ static inline struct mat33 mcolumns(struct vec a, struct vec b, struct vec c) {
 
 	return m;
 }
+// construct a matrix from two vectors A = q*q.T 
+static inline struct mat33 vecmult(struct vec a) {
+	struct mat33 m;
+	m.m[0][0] = a.x*a.x;
+	m.m[1][0] = a.y*a.x;
+	m.m[2][0] = a.z*a.x;
+
+	m.m[0][1] = a.x*a.y;
+	m.m[1][1] = a.y*a.y;
+	m.m[2][1] = a.z*a.y;
+
+	m.m[0][2] = a.x*a.x;
+	m.m[1][2] = a.y*a.z;
+	m.m[2][2] = a.z*a.z;
+
+	return m;
+}
+
 // construct a matrix from three row vectors.
 static inline struct mat33 mrows(struct vec a, struct vec b, struct vec c) {
 	struct mat33 m;
diff --git a/src/modules/interface/power_distribution.h b/src/modules/interface/power_distribution.h
index 6a7a17405b..226b1f42e9 100644
--- a/src/modules/interface/power_distribution.h
+++ b/src/modules/interface/power_distribution.h
@@ -31,7 +31,7 @@
 
 void powerDistributionInit(void);
 bool powerDistributionTest(void);
-void powerDistribution(motors_thrust_t* motorPower, const control_t *control);
+void powerDistribution(motors_thrust_t* motorPower, const control_t *control, float maxThrust);
 
 /**
  * Returns a 1 when motor 'id' gives thrust, returns 0 otherwise
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 b7d3ac0be4..87e80acd2b 100644
--- a/src/modules/interface/stabilizer_types.h
+++ b/src/modules/interface/stabilizer_types.h
@@ -62,6 +62,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 {
@@ -125,7 +126,8 @@ typedef struct poseMeasurement_s {
   };
   quaternion_t quat;
   float stdDevPos;
-  float stdDevQuat;
+  float stdDevRollPitch;
+  float stdDevYaw;
 } poseMeasurement_t;
 
 typedef struct distanceMeasurement_s {
@@ -165,20 +167,58 @@ typedef struct state_s {
   point_t position;         // m
   velocity_t velocity;      // m/s
   acc_t acc;                // Gs (but acc.z without considering gravity)
+
+  // Measured state of the payload
+  point_t payload_pos;         // m   (world frame)
+  velocity_t payload_vel;      // m/s (world frame)
 } state_t;
 
+typedef enum control_mode_e {
+  controlModeLegacy      = 0, // legacy mode with int16_t roll, pitch, yaw and float thrust
+  controlModeForceTorque = 1,
+  controlModeForce       = 2,
+} control_mode_t;
+
 typedef struct control_s {
-  int16_t roll;
-  int16_t pitch;
-  int16_t yaw;
-  float thrust;
+  union {
+    struct {
+      // legacy part
+      int16_t roll;
+      int16_t pitch;
+      int16_t yaw;
+      float thrust;
+    };
+    struct {
+      float thrustSI;  // N
+      float torque[3]; // Nm
+      float u_all[3];
+    };
+    float normalizedForces[4]; // 0 ... 1
+  };
+  control_mode_t controlMode;
 } control_t;
 
+typedef enum motors_thrust_mode_e {
+  motorsThrustModePWM    = 0,
+  motorsThrustModeForce  = 1,
+} motors_thrust_mode_t;
+
 typedef struct motors_thrust_s {
-  uint16_t m1;  // PWM ratio
-  uint16_t m2;  // PWM ratio
-  uint16_t m3;  // PWM ratio
-  uint16_t m4;  // PWM ratio
+  union {
+    struct {
+      uint16_t m1;  // PWM ratio
+      uint16_t m2;  // PWM ratio
+      uint16_t m3;  // PWM ratio
+      uint16_t m4;  // PWM ratio
+    };
+    struct {
+      float f1;  // force in grams
+      float f2;  // force in grams
+      float f3;  // force in grams
+      float f4;  // force in grams
+    };
+  };
+  motors_thrust_mode_t mode;
 } motors_thrust_t;
 
 typedef enum mode_e {
@@ -197,6 +237,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/Kbuild b/src/modules/src/Kbuild
index c361c0f3f9..fcdfe3776e 100644
--- a/src/modules/src/Kbuild
+++ b/src/modules/src/Kbuild
@@ -8,9 +8,13 @@ obj-y += commander.o
 obj-y += comm.o
 obj-y += console.o
 obj-y += controller_indi.o
+obj-y += controller_lee.o
+obj-y += controller_lee_payload.o
 obj-y += controller_mellinger.o
+obj-y += controller_mellingerSI.o
 obj-y += controller.o
 obj-y += controller_pid.o
+obj-y += controller_sjc.o
 obj-y += crtp_commander_generic.o
 obj-y += crtp_commander_high_level.o
 obj-y += crtp_commander.o
@@ -72,3 +76,6 @@ obj-$(CONFIG_ESTIMATOR_KALMAN_ENABLE) += kalman_core/
 
 # Lighthouse
 obj-y += lighthouse/
+
+# App
+obj-y += cvmrs.o
\ No newline at end of file
diff --git a/src/modules/src/attitude_pid_controller.c b/src/modules/src/attitude_pid_controller.c
index 92aa365c35..6ebcbbe248 100644
--- a/src/modules/src/attitude_pid_controller.c
+++ b/src/modules/src/attitude_pid_controller.c
@@ -153,6 +153,11 @@ void attitudeControllerCorrectRatePID(
   yawOutput = saturateSignedInt16(pidUpdate(&pidYawRate, yawRateActual, true));
 }
 
+static float my_mod(float a, float n)
+{
+  return a - floorf(a/n) * n;
+}
+
 void attitudeControllerCorrectAttitudePID(
        float eulerRollActual, float eulerPitchActual, float eulerYawActual,
        float eulerRollDesired, float eulerPitchDesired, float eulerYawDesired,
@@ -168,10 +173,8 @@ void attitudeControllerCorrectAttitudePID(
   // Update PID for yaw axis
   float yawError;
   yawError = eulerYawDesired - eulerYawActual;
-  if (yawError > 180.0f)
-    yawError -= 360.0f;
-  else if (yawError < -180.0f)
-    yawError += 360.0f;
+  // see https://stackoverflow.com/questions/1878907/how-can-i-find-the-difference-between-two-angles
+  yawError = my_mod(yawError + 180, 360) - 180;
   pidSetError(&pidYaw, yawError);
   *yawRateDesired = pidUpdate(&pidYaw, eulerYawActual, false);
 }
diff --git a/src/modules/src/controller.c b/src/modules/src/controller.c
index f18c19ff5e..9f73f61c96 100644
--- a/src/modules/src/controller.c
+++ b/src/modules/src/controller.c
@@ -6,6 +6,10 @@
 #include "controller_pid.h"
 #include "controller_mellinger.h"
 #include "controller_indi.h"
+#include "controller_sjc.h"
+#include "controller_mellingerSI.h"
+#include "controller_lee.h"
+#include "controller_lee_payload.h"
 
 #include "autoconf.h"
 
@@ -26,6 +30,10 @@ static ControllerFcns controllerFunctions[] = {
   {.init = controllerPidInit, .test = controllerPidTest, .update = controllerPid, .name = "PID"},
   {.init = controllerMellingerInit, .test = controllerMellingerTest, .update = controllerMellinger, .name = "Mellinger"},
   {.init = controllerINDIInit, .test = controllerINDITest, .update = controllerINDI, .name = "INDI"},
+  {.init = controllerSJCInit, .test = controllerSJCTest, .update = controllerSJC, .name = "SJC"},
+  {.init = controllerMellingerSIInit, .test = controllerMellingerSITest, .update = controllerMellingerSI, .name = "MellingerSI"},
+  {.init = controllerLeeFirmwareInit, .test = controllerLeeFirmwareTest, .update = controllerLeeFirmware, .name = "Lee"},
+  {.init = controllerLeePayloadFirmwareInit, .test = controllerLeePayloadFirmwareTest, .update = controllerLeePayloadFirmware, .name = "LeePayload"},
 };
 
 
diff --git a/src/modules/src/controller_lee.c b/src/modules/src/controller_lee.c
new file mode 100644
index 0000000000..a92036bed7
--- /dev/null
+++ b/src/modules/src/controller_lee.c
@@ -0,0 +1,385 @@
+/*
+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, 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);
+
+  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, 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/controller_lee_payload.c b/src/modules/src/controller_lee_payload.c
new file mode 100644
index 0000000000..10a6ee61b2
--- /dev/null
+++ b/src/modules/src/controller_lee_payload.c
@@ -0,0 +1,451 @@
+/*
+The MIT License (MIT)
+
+Copyright (c) 2022 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.
+*/
+
+/*
+This controller is based on the following publication:
+
+TODO
+*/
+
+#include <math.h>
+#include <string.h>
+#include "math3d.h"
+#include "controller_lee_payload.h"
+
+#define GRAVITY_MAGNITUDE (9.81f)
+
+static controllerLeePayload_t g_self = {
+  .mass = 0.034,
+  .mp   = 0.01,
+  .l    = 0.6,
+  .offset = 0.0,
+  // 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 = {18, 18, 18},
+  .Kpos_P_limit = 100,
+  .Kpos_D = {15, 15, 15},
+  .Kpos_D_limit = 100,
+  .Kpos_I ={0, 0, 0},
+  .Kpos_I_limit = 100,
+
+  // Cables PD
+  .K_q = {25, 25, 25},
+  .K_w = {24, 24, 24},
+
+  //Attitude PID 
+  .KR = {0.008, 0.008, 0.01},
+  .Komega = {0.0013, 0.0013, 0.002},
+  .KI = {0.02, 0.02, 0.05},
+
+};
+
+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 controllerLeePayloadReset(controllerLeePayload_t* self)
+{
+  self->i_error_pos = vzero();
+  self->i_error_att = vzero();
+  self->qi_prev = mkvec(0,0,-1);
+  self->qidot_prev = vzero();
+  self->acc_prev   = vzero();
+  self->payload_vel_prev = vzero();
+  self->qdi_prev = vzero();
+}
+
+void controllerLeePayloadInit(controllerLeePayload_t* self)
+{
+  // copy default values (bindings), or NOP (firmware)
+  *self = g_self;
+
+  controllerLeePayloadReset(self);
+}
+
+bool controllerLeePayloadTest(controllerLeePayload_t* self)
+{
+  return true;
+}
+
+void controllerLeePayload(controllerLeePayload_t* self, control_t *control, 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);
+  // 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 plPos_d = mkvec(setpoint->position.x, setpoint->position.y, setpoint->position.z-self->offset);
+    struct vec plVel_d = mkvec(setpoint->velocity.x, setpoint->velocity.y, setpoint->velocity.z);
+    struct vec plAcc_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);
+    struct vec plStPos = mkvec(state->payload_pos.x, state->payload_pos.y, state->payload_pos.z);
+    struct vec plStVel = mkvec(state->payload_vel.x, state->payload_vel.y, state->payload_vel.z);
+
+    // errors
+    struct vec plpos_e = vclampscl(vsub(plPos_d, plStPos), -self->Kpos_P_limit, self->Kpos_P_limit);
+    struct vec plvel_e = vclampscl(vsub(plVel_d, plStVel), -self->Kpos_D_limit, self->Kpos_D_limit);
+
+    self->plp_error = plpos_e;
+    self->plv_error = plvel_e;
+
+    struct vec F_d =vscl(self->mp ,vadd3(
+      plAcc_d,
+      veltmul(self->Kpos_P, plpos_e),
+      veltmul(self->Kpos_D, plvel_e)));
+
+    struct vec desVirtInp = F_d;
+    //directional unit vector qi and angular velocity wi pointing from UAV to payload
+    struct vec qi = vnormalize(vsub(plStPos, statePos)); 
+
+    struct vec qidot = vdiv(vsub(plStVel, stateVel), vmag(vsub(plStPos, statePos)));
+    struct vec wi = vcross(qi, qidot);
+    struct mat33 qiqiT = vecmult(qi);
+    struct vec virtualInp = mvmul(qiqiT, desVirtInp);
+    
+    // Compute parallel component
+    struct vec acc_ = plAcc_d;
+
+    struct vec u_parallel = vadd3(virtualInp, vscl(self->mass*self->l*vmag2(wi), qi), vscl(self->mass, mvmul(qiqiT, acc_)));
+    
+    // Compute Perpindicular Component
+    struct vec qdi = vneg(vnormalize(desVirtInp));
+    struct vec eq  = vcross(qdi, qi);
+    struct mat33 skewqi = mcrossmat(qi);
+    struct mat33 skewqi2 = mmul(skewqi,skewqi);
+
+    // struct vec qdidot = vdiv(vsub(qdi, qdi_prev), dt);
+    struct vec qdidot = vzero();
+    self->qdi_prev = qdi;
+    struct vec wdi = vcross(qdi, qdidot);
+    struct vec ew = vadd(wi, mvmul(skewqi2, wdi));
+
+    struct vec u_perpind = vsub(
+      vscl(self->mass*self->l, mvmul(skewqi, vsub(vneg(veltmul(self->K_q, eq)), veltmul(self->K_w, ew)))),
+      vscl(self->mass, mvmul(skewqi2, acc_))
+    );
+
+    self->u_i = vadd(u_parallel, u_perpind);
+    
+    control->thrustSI = vmag(self->u_i);
+    control->u_all[0] = self->u_i.x;
+    control->u_all[1] = self->u_i.y;
+    control->u_all[2] = self->u_i.z;
+
+
+    self->thrustSI = control->thrustSI;
+  //  Reset the accumulated error while on the ground
+    if (control->thrustSI < 0.01f) {
+      controllerLeePayloadReset(self);
+    }
+  
+  self->q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w);
+  self->rpy = quat2rpy(self->q);
+  self->R = quat2rotmat(self->q);
+
+  // Compute Desired Rotation matrix
+    struct vec Fd_ = self->u_i;
+    struct vec xdes = vbasis(0);
+    struct vec ydes = vbasis(1);
+    struct vec zdes = vbasis(2);
+   
+    if (self->thrustSI > 0) {
+      zdes = vnormalize(Fd_);
+    } 
+    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;
+          controllerLeePayloadReset(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]
+  self->q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w);
+  self->rpy = quat2rpy(self->q);
+  self->R = quat2rotmat(self->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), self->R), mmul(mtranspose(self->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);
+
+  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(self->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 controllerLeePayloadFirmwareInit(void)
+{
+  controllerLeePayloadInit(&g_self);
+}
+
+bool controllerLeePayloadFirmwareTest(void) 
+{
+  return true;
+}
+
+void controllerLeePayloadFirmware(control_t *control, setpoint_t *setpoint,
+                                         const sensorData_t *sensors,
+                                         const state_t *state,
+                                         const uint32_t tick)
+{
+  controllerLeePayload(&g_self, control, setpoint, sensors, state, tick);  
+}
+
+PARAM_GROUP_START(ctrlLeeP)
+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 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)
+
+// Attitude P
+PARAM_ADD(PARAM_FLOAT, KRx, &g_self.KR.x)
+PARAM_ADD(PARAM_FLOAT, KRy, &g_self.KR.y)
+PARAM_ADD(PARAM_FLOAT, KRz, &g_self.KR.z)
+
+// Attitude D
+PARAM_ADD(PARAM_FLOAT, Komx, &g_self.Komega.x)
+PARAM_ADD(PARAM_FLOAT, Komy, &g_self.Komega.y)
+PARAM_ADD(PARAM_FLOAT, Komz, &g_self.Komega.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)
+
+// Cable P
+PARAM_ADD(PARAM_FLOAT, Kqx, &g_self.K_q.x)
+PARAM_ADD(PARAM_FLOAT, Kqy, &g_self.K_q.y)
+PARAM_ADD(PARAM_FLOAT, Kqz, &g_self.K_q.z)
+
+// Cable D
+PARAM_ADD(PARAM_FLOAT, Kwx, &g_self.K_w.x)
+PARAM_ADD(PARAM_FLOAT, Kwy, &g_self.K_w.y)
+PARAM_ADD(PARAM_FLOAT, Kwz, &g_self.K_w.z)
+
+PARAM_ADD(PARAM_FLOAT, mass, &g_self.mass)
+PARAM_ADD(PARAM_FLOAT, massP, &g_self.mp)
+PARAM_ADD(PARAM_FLOAT, length, &g_self.l)
+PARAM_ADD(PARAM_FLOAT, offset, &g_self.offset)
+
+PARAM_GROUP_STOP(ctrlLeeP)
+
+
+LOG_GROUP_START(ctrlLeeP)
+
+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, Kqx, &g_self.K_q.x)
+LOG_ADD(LOG_FLOAT, Kqy, &g_self.K_q.y)
+LOG_ADD(LOG_FLOAT, Kqz, &g_self.K_q.z)
+
+LOG_ADD(LOG_FLOAT, Kwx, &g_self.K_w.x)
+LOG_ADD(LOG_FLOAT, Kwy, &g_self.K_w.y)
+LOG_ADD(LOG_FLOAT, Kwz, &g_self.K_w.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)
+
+// 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_FLOAT, ux, &g_self.u_i.x)
+LOG_ADD(LOG_FLOAT, uy, &g_self.u_i.y)
+LOG_ADD(LOG_FLOAT, uz, &g_self.u_i.z)
+
+
+// LOG_ADD(LOG_UINT32, ticks, &ticks)
+
+LOG_GROUP_STOP(ctrlLeeP)
+
+#endif // CRAZYFLIE_FW defined
+
+
+               
+
+
+          
diff --git a/src/modules/src/controller_mellinger.c b/src/modules/src/controller_mellinger.c
index f45206a85d..dd392df8ec 100644
--- a/src/modules/src/controller_mellinger.c
+++ b/src/modules/src/controller_mellinger.c
@@ -101,6 +101,10 @@ static float r_pitch;
 static float r_yaw;
 static float accelz;
 
+static struct vec r_error;
+static struct vec v_error;
+static struct vec eR, ew;
+
 void controllerMellingerReset(void)
 {
   i_error_x = 0;
@@ -126,15 +130,15 @@ void controllerMellinger(control_t *control, setpoint_t *setpoint,
                                          const state_t *state,
                                          const uint32_t tick)
 {
-  struct vec r_error;
-  struct vec v_error;
+  // struct vec r_error;
+  // struct vec v_error;
   struct vec target_thrust;
   struct vec z_axis;
   float current_thrust;
   struct vec x_axis_desired;
   struct vec y_axis_desired;
   struct vec x_c_des;
-  struct vec eR, ew, M;
+  struct vec /*eR, ew,*/ M;
   float dt;
   float desiredYaw = 0; //deg
 
@@ -358,4 +362,20 @@ LOG_ADD(LOG_FLOAT, zdz, &z_axis_desired.z)
 LOG_ADD(LOG_FLOAT, i_err_x, &i_error_x)
 LOG_ADD(LOG_FLOAT, i_err_y, &i_error_y)
 LOG_ADD(LOG_FLOAT, i_err_z, &i_error_z)
+
+LOG_ADD(LOG_FLOAT, ep_x, &r_error.x)
+LOG_ADD(LOG_FLOAT, ep_y, &r_error.y)
+LOG_ADD(LOG_FLOAT, ep_z, &r_error.z)
+
+LOG_ADD(LOG_FLOAT, ev_x, &v_error.x)
+LOG_ADD(LOG_FLOAT, ev_y, &v_error.y)
+LOG_ADD(LOG_FLOAT, ev_z, &v_error.z)
+
+LOG_ADD(LOG_FLOAT, eR_x, &eR.x)
+LOG_ADD(LOG_FLOAT, eR_y, &eR.y)
+LOG_ADD(LOG_FLOAT, eR_z, &eR.z)
+
+LOG_ADD(LOG_FLOAT, ew_x, &ew.x)
+LOG_ADD(LOG_FLOAT, ew_y, &ew.y)
+LOG_ADD(LOG_FLOAT, ew_z, &ew.z)
 LOG_GROUP_STOP(ctrlMel)
diff --git a/src/modules/src/controller_mellingerSI.c b/src/modules/src/controller_mellingerSI.c
new file mode 100644
index 0000000000..09081fba4c
--- /dev/null
+++ b/src/modules/src/controller_mellingerSI.c
@@ -0,0 +1,313 @@
+/*
+The MIT License (MIT)
+
+Copyright (c) 2018 Wolfgang Hoenig and James Alan Preiss
+
+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:
+
+Daniel Mellinger, Vijay Kumar:
+Minimum snap trajectory generation and control for quadrotors.
+IEEE International Conference on Robotics and Automation (ICRA), 2011.
+
+We added the following:
+ * Integral terms (compensates for: battery voltage drop over time, unbalanced center of mass due to asymmmetries, and uneven wear on propellers and motors)
+ * Support to use this controller as an attitude-only controller for manual flight
+
+ * This version uses SI units and math3d
+
+
+TODO
+ * switch position controller
+ * remove integral part from attitude controller
+ * Tune yaw
+
+*/
+
+#include <math.h>
+
+#include "param.h"
+#include "log.h"
+#include "math3d.h"
+#include "controller_mellingerSI.h"
+#include "usec_time.h"
+// #include "debug.h"
+#include "power_distribution.h"
+
+#define GRAVITY_MAGNITUDE (9.81f)
+
+static float g_vehicleMass = 0.033; // TODO: should be CF global for other modules
+
+// Position PID
+static struct vec Kpos_P = {6, 6, 6}; // Kp in paper
+static float Kpos_P_limit = 100;
+static struct vec Kpos_D = {4, 4, 4}; // Kv in paper
+static float Kpos_D_limit = 100;
+static struct vec Kpos_I = {0, 0, 0}; // not in paper
+static float Kpos_I_limit = 2;
+static struct vec i_error_pos;
+
+// Attitude PID
+static struct vec KR = {10, 10, 10};
+static struct vec Komega = {0.0005, 0.0005, 0.001};
+
+// Attitude I on omega
+static struct vec Katt_I = {0.00025, 0.00025, 0.0005};
+static float Katt_I_limit = 2;
+static struct vec i_error_att;
+
+// Logging variables
+static struct vec rpy;
+static struct vec rpy_des;
+static struct vec qr;
+static struct vec omega;
+static struct vec omega_des;
+static struct vec u;
+
+static uint32_t ticks;
+
+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 controllerMellingerSIReset(void)
+{
+  i_error_pos = vzero();
+  i_error_att = vzero();
+}
+
+void controllerMellingerSIInit(void)
+{
+  controllerMellingerSIReset();
+}
+
+bool controllerMellingerSITest(void)
+{
+  return true;
+}
+
+void controllerMellingerSI(control_t *control, 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);
+
+  // 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);
+    rpy_des = quat2rpy(setpoint_quat);
+    desiredYaw = rpy_des.z;
+  }
+
+  // qr: Desired/reference angles in rad
+  // struct vec qr;
+
+  // 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), -Kpos_P_limit, Kpos_P_limit);
+    struct vec vel_e = vclampscl(vsub(vel_d, stateVel), -Kpos_D_limit, Kpos_D_limit);
+    i_error_pos = vclampscl(vadd(i_error_pos, vscl(dt, pos_e)), -Kpos_I_limit, Kpos_I_limit);
+
+    struct vec F_d = vscl(g_vehicleMass, vadd4(
+      acc_d,
+      veltmul(Kpos_D, vel_e),
+      veltmul(Kpos_P, pos_e),
+      veltmul(Kpos_I, i_error_pos)));
+
+    control->thrustSI = vmag(F_d);
+    // Reset the accumulated error while on the ground
+    if (control->thrustSI < 0.01f) {
+      controllerMellingerSIReset();
+    }
+
+    // Use current yaw instead of desired yaw for roll/pitch
+    float yaw = radians(state->attitude.yaw);
+    qr = mkvec(
+      asinf((F_d.x * sinf(yaw) - F_d.y * cosf(yaw)) / control->thrustSI),
+      atanf((F_d.x * cosf(yaw) + F_d.y * sinf(yaw)) / F_d.z),
+      desiredYaw);
+  } 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;
+          controllerMellingerSIReset();
+          return;
+      }
+    }
+    // On CF2, thrust is mapped 65536 <==> 4 * 12 grams
+    const float max_thrust = 4 * 12.0 / 1000.0 * 9.81; // N
+    control->thrustSI = setpoint->thrust / 65536.0f * max_thrust;
+
+    qr = mkvec(
+      radians(setpoint->attitude.roll),
+      -radians(setpoint->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted
+      radians(desiredYaw));
+  }
+
+  // Attitude controller
+
+  // current rotation [R]
+  struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w);
+  struct mat33 R = quat2rotmat(q);
+
+  rpy = quat2rpy(q);
+
+  // desired rotation [Rdes]
+  struct quat q_des = rpy2quat(qr);
+  struct mat33 R_des = quat2rotmat(q_des);
+
+  rpy_des = quat2rpy(q_des);
+
+  // rotation error
+  struct mat33 eRM = msub(mmul(mtranspose(R_des), R), mmul(mtranspose(R), R_des));
+
+  struct vec eR = vscl(0.5f, mkvec(eRM.m[2][1], eRM.m[0][2], eRM.m[1][0]));
+
+  // angular velocity
+  omega = mkvec(
+    radians(sensors->gyro.x),
+    radians(sensors->gyro.y),
+    radians(sensors->gyro.z));
+
+  omega_des = mkvec(
+    radians(setpoint->attitudeRate.roll),
+    radians(setpoint->attitudeRate.pitch),
+    radians(setpoint->attitudeRate.yaw));
+
+  // Integral part on omega
+  struct vec omega_error = vsub(omega, omega_des);
+  i_error_att = vclampscl(vadd(i_error_att, vscl(dt, omega_error)), -Katt_I_limit, Katt_I_limit);
+
+  // compute moments
+  u = vadd3(
+    vneg(veltmul(KR, eR)),
+    vneg(veltmul(Komega, omega_error)),
+    vneg(veltmul(Katt_I, i_error_att)));
+
+  control->controlMode = controlModeForceTorque;
+  control->torque[0] = u.x;
+  control->torque[1] = u.y;
+  control->torque[2] = u.z;
+
+  ticks = usecTimestamp() - startTime;
+}
+
+PARAM_GROUP_START(ctrlMelSI)
+// Attitude P
+PARAM_ADD(PARAM_FLOAT, KR_x, &KR.x)
+PARAM_ADD(PARAM_FLOAT, KR_y, &KR.y)
+PARAM_ADD(PARAM_FLOAT, KR_z, &KR.z)
+// Attitude D
+PARAM_ADD(PARAM_FLOAT, Kw_x, &Komega.x)
+PARAM_ADD(PARAM_FLOAT, Kw_y, &Komega.y)
+PARAM_ADD(PARAM_FLOAT, Kw_z, &Komega.z)
+// Attitude I
+PARAM_ADD(PARAM_FLOAT, Katt_Ix, &Katt_I.x)
+PARAM_ADD(PARAM_FLOAT, Katt_Iy, &Katt_I.y)
+PARAM_ADD(PARAM_FLOAT, Katt_Iz, &Katt_I.z)
+PARAM_ADD(PARAM_FLOAT, Katt_I_limit, &Katt_I_limit)
+
+// Position P
+PARAM_ADD(PARAM_FLOAT, Kpos_Px, &Kpos_P.x)
+PARAM_ADD(PARAM_FLOAT, Kpos_Py, &Kpos_P.y)
+PARAM_ADD(PARAM_FLOAT, Kpos_Pz, &Kpos_P.z)
+PARAM_ADD(PARAM_FLOAT, Kpos_P_limit, &Kpos_P_limit)
+// Position D
+PARAM_ADD(PARAM_FLOAT, Kpos_Dx, &Kpos_D.x)
+PARAM_ADD(PARAM_FLOAT, Kpos_Dy, &Kpos_D.y)
+PARAM_ADD(PARAM_FLOAT, Kpos_Dz, &Kpos_D.z)
+PARAM_ADD(PARAM_FLOAT, Kpos_D_limit, &Kpos_D_limit)
+// Position I
+PARAM_ADD(PARAM_FLOAT, Kpos_Ix, &Kpos_I.x)
+PARAM_ADD(PARAM_FLOAT, Kpos_Iy, &Kpos_I.y)
+PARAM_ADD(PARAM_FLOAT, Kpos_Iz, &Kpos_I.z)
+PARAM_ADD(PARAM_FLOAT, Kpos_I_limit, &Kpos_I_limit)
+
+PARAM_GROUP_STOP(ctrlMelSI)
+
+
+LOG_GROUP_START(ctrlMelSI)
+LOG_ADD(LOG_FLOAT, torquex, &u.x)
+LOG_ADD(LOG_FLOAT, torquey, &u.y)
+LOG_ADD(LOG_FLOAT, torquez, &u.z)
+
+// current angles
+LOG_ADD(LOG_FLOAT, rpyx, &rpy.x)
+LOG_ADD(LOG_FLOAT, rpyy, &rpy.y)
+LOG_ADD(LOG_FLOAT, rpyz, &rpy.z)
+
+// desired angles
+LOG_ADD(LOG_FLOAT, rpydx, &rpy_des.x)
+LOG_ADD(LOG_FLOAT, rpydy, &rpy_des.y)
+LOG_ADD(LOG_FLOAT, rpydz, &rpy_des.z)
+
+// errors
+LOG_ADD(LOG_FLOAT, i_error_attx, &i_error_att.x)
+LOG_ADD(LOG_FLOAT, i_error_atty, &i_error_att.y)
+LOG_ADD(LOG_FLOAT, i_error_attz, &i_error_att.z)
+
+LOG_ADD(LOG_FLOAT, i_error_posx, &i_error_pos.x)
+LOG_ADD(LOG_FLOAT, i_error_posy, &i_error_pos.y)
+LOG_ADD(LOG_FLOAT, i_error_posz, &i_error_pos.z)
+
+// omega
+LOG_ADD(LOG_FLOAT, omegax, &omega.x)
+LOG_ADD(LOG_FLOAT, omegay, &omega.y)
+LOG_ADD(LOG_FLOAT, omegaz, &omega.z)
+
+// omega_des
+LOG_ADD(LOG_FLOAT, omegadx, &omega_des.x)
+LOG_ADD(LOG_FLOAT, omegady, &omega_des.y)
+LOG_ADD(LOG_FLOAT, omegadz, &omega_des.z)
+
+LOG_ADD(LOG_UINT32, ticks, &ticks)
+
+LOG_GROUP_STOP(ctrlMelSI)
diff --git a/src/modules/src/controller_sjc.c b/src/modules/src/controller_sjc.c
new file mode 100644
index 0000000000..cb5c4c4e6e
--- /dev/null
+++ b/src/modules/src/controller_sjc.c
@@ -0,0 +1,451 @@
+/*
+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 (section 7):
+
+Daniel Morgan, Giri P Subramanian, Soon-Jo Chung, Fred Y Hadaegh
+Swarm assignment and trajectory optimization using variable-swarm, distributed auction assignment and sequential convex programming 
+IJRR 2016
+
+Notes:
+  * There is a typo in the paper, in eq 71: It should be -K (omega - omega_r) (i.e., no dot)
+  * We make the following changes:
+    1. The first term of the control law (J omega_r_dot) is treated as a damping term on omega_r
+       to capture higher-order effects such as motor delays. Thus, we allow to tune this J (the J
+       in the second part is the real inertia matrix)
+    2. We use the current yaw angle instead of the desired yaw angle to compute desired roll/pitch angles
+    3. We add a D-term on omega_r - omega to account for motor delays
+  * This implementation is inspired by the implementation of the controller by 
+    Giri P Subramanian for CF 1.0
+  * Runtime attitude controller: 6us
+  * Note that a quaternion-based version is too complex for on-board execution if an analytical model
+    of Z^-1 and Z_dot^-1 is to be used
+*/
+
+#include <math.h>
+
+#include "param.h"
+#include "log.h"
+#include "math3d.h"
+#include "controller_sjc.h"
+#include "usec_time.h"
+// #include "debug.h"
+
+#define GRAVITY_MAGNITUDE (9.81f)
+
+static uint32_t ticks;
+
+static uint8_t trigReset;
+
+static float g_vehicleMass = 0.032; // TODO: should be CF global for other modules
+
+// Attitude P on omega
+static struct vec K = {0.0005, 0.0005, 0.001};
+// static float K_limit = 8.7; // ~500 deg/s
+
+// Attitude P on angle
+static struct vec lambda = {10, 10, 10};
+// static float lambda_limit = M_PI / 2.0; // 90 deg
+
+// Attitude I on omega
+static struct vec Katt_I = {0.00025, 0.00025, 0.0005};
+static float Katt_I_limit = 2;
+static struct vec i_error_att;
+
+// D on omega_dot
+static struct vec omega_error_prev;
+static struct vec omega_error_dot;
+static struct vec Katt_Dw = {0, 0, 0};
+
+// Position gains
+static struct vec Kpos_P = {12, 12, 12};
+static float Kpos_P_limit = 100;
+static struct vec Kpos_D = {8, 8, 8};
+static float Kpos_D_limit = 100;
+static struct vec Kpos_I = {8, 8, 8};
+static float Kpos_I_limit = 2;
+static struct vec i_error_pos;
+
+static float Kpos_I_decay = 1.0f;
+
+static struct vec Kpos_A = {0, 0, 0};
+static float Kpos_A_limit = 100;
+
+// delay compensation idea #1
+static float T_d_last = 0;
+static float T_d_lambda = 0.0; // 0 means disabled
+
+// 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
+static struct vec J = {16.571710e-6, 16.655602e-6, 29.261652e-6}; // kg m^2
+
+// We treat the first part of the control law (J omega_r_dot) as damping on omega_r
+// by allowing to change J
+static struct vec Jtune = {48e-6, 48e-6, 29e-6};
+
+// logging variables
+static struct vec u;
+static struct vec qr;
+static struct vec q;
+static struct vec omega;
+static struct vec omega_r;
+static float thrustSI;
+static struct vec qrp;
+static struct vec qr_dot;
+
+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));
+}
+
+// subtract d, b and c from a.
+static inline struct vec vsub3(struct vec a, struct vec b, struct vec c, struct vec d) {
+  return vadd4(a, vneg(b), vneg(c), vneg(d));
+}
+
+static inline struct vec vsub4(struct vec a, struct vec b, struct vec c, struct vec d, struct vec e) {
+  return vadd(vadd4(a, vneg(b), vneg(c), vneg(d)), vneg(e));
+}
+
+void controllerSJCReset(void)
+{
+  i_error_pos = vzero();
+  i_error_att = vzero();
+  omega_error_prev = vzero();
+  omega_error_dot = vzero();
+}
+
+void controllerSJCInit(void)
+{
+  controllerSJCReset();
+}
+
+bool controllerSJCTest(void)
+{
+  return true;
+}
+
+void controllerSJC(control_t *control, setpoint_t *setpoint,
+                                         const sensorData_t *sensors,
+                                         const state_t *state,
+                                         const uint32_t tick)
+{
+  if (!RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
+    return;
+  }
+
+  if (trigReset == 1) {
+    controllerSJCReset();
+  } else if (trigReset == 2) {
+    i_error_pos.z = 0;
+  }
+  trigReset = 0;
+
+  float dt = (float)(1.0f/ATTITUDE_RATE);
+
+  // 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);
+    struct vec rpy = quat2rpy(setpoint_quat);
+    desiredYaw = rpy.z;
+  }
+
+  // qr: Desired/reference angles in rad
+  // struct vec qr;
+
+  // Position controller
+  if (   setpoint->mode.x == modeAbs
+      || setpoint->mode.y == modeAbs
+      || setpoint->mode.z == modeAbs) {
+
+    if (RATE_DO_EXECUTE(POSITION_RATE, tick)) {
+      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);
+      struct vec stateAcc = vscl(9.81, mkvec(state->acc.x, state->acc.y, state->acc.z + 1)); // Gs -> m/s^2
+
+      // errors
+      struct vec pos_e = vclampscl(vsub(pos_d, statePos), -Kpos_P_limit, Kpos_P_limit);
+      struct vec vel_e = vclampscl(vsub(vel_d, stateVel), -Kpos_D_limit, Kpos_D_limit);
+      // This is for delay compensation (idea #2): feedback term on acceleration
+      struct vec acc_e = vclampscl(vsub(acc_d, stateAcc), -Kpos_A_limit, Kpos_A_limit);
+      i_error_pos = vclampscl(vadd(vscl(Kpos_I_decay, i_error_pos), vscl(dt, pos_e)), -Kpos_I_limit, Kpos_I_limit);
+
+      struct vec F_d = vscl(g_vehicleMass, vadd5(
+        acc_d,
+        veltmul(Kpos_A, acc_e),
+        veltmul(Kpos_D, vel_e),
+        veltmul(Kpos_P, pos_e),
+        veltmul(Kpos_I, i_error_pos)));
+
+      control->thrustSI = vmag(F_d);
+      thrustSI = control->thrustSI;
+      // This is for delay compensation (idea #1)
+      // replace by T_d by T = T_d + T_d dot / lambda
+      float T_d_dot = (control->thrustSI - T_d_last) * POSITION_RATE;
+      if (T_d_lambda > 0) {
+        control->thrustSI = control->thrustSI + T_d_dot / T_d_lambda;
+      }
+      T_d_last = control->thrustSI;
+
+      // Reset the accumulated error while on the ground
+      if (control->thrustSI < 0.01f) {
+        controllerSJCReset();
+      }
+
+      // Use current yaw instead of desired yaw for roll/pitch
+      float yaw = radians(state->attitude.yaw);
+      qr = mkvec(
+        asinf((F_d.x * sinf(yaw) - F_d.y * cosf(yaw)) / control->thrustSI),
+        atanf((F_d.x * cosf(yaw) + F_d.y * sinf(yaw)) / F_d.z),
+        desiredYaw);
+    }
+  } 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;
+          controllerSJCReset();
+          return;
+      }
+    }
+    // On CF2, thrust is mapped 65536 <==> 4 * 12 grams
+    const float max_thrust = 4 * 12.0 / 1000.0 * 9.81; // N
+    control->thrustSI = setpoint->thrust / 65536.0f * max_thrust;
+
+    qr = mkvec(
+      radians(setpoint->attitude.roll),
+      -radians(setpoint->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted
+      radians(desiredYaw));
+  }
+
+  // Attitude controller
+  // DEBUG_PRINT("q %d %d %d %d\n", (int)(qr.x * 1000), (int)(qr.y * 1000), (int)(qr.z * 1000), (int)(control->thrustSI * 1000));
+
+  // q: vector of three-dimensional attitude representation
+  //    here: Euler angles in rad
+  q = mkvec(
+    radians(state->attitude.roll),
+    radians(-state->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted
+    radians(state->attitude.yaw));
+
+  // omega: angular velocity in rad/s
+  omega = mkvec(
+    radians(sensors->gyro.x),
+    radians(sensors->gyro.y),
+    radians(sensors->gyro.z));
+
+  // q_dot = Z(q)*omega
+  // For euler angles Z(q) is defined in Giri P Subramanian MS thesis, equation 8
+  //        [1  sin(r)tan(p)  cos(r)tan(p)]
+  // Z(q) = [0  cos(r)        -sin(r)     ]
+  //        [0  sin(r)sec(p)  cos(r)sec(p)]
+  // TODO: add check for division by zero?
+  struct vec q_dot = mkvec(
+    omega.x + sinf(q.x)*tanf(q.y)*omega.y + cosf(q.x)*tanf(q.y)*omega.z,
+              cosf(q.x)          *omega.y - sinf(q.x)          *omega.z,
+              sinf(q.x)/cosf(q.y)*omega.y + cosf(q.x)/cosf(q.y)*omega.z);
+
+  // qr_dot desired/reference angular velocity in rad/s
+  qr_dot = mkvec(
+    radians(setpoint->attitudeRate.roll),
+    radians(setpoint->attitudeRate.pitch),
+    radians(setpoint->attitudeRate.yaw));
+
+  // qr_ddot desired/reference angular acceleration in rad/s^2
+  struct vec qr_ddot = vzero();
+
+  // omega_r: desired/reference angular velocity in rad/s
+  // omega_r = Z(q)^-1 * (qr_dot + lambda (qr - q)) = Z(q)^-1 * qrp
+  // For euler angles Z(q)^-1 is:
+  //           [1  0       -sin(p)     ]
+  // Z(q)^-1 = [0  cos(r)  cos(p)sin(r)]
+  //           [0 -sin(r)  cos(p)cos(r)]
+  qrp = vadd(qr_dot, veltmul(lambda, vsub(qr, q)));
+  omega_r = mkvec(
+    qrp.x +                 -sinf(q.y)           *qrp.z,
+            cosf(q.x)*qrp.y + cosf(q.y)*sinf(q.x)*qrp.z,
+          - sinf(q.x)*qrp.y + cosf(q.y)*cosf(q.x)*qrp.z);
+
+  // omegar_dot =   Z(q)^-1_dot*qr_dot         + Z(q)^-1*qr_ddot
+  //              + Z(q)^-1_dot*lambda(qr - q) + Z(q)^-1*lambda*(qr_dot - q_dot)
+  //            =   Z(q)^-1_dot (qr_dot + lambda (qr - q))
+  //              + Z(q)^-1 (qr_ddot + lambda (qr_dot - q_dot))
+  //            =   Z(q)^-1_dot qrp + Z(q)^-1 qrp_dot
+  // For euler angles Z(q)^-1_dot is:
+  //               [0  0            -cos(p)p_dot                        ]
+  // Z(q)^-1_dot = [0 -sin(r)r_dot  -sin(p)sin(r)p_dot+cos(p)cos(r)r_dot]
+  //               [0 -cos(r)r_dot  -cos(r)sin(p)p_dot-cos(p)sin(r)r_dot]
+  struct vec qrp_dot = vadd(qr_ddot, veltmul(lambda, vsub(qr_dot, q_dot)));
+  struct vec omega_r_dot = mkvec(
+                             -cosf(q.y)             *q_dot.y                             *qrp.z + qrp_dot.x                     -sinf(q.y)           *qrp_dot.z,
+    -sinf(q.x)*q_dot.x*qrp.y + (-sinf(q.y)*sinf(q.x)*q_dot.y+cosf(q.y)*cosf(q.x)*q_dot.x)*qrp.z +           cosf(q.x)*qrp_dot.y + cosf(q.y)*sinf(q.x)*qrp_dot.z,
+    -cosf(q.x)*q_dot.x*qrp.y + (-cosf(q.x)*sinf(q.y)*q_dot.y-cosf(q.y)*sinf(q.x)*q_dot.x)*qrp.z +          -sinf(q.x)*qrp_dot.y + cosf(q.y)*cosf(q.x)*qrp_dot.z);
+
+  // Integral part on omega (as in paper)
+  struct vec omega_error = vsub(omega, omega_r);
+  i_error_att = vclampscl(vadd(i_error_att, vscl(dt, omega_error)), -Katt_I_limit, Katt_I_limit);
+
+  // D part on omega error (to deal with motor delays)
+  omega_error_dot = vdiv(vsub(omega_error, omega_error_prev), dt);
+  // filter estimate
+  omega_error_prev = omega_error;
+
+  // // Integral part on angle
+  // struct vec q_error = vsub(qr, q);
+  // i_error_att = vclampscl(vadd(i_error_att, vscl(dt, q_error)), -Katt_I_limit, Katt_I_limit);
+
+  // compute moments (note there is a typo on the paper in equation 71)
+  // u = J omega_r_dot - (J omega) x omega_r - K(omega - omega_r) - Katt_I \integral(w-w_r, dt)
+  u = vsub4(
+    veltmul(Jtune, omega_r_dot),
+    vcross(veltmul(J, omega), omega_r),
+    veltmul(K, omega_error),
+    veltmul(Katt_I, i_error_att),
+    veltmul(Katt_Dw, omega_error_dot));
+
+  control->controlMode = controlModeForceTorque;
+  control->torque[0] = u.x;
+  control->torque[1] = u.y;
+  control->torque[2] = u.z;
+}
+
+PARAM_GROUP_START(ctrlSJC)
+// Attitude P
+PARAM_ADD(PARAM_FLOAT, Katt_Px, &lambda.x)
+PARAM_ADD(PARAM_FLOAT, Katt_Py, &lambda.y)
+PARAM_ADD(PARAM_FLOAT, Katt_Pz, &lambda.z)
+// Attitude D
+PARAM_ADD(PARAM_FLOAT, Katt_Dx, &K.x)
+PARAM_ADD(PARAM_FLOAT, Katt_Dy, &K.y)
+PARAM_ADD(PARAM_FLOAT, Katt_Dz, &K.z)
+// Attitude I
+PARAM_ADD(PARAM_FLOAT, Katt_Ix, &Katt_I.x)
+PARAM_ADD(PARAM_FLOAT, Katt_Iy, &Katt_I.y)
+PARAM_ADD(PARAM_FLOAT, Katt_Iz, &Katt_I.z)
+PARAM_ADD(PARAM_FLOAT, Katt_I_limit, &Katt_I_limit)
+// Attitude Dw
+PARAM_ADD(PARAM_FLOAT, Katt_Dwx, &Katt_Dw.x)
+PARAM_ADD(PARAM_FLOAT, Katt_Dwy, &Katt_Dw.y)
+PARAM_ADD(PARAM_FLOAT, Katt_Dwz, &Katt_Dw.z)
+// Position P
+PARAM_ADD(PARAM_FLOAT, Kpos_Px, &Kpos_P.x)
+PARAM_ADD(PARAM_FLOAT, Kpos_Py, &Kpos_P.y)
+PARAM_ADD(PARAM_FLOAT, Kpos_Pz, &Kpos_P.z)
+PARAM_ADD(PARAM_FLOAT, Kpos_P_limit, &Kpos_P_limit)
+// Position D
+PARAM_ADD(PARAM_FLOAT, Kpos_Dx, &Kpos_D.x)
+PARAM_ADD(PARAM_FLOAT, Kpos_Dy, &Kpos_D.y)
+PARAM_ADD(PARAM_FLOAT, Kpos_Dz, &Kpos_D.z)
+PARAM_ADD(PARAM_FLOAT, Kpos_D_limit, &Kpos_D_limit)
+
+// Position A
+PARAM_ADD(PARAM_FLOAT, Kpos_Ax, &Kpos_A.x)
+PARAM_ADD(PARAM_FLOAT, Kpos_Ay, &Kpos_A.y)
+PARAM_ADD(PARAM_FLOAT, Kpos_Az, &Kpos_A.z)
+PARAM_ADD(PARAM_FLOAT, Kpos_A_limit, &Kpos_A_limit)
+
+// Position I
+PARAM_ADD(PARAM_FLOAT, Kpos_Ix, &Kpos_I.x)
+PARAM_ADD(PARAM_FLOAT, Kpos_Iy, &Kpos_I.y)
+PARAM_ADD(PARAM_FLOAT, Kpos_Iz, &Kpos_I.z)
+PARAM_ADD(PARAM_FLOAT, Kpos_I_limit, &Kpos_I_limit)
+PARAM_ADD(PARAM_FLOAT, Kpos_I_decay, &Kpos_I_decay)
+
+// Jtune
+PARAM_ADD(PARAM_FLOAT, Jtune_x, &Jtune.x)
+PARAM_ADD(PARAM_FLOAT, Jtune_y, &Jtune.y)
+PARAM_ADD(PARAM_FLOAT, Jtune_z, &Jtune.z)
+
+PARAM_ADD(PARAM_FLOAT, mass, &g_vehicleMass)
+
+PARAM_ADD(PARAM_UINT8, trigReset, &trigReset)
+
+// delay compensation (0 == disabled)
+PARAM_ADD(PARAM_UINT8, T_d_lambda, &T_d_lambda)
+
+PARAM_GROUP_STOP(ctrlSJC)
+
+LOG_GROUP_START(ctrlSJC)
+LOG_ADD(LOG_FLOAT, thrustSI, &thrustSI)
+LOG_ADD(LOG_FLOAT, torquex, &u.x)
+LOG_ADD(LOG_FLOAT, torquey, &u.y)
+LOG_ADD(LOG_FLOAT, torquez, &u.z)
+// current angles
+LOG_ADD(LOG_FLOAT, qx, &q.x)
+LOG_ADD(LOG_FLOAT, qy, &q.y)
+LOG_ADD(LOG_FLOAT, qz, &q.z)
+// desired angles
+LOG_ADD(LOG_FLOAT, qrx, &qr.x)
+LOG_ADD(LOG_FLOAT, qry, &qr.y)
+LOG_ADD(LOG_FLOAT, qrz, &qr.z)
+
+// errors
+LOG_ADD(LOG_FLOAT, i_error_attx, &i_error_att.x)
+LOG_ADD(LOG_FLOAT, i_error_atty, &i_error_att.y)
+LOG_ADD(LOG_FLOAT, i_error_attz, &i_error_att.z)
+
+LOG_ADD(LOG_FLOAT, i_error_posx, &i_error_pos.x)
+LOG_ADD(LOG_FLOAT, i_error_posy, &i_error_pos.y)
+LOG_ADD(LOG_FLOAT, i_error_posz, &i_error_pos.z)
+
+// omega
+LOG_ADD(LOG_FLOAT, omegax, &omega.x)
+LOG_ADD(LOG_FLOAT, omegay, &omega.y)
+LOG_ADD(LOG_FLOAT, omegaz, &omega.z)
+
+// omega dot hat
+LOG_ADD(LOG_FLOAT, domegax, &omega_error_dot.x)
+LOG_ADD(LOG_FLOAT, domegay, &omega_error_dot.y)
+LOG_ADD(LOG_FLOAT, domegaz, &omega_error_dot.z)
+
+// omega_r
+LOG_ADD(LOG_FLOAT, omegarx, &omega_r.x)
+LOG_ADD(LOG_FLOAT, omegary, &omega_r.y)
+LOG_ADD(LOG_FLOAT, omegarz, &omega_r.z)
+
+LOG_ADD(LOG_FLOAT, qrpx, &qrp.x)
+LOG_ADD(LOG_FLOAT, qrpz, &qrp.z)
+
+LOG_ADD(LOG_FLOAT, qrdotz, &qr_dot.z)
+
+LOG_ADD(LOG_UINT32, ticks, &ticks)
+
+LOG_GROUP_STOP(ctrlSJC)
\ No newline at end of file
diff --git a/src/modules/src/crtp.c b/src/modules/src/crtp.c
index 401915f7c1..d2518eba4b 100644
--- a/src/modules/src/crtp.c
+++ b/src/modules/src/crtp.c
@@ -70,7 +70,7 @@ static struct {
 static xQueueHandle  txQueue;
 
 #define CRTP_NBR_OF_PORTS 16
-#define CRTP_TX_QUEUE_SIZE 120
+#define CRTP_TX_QUEUE_SIZE 200
 #define CRTP_RX_QUEUE_SIZE 16
 
 static void crtpTxTask(void *param);
diff --git a/src/modules/src/crtp_commander_generic.c b/src/modules/src/crtp_commander_generic.c
index 6148d608ae..75a8831374 100644
--- a/src/modules/src/crtp_commander_generic.c
+++ b/src/modules/src/crtp_commander_generic.c
@@ -35,6 +35,11 @@
 #include "num.h"
 #include "quatcompress.h"
 #include "FreeRTOS.h"
+#include "eventtrigger.h"
+
+
+EVENTTRIGGER(cmdFullState)
+
 
 /* The generic commander format contains a packet type and data that has to be
  * decoded into a setpoint_t structure. The aim is to make it future-proof
@@ -353,6 +358,7 @@ 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)
@@ -369,6 +375,8 @@ static void fullStateDecoder(setpoint_t *setpoint, uint8_t type, const void *dat
   setpoint->mode.roll = modeDisable;
   setpoint->mode.pitch = modeDisable;
   setpoint->mode.yaw = modeDisable;
+
+  eventTrigger(&eventTrigger_cmdFullState);
 }
 
 /* positionDecoder
diff --git a/src/modules/src/crtp_commander_high_level.c b/src/modules/src/crtp_commander_high_level.c
index eff53dbe72..d7bd4e8be7 100644
--- a/src/modules/src/crtp_commander_high_level.c
+++ b/src/modules/src/crtp_commander_high_level.c
@@ -111,6 +111,11 @@ static StaticSemaphore_t lockTrajBuffer;
 static float defaultTakeoffVelocity = 0.5f;
 static float defaultLandingVelocity = 0.5f;
 
+static float yawrate_desired = 0.0f;
+static float yawacc_limit = 1.0f;
+static float yawrate_limit = 5.0f;
+static float yawrate_current = 0.0f;
+
 // Trajectory memory handling from the memory module
 static uint32_t handleMemGetSize(void) { return crtpCommanderHighLevelTrajectoryMemSize(); }
 static bool handleMemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer);
@@ -303,6 +308,7 @@ bool crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *stat
   if (!RATE_DO_EXECUTE(RATE_HL_COMMANDER, tick)) {
     return false;
   }
+  const float dt = (float)(1.0f/RATE_HL_COMMANDER);
 
   xSemaphoreTake(lockTraj, portMAX_DELAY);
   float t = usecTimestamp() / 1e6;
@@ -347,11 +353,29 @@ 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;
     vel = ev.vel;
-    yaw = ev.yaw;
+
+    float yawrate_desired_adjusted = yawrate_desired;
+    if (yawrate_desired == 0.0f) {
+      float signed_yaw = shortest_signed_angle_radians(yaw, 0.0f);
+      // attempt to go there in 1.0s
+      yawrate_desired_adjusted = copysignf(fabsf(signed_yaw/1.0f), signed_yaw);
+    }
+
+    float yawacc_desired = (yawrate_desired_adjusted - yawrate_current)/dt;
+    float yawacc = clamp(yawacc_desired, -yawacc_limit, yawacc_limit);
+    yawrate_current += yawacc * dt;
+    yawrate_current = clamp(yawrate_current, -yawrate_limit, yawrate_limit);
+
+    yaw = normalize_radians(yaw + yawrate_current * dt);
+    setpoint->attitude.yaw = degrees(yaw);
+    setpoint->attitudeRate.yaw = degrees(yawrate_current);
 
     return true;
   }
@@ -837,4 +861,9 @@ PARAM_ADD_CORE(PARAM_FLOAT, vtoff, &defaultTakeoffVelocity)
  */
 PARAM_ADD_CORE(PARAM_FLOAT, vland, &defaultLandingVelocity)
 
+
+PARAM_ADD(PARAM_FLOAT, yawrate, &yawrate_desired)
+PARAM_ADD(PARAM_FLOAT, yawacc, &yawacc_limit)
+PARAM_ADD(PARAM_FLOAT, yawrlim, &yawrate_limit)
+
 PARAM_GROUP_STOP(hlCommander)
diff --git a/src/modules/src/crtp_localization_service.c b/src/modules/src/crtp_localization_service.c
index 88fab9b48e..20fdd1f563 100644
--- a/src/modules/src/crtp_localization_service.c
+++ b/src/modules/src/crtp_localization_service.c
@@ -120,8 +120,10 @@ static CRTPPacket LhAngle;
 #endif
 static bool enableLighthouseAngleStream = false;
 static float extPosStdDev = 0.01;
-static float extQuatStdDev = 4.5e-3;
+static float extRollPitchStdDev = 4.5e-3;
+static float extYawStdDev = 4.5e-3;
 static bool isInit = false;
+static bool isTimeSynced = false;
 static uint8_t my_id;
 static uint16_t tickOfLastPacket; // tick when last packet was received
 
@@ -168,7 +170,29 @@ static void updateLogFromExtPos()
   ext_pose.z = ext_pos.z;
 }
 
+/* This function is called from all localization events that use
+   broadcasts (i.e., pos, pos_packed, pose, pose_packed). The first
+   time such a broadcast is received, the usecTimer is reset to 0.
+   Since broadcasts are received simultanously at all Crazyflies,
+   this should reset the local on-board timer of all Crazyflies
+   at almost the same exact time.
+
+   TODO: In the future, it would be better to have a dedicated CRTP
+         functionality for this, rather than "piggybacking" on the
+         localization service for the motion capture.
+*/
+
+static void syncTimeIfNeeded()
+{
+  if (!isTimeSynced) {
+    usecTimerReset();
+    isTimeSynced = true;
+  }
+}
+
 static void extPositionHandler(CRTPPacket* pk) {
+  syncTimeIfNeeded();
+
   const struct CrtpExtPosition* data = (const struct CrtpExtPosition*)pk->data;
 
   ext_pos.x = data->x;
@@ -183,6 +207,8 @@ static void extPositionHandler(CRTPPacket* pk) {
 }
 
 static void extPoseHandler(const CRTPPacket* pk) {
+  syncTimeIfNeeded();
+
   const struct CrtpExtPose* data = (const struct CrtpExtPose*)&pk->data[1];
 
   ext_pose.x = data->x;
@@ -193,13 +219,16 @@ static void extPoseHandler(const CRTPPacket* pk) {
   ext_pose.quat.z = data->qz;
   ext_pose.quat.w = data->qw;
   ext_pose.stdDevPos = extPosStdDev;
-  ext_pose.stdDevQuat = extQuatStdDev;
+  ext_pose.stdDevRollPitch = extRollPitchStdDev;
+  ext_pose.stdDevYaw = extYawStdDev;
 
   estimatorEnqueuePose(&ext_pose);
   tickOfLastPacket = xTaskGetTickCount();
 }
 
 static void extPosePackedHandler(const CRTPPacket* pk) {
+  syncTimeIfNeeded();
+
   uint8_t numItems = (pk->size - 1) / sizeof(extPosePackedItem);
   for (uint8_t i = 0; i < numItems; ++i) {
     const extPosePackedItem* item = (const extPosePackedItem*)&pk->data[1 + i * sizeof(extPosePackedItem)];
@@ -209,7 +238,8 @@ static void extPosePackedHandler(const CRTPPacket* pk) {
       ext_pose.z = item->z / 1000.0f;
       quatdecompress(item->quat, (float *)&ext_pose.quat.q0);
       ext_pose.stdDevPos = extPosStdDev;
-      ext_pose.stdDevQuat = extQuatStdDev;
+      ext_pose.stdDevRollPitch = extRollPitchStdDev;
+      ext_pose.stdDevYaw = extYawStdDev;
       estimatorEnqueuePose(&ext_pose);
       tickOfLastPacket = xTaskGetTickCount();
     } else {
@@ -316,6 +346,8 @@ static void genericLocHandle(CRTPPacket* pk)
 
 static void extPositionPackedHandler(CRTPPacket* pk)
 {
+  syncTimeIfNeeded();
+
   uint8_t numItems = pk->size / sizeof(extPositionPackedItem);
   for (uint8_t i = 0; i < numItems; ++i) {
     const extPositionPackedItem* item = (const extPositionPackedItem*)&pk->data[i * sizeof(extPositionPackedItem)];
@@ -460,5 +492,9 @@ PARAM_GROUP_START(locSrv)
   /**
  * @brief Standard deviation of the quarternion data to kalman filter
  */
-  PARAM_ADD_CORE(PARAM_FLOAT, extQuatStdDev, &extQuatStdDev)
+  PARAM_ADD_CORE(PARAM_FLOAT, extRPStdDev, &extRollPitchStdDev)
+  /**
+ * @brief Standard deviation of the quarternion data to kalman filter
+ */
+  PARAM_ADD_CORE(PARAM_FLOAT, extYStdDev, &extYawStdDev)
 PARAM_GROUP_STOP(locSrv)
diff --git a/src/modules/src/cvmrs.c b/src/modules/src/cvmrs.c
new file mode 100644
index 0000000000..59e0137c37
--- /dev/null
+++ b/src/modules/src/cvmrs.c
@@ -0,0 +1,169 @@
+#include "FreeRTOS.h"
+#include "task.h"
+#include "system.h"
+#include "log.h"
+#include "cpx_internal_router.h"
+#include "uart1.h"
+#include "usec_time.h"
+#include "debug.h"
+#include "param.h"
+#define TASK_FREQ 100
+
+// GAP8 register settings
+
+static uint8_t aeg;
+static uint8_t aGain;
+static uint8_t dGain;
+static uint16_t exposure;
+static uint8_t num_consecutive_images = 3;
+static uint8_t stream_mode = 0;
+static uint8_t trigger = 0;
+const uint32_t baudrate_esp32 = 115200;
+
+typedef struct
+{
+    uint8_t cmd;
+    uint64_t timestamp; // usec timestamp from STM32
+    float x; // m
+    float y; // m
+    float z; // m
+    float qx;
+    float qy;
+    float qz;
+    float qw;
+} __attribute__((packed)) StatePacket_t;
+// static StatePacket_t cf_state;
+static CPXPacket_t cpx_packet;
+static StatePacket_t state_packet;
+
+typedef struct
+{
+    uint8_t cmd;
+    uint8_t aeg;
+    uint8_t aGain;
+    uint8_t dGain;
+    uint16_t exposure;
+    uint8_t num_consecutive_images;
+    uint8_t stream_mode;
+} __attribute__((packed)) RegisterPacket_t;
+
+void appMain()
+{
+    uint32_t lastWakeTime;
+
+    //Wait for the system to be fully started to start stabilization loop
+    systemWaitStart();
+
+    paramVarId_t deck_id = paramGetVarId("deck", "bcAI");
+    uint8_t ai_deck_available = paramGetInt(deck_id);
+    if (!ai_deck_available) {
+        DEBUG_PRINT("No AI deck - do not start CVMRS task\n");
+        return;
+    }
+
+    logVarId_t x_id = logGetVarId("stateEstimate", "x");
+    logVarId_t y_id = logGetVarId("stateEstimate", "y");
+    logVarId_t z_id = logGetVarId("stateEstimate", "z");
+    logVarId_t qx_id = logGetVarId("stateEstimate", "qx");
+    logVarId_t qy_id = logGetVarId("stateEstimate", "qy");
+    logVarId_t qz_id = logGetVarId("stateEstimate", "qz");
+    logVarId_t qw_id = logGetVarId("stateEstimate", "qw");
+
+    cpxInitRoute(CPX_T_STM32, CPX_T_GAP8, CPX_F_APP, &cpx_packet.route);
+    // cpx_packet.dataLength = sizeof(StatePacket_t);
+
+    // StatePacket_t* state_packet = (StatePacket_t*)&cpx_packet.data;
+    state_packet.cmd = 1;
+
+    // Delay is only needed for CPX
+    // vTaskDelay(60000);
+
+    DEBUG_PRINT("Starting CVMRS task\n");
+
+    lastWakeTime = xTaskGetTickCount();
+    uart1Init(baudrate_esp32);
+    while (1)
+    {
+        vTaskDelayUntil(&lastWakeTime, F2T(TASK_FREQ));
+
+        // Sending current state information to GAP8
+        state_packet.timestamp = usecTimestamp();
+        state_packet.x = logGetFloat(x_id);
+        state_packet.y = logGetFloat(y_id);
+        state_packet.z = logGetFloat(z_id);
+        state_packet.qx = logGetFloat(qx_id);
+        state_packet.qy = logGetFloat(qy_id);
+        state_packet.qz = logGetFloat(qz_id);
+        state_packet.qw = logGetFloat(qw_id);
+        // cpxSendPacket(&cpx_packet, /*timeout*/ 10 /* ms */);
+
+        uint8_t magic = 0xBC;
+        uint8_t length = sizeof(StatePacket_t);
+        uart1SendData(1, &magic);
+        uart1SendData(1, &length);
+        uart1SendData(length, (uint8_t*)&state_packet);
+
+        // compute crc
+        uint8_t crc = 0;
+        for (const uint8_t* p = (uint8_t*)&state_packet; p < (uint8_t*)&state_packet + length; p++) {
+            crc ^= *p;
+        }
+        uart1SendData(1, &crc);
+
+        if (uart1DidOverrun()) {
+            DEBUG_PRINT("UART overrun!\n");
+        }
+
+        // DEBUG_PRINT("cpxSendPacket\n");
+
+        if (trigger == 1) {
+            cpx_packet.dataLength = sizeof(RegisterPacket_t);
+            RegisterPacket_t* reg_packet = (RegisterPacket_t*)&cpx_packet.data;
+            reg_packet->cmd = 0;
+            reg_packet->aeg = aeg;
+            reg_packet->aGain = aGain;
+            reg_packet->dGain = dGain;
+            reg_packet->exposure = exposure;
+            reg_packet->num_consecutive_images = num_consecutive_images;
+            reg_packet->stream_mode = stream_mode;
+
+            cpxSendPacketBlocking(&cpx_packet);
+            trigger = 0;
+        }
+    }
+}
+
+
+PARAM_GROUP_START(cvmrs)
+/**
+ * @brief Automatic Exposure Gain (0: disabled; 1: enabled)
+ */
+PARAM_ADD(PARAM_UINT8, aeg, &aeg)
+/**
+ * @brief Analog gain (1, 2, 4, or 8) [If AEG is disabled]
+ */
+PARAM_ADD(PARAM_UINT8, aGain, &aGain)
+/**
+ * @brief Digital gain in 2.6 format (2-bit integer, 6-bit floating)  [If AEG is disabled]
+ */
+PARAM_ADD(PARAM_UINT8, dGain, &dGain)
+/**
+ * @brief Exposure 2 to (frame_length_lines - 2) [If AEG is disabled]
+ */
+PARAM_ADD(PARAM_UINT16, exposure, &exposure)
+
+/**
+ * @brief Number of consecutive images to record (1-3)
+ */
+PARAM_ADD(PARAM_UINT8, num_img, &num_consecutive_images)
+
+/**
+ * @brief Streaming mode (0: raw; 1: jpeg)
+ */
+PARAM_ADD(PARAM_UINT8, stream_mode, &stream_mode)
+
+/**
+ * @brief Triggers an update of all settings/registers on the GAP8
+ */
+PARAM_ADD(PARAM_UINT8, trigger, &trigger)
+PARAM_GROUP_STOP(cvmrs)
\ No newline at end of file
diff --git a/src/modules/src/estimator_kalman.c b/src/modules/src/estimator_kalman.c
index 0400a15302..bea26ebfca 100644
--- a/src/modules/src/estimator_kalman.c
+++ b/src/modules/src/estimator_kalman.c
@@ -146,6 +146,7 @@ static uint32_t gyroAccumulatorCount;
 static Axis3f accLatest;
 static Axis3f gyroLatest;
 static bool quadIsFlying = false;
+static uint8_t quadIsFlyingOverwrite = 2; // 0 - quad is not flying, 1 - quad is flying, 2 - automatic (default)
 
 static OutlierFilterLhState_t sweepOutlierFilterState;
 
@@ -335,7 +336,12 @@ static bool predictStateForward(uint32_t osTick, float dt) {
   gyroAccumulator = (Axis3f){.axis={0}};
   gyroAccumulatorCount = 0;
 
-  quadIsFlying = supervisorIsFlying();
+  if (quadIsFlyingOverwrite == 2) {
+    quadIsFlying = supervisorIsFlying();
+  } else {
+    quadIsFlying = quadIsFlyingOverwrite;
+  }
+
   kalmanCorePredict(&coreData, &accAverage, &gyroAverage, dt, quadIsFlying);
 
   return true;
@@ -594,7 +600,7 @@ PARAM_GROUP_START(kalman)
  * @brief Reset the kalman estimator
  */
   PARAM_ADD_CORE(PARAM_UINT8, resetEstimation, &resetEstimation)
-  PARAM_ADD(PARAM_UINT8, quadIsFlying, &quadIsFlying)
+  PARAM_ADD(PARAM_UINT8, quadIsFlying, &quadIsFlyingOverwrite)
 /**
  * @brief Nonzero to use robust TDOA method (default: 0)
  */
diff --git a/src/modules/src/kalman_core/mm_pose.c b/src/modules/src/kalman_core/mm_pose.c
index 4f1fc7fbb6..48b509fd42 100644
--- a/src/modules/src/kalman_core/mm_pose.c
+++ b/src/modules/src/kalman_core/mm_pose.c
@@ -49,14 +49,14 @@ void kalmanCoreUpdateWithPose(kalmanCoreData_t* this, poseMeasurement_t *pose)
     float h[KC_STATE_DIM] = {0};
     arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
     h[KC_STATE_D0] = 1;
-    kalmanCoreScalarUpdate(this, &H, err_quat.x, pose->stdDevQuat);
+    kalmanCoreScalarUpdate(this, &H, err_quat.x, pose->stdDevRollPitch);
     h[KC_STATE_D0] = 0;
 
     h[KC_STATE_D1] = 1;
-    kalmanCoreScalarUpdate(this, &H, err_quat.y, pose->stdDevQuat);
+    kalmanCoreScalarUpdate(this, &H, err_quat.y, pose->stdDevRollPitch);
     h[KC_STATE_D1] = 0;
 
     h[KC_STATE_D2] = 1;
-    kalmanCoreScalarUpdate(this, &H, err_quat.z, pose->stdDevQuat);
+    kalmanCoreScalarUpdate(this, &H, err_quat.z, pose->stdDevYaw);
   }
 }
diff --git a/src/modules/src/log.c b/src/modules/src/log.c
index 3f7094a343..0442d61fdf 100644
--- a/src/modules/src/log.c
+++ b/src/modules/src/log.c
@@ -121,6 +121,9 @@ struct ops_setting_v2 {
     uint16_t id;
 } __attribute__((packed));
 
+struct control_start_block_v2 {
+  uint16_t period_in_ms;
+} __attribute__((packed));
 
 #define TOC_CH      0
 #define CONTROL_CH  1
@@ -139,6 +142,7 @@ struct ops_setting_v2 {
 #define CONTROL_RESET           5
 #define CONTROL_CREATE_BLOCK_V2 6
 #define CONTROL_APPEND_BLOCK_V2 7
+#define CONTROL_START_BLOCK_V2  8
 
 #define BLOCK_ID_FREE -1
 
@@ -420,6 +424,12 @@ void logControlProcess()
                             (struct ops_setting_v2*)&p.data[2],
                             (p.size-2)/sizeof(struct ops_setting_v2) );
       break;
+    case CONTROL_START_BLOCK_V2:
+      {
+        struct control_start_block_v2* args = (struct control_start_block_v2 *)&p.data[2];
+        ret = logStartBlock(p.data[1], args->period_in_ms);
+      }
+      break;
   }
 
   //Commands answer
diff --git a/src/modules/src/param_logic.c b/src/modules/src/param_logic.c
index 638e0ba1ee..37502e315a 100644
--- a/src/modules/src/param_logic.c
+++ b/src/modules/src/param_logic.c
@@ -407,6 +407,10 @@ static char paramWriteByNameProcess(char* group, char* name, int type, void *val
 
   paramSet(index, valptr);
 
+  if (params[index].callback) {
+    params[index].callback();
+  }
+
   return 0;
 }
 
diff --git a/src/modules/src/power_distribution_quadrotor.c b/src/modules/src/power_distribution_quadrotor.c
index 5b8dd68732..dc965030bc 100644
--- a/src/modules/src/power_distribution_quadrotor.c
+++ b/src/modules/src/power_distribution_quadrotor.c
@@ -30,9 +30,19 @@
 #include "log.h"
 #include "param.h"
 #include "num.h"
+#include "math3d.h"
 #include "autoconf.h"
 #include "config.h"
 
+static uint8_t saturationStatus = 0;
+
+enum saturationBits
+{
+  SaturationOffsetThrust = 1,
+  SaturationRollPitch    = 2,
+  SaturationYaw          = 4,
+};
+
 #ifndef CONFIG_MOTORS_DEFAULT_IDLE_THRUST
 #  define DEFAULT_IDLE_THRUST 0
 #else
@@ -41,6 +51,21 @@
 
 static uint32_t idleThrust = DEFAULT_IDLE_THRUST;
 
+float motorForce[4];
+
+// Logging
+static int16_t log_thrustpart;  // in 1/100th of a gram
+static int16_t log_rollpart;    // in 1/100th of a gram
+static int16_t log_pitchpart;   // in 1/100th of a gram
+static int16_t log_yawpart;     // in 1/100th of a gram
+static int16_t log_maxThrust;   // in 1/100th of a gram
+
+static float thrust;
+static struct vec torque;
+
+static float thrust_to_torque = 0.006f;
+static float arm_length = 0.046f; // m
+
 int powerDistributionMotorType(uint32_t id)
 {
   return 1;
@@ -63,8 +88,9 @@ bool powerDistributionTest(void)
 
 #define limitThrust(VAL) limitUint16(VAL)
 
-void powerDistribution(motors_thrust_t* motorPower, const control_t *control)
+static void powerDistributionLegacy(motors_thrust_t* motorPower, const control_t *control)
 {
+  motorPower->mode = motorsThrustModePWM;
   int16_t r = control->roll / 2.0f;
   int16_t p = control->pitch / 2.0f;
   motorPower->m1 = limitThrust(control->thrust - r + p + control->yaw);
@@ -86,6 +112,207 @@ void powerDistribution(motors_thrust_t* motorPower, const control_t *control)
   }
 }
 
+static void powerDistributionForceTorque(motors_thrust_t* motorPower, const control_t *control, float maxThrust)
+{
+  // On CF2, thrust is mapped 65536 <==> 60 grams
+  thrust = control->thrustSI;
+  torque = mkvec(control->torque[0], control->torque[1], control->torque[2]);
+
+  // torque.x = clamp(torque.x, -0.002, 0.002);
+  // torque.y = clamp(torque.y, -0.002, 0.002);
+  // torque.z = clamp(torque.z, -0.0005, 0.0005);
+
+  // see https://github.com/jpreiss/libquadrotor/blob/master/src/quad_control.c
+  const float thrustpart = 0.25f * control->thrustSI; // N (per rotor)
+  const float yawpart = -0.25f * torque.z / thrust_to_torque;
+
+  float const arm = 0.707106781f * arm_length;
+  const float rollpart = 0.25f / arm * torque.x;
+  const float pitchpart = 0.25f / arm * torque.y;
+
+  // Thrust for each motor in N
+  saturationStatus = 0;
+
+  // Simple thrust mixing
+#if 1
+  motorForce[0] = thrustpart - rollpart - pitchpart + yawpart;
+  motorForce[1] = thrustpart - rollpart + pitchpart - yawpart;
+  motorForce[2] = thrustpart + rollpart + pitchpart + yawpart;
+  motorForce[3] = thrustpart + rollpart - pitchpart - yawpart;
+
+  log_thrustpart = thrustpart / 9.81f * 1000.0f * 100.0f; // convert to 1/100th of a gram
+  log_rollpart = rollpart / 9.81f * 1000.0f * 100.0f;
+  log_pitchpart = pitchpart / 9.81f * 1000.0f * 100.0f;
+  log_yawpart = yawpart / 9.81f * 1000.0f * 100.0f;
+  log_maxThrust = maxThrust * 100.0f;
+#else
+  // Thrust mixing similar to PX4 (see https://px4.github.io/Firmware-Doxygen/d7/d2a/mixer__multirotor_8cpp_source.html)
+  // 1. Mix thrust, roll, pitch without yaw
+  motorForce[0] = thrustpart - rollpart - pitchpart;
+  motorForce[1] = thrustpart - rollpart + pitchpart;
+  motorForce[2] = thrustpart + rollpart + pitchpart;
+  motorForce[3] = thrustpart + rollpart - pitchpart;
+
+  float minForce = motorForce[0];
+  float maxForce = motorForce[0];
+  for (int i = 1; i < 4; ++i) {
+    minForce = fminf(minForce, motorForce[i]);
+    maxForce = fmaxf(maxForce, motorForce[i]);
+  }
+
+  float deltaForce = maxForce - minForce;
+  float thrustOffset = 0;
+  float rollPitchScale = 1.0f;
+
+  // 2. Check if we can shift thrust to avoid saturation
+  if (deltaForce <= max_thrust) {
+    if (minForce < 0) {
+      thrustOffset = -minForce;
+    } else if (maxForce > max_thrust) {
+      thrustOffset = -(maxForce - max_thrust);
+    }
+  } else {
+    // shifting thrust is not sufficient => scale roll and pitch as well
+    rollPitchScale = max_thrust / deltaForce;
+    thrustOffset = max_thrust - ((maxForce - thrustpart) * rollPitchScale + thrustpart);
+  }
+
+  // 3. Mix thrust (with offset), roll/pitch (with scale) to identify margin for yaw
+  motorForce[0] = thrustpart+thrustOffset - rollpart*rollPitchScale - pitchpart*rollPitchScale;
+  motorForce[1] = thrustpart+thrustOffset - rollpart*rollPitchScale + pitchpart*rollPitchScale;
+  motorForce[2] = thrustpart+thrustOffset + rollpart*rollPitchScale + pitchpart*rollPitchScale;
+  motorForce[3] = thrustpart+thrustOffset + rollpart*rollPitchScale - pitchpart*rollPitchScale;
+
+  float maxYawPart = max_thrust - motorForce[0]; // positive yaw can be at most
+  float minYawPart = 0 - motorForce[0]; // negative yaw can be at most
+  maxYawPart = fmaxf(maxYawPart, motorForce[1]);
+  maxYawPart = fmaxf(maxYawPart, max_thrust - motorForce[2]);
+  maxYawPart = fmaxf(maxYawPart, motorForce[3]);
+  minYawPart = fminf(minYawPart, motorForce[1] - max_thrust);
+  minYawPart = fminf(minYawPart, 0 - motorForce[2]);
+  minYawPart = fminf(minYawPart, motorForce[3] - max_thrust);
+
+  float clamped_yawpart = clamp(yawpart, minYawPart, maxYawPart);
+  if (thrustOffset != 0) {
+    saturationStatus |= SaturationOffsetThrust;
+  }
+  if (rollPitchScale != 1.0f) {
+    saturationStatus |= SaturationRollPitch;
+  }
+  if (yawpart != clamped_yawpart) {
+    saturationStatus |= SaturationYaw;
+  }
+
+  // 4. Final thrust mixing (unlike PX4, we do not allow to reduce thrust to get yaw response)
+  motorForce[0] = thrustpart+thrustOffset - rollpart*rollPitchScale - pitchpart*rollPitchScale + clamped_yawpart;
+  motorForce[1] = thrustpart+thrustOffset - rollpart*rollPitchScale + pitchpart*rollPitchScale - clamped_yawpart;
+  motorForce[2] = thrustpart+thrustOffset + rollpart*rollPitchScale + pitchpart*rollPitchScale + clamped_yawpart;
+  motorForce[3] = thrustpart+thrustOffset + rollpart*rollPitchScale - pitchpart*rollPitchScale - clamped_yawpart;
+#endif
+  // for CF2, motorratio directly maps to thrust (not rpm etc.)
+  // Thus, we only need to scale the values here
+
+
+#if 0
+  const float maxThrust = maxThrustInGram * 9.81f / 1000.0f; // N
+
+  // yaw-torque saturation
+  // a) mix without yaw
+  motorForce[0] = thrustpart - rollpart - pitchpart;
+  motorForce[1] = thrustpart - rollpart + pitchpart;
+  motorForce[2] = thrustpart + rollpart + pitchpart;
+  motorForce[3] = thrustpart + rollpart - pitchpart;
+
+  // b) find maxYawPart
+  float maxYawPart = maxThrust - motorForce[0]; // positive yaw can be at most
+  float minYawPart = 0 - motorForce[0]; // negative yaw can be at most
+  maxYawPart = fmaxf(maxYawPart, motorForce[1]);
+  maxYawPart = fmaxf(maxYawPart, maxThrust - motorForce[2]);
+  maxYawPart = fmaxf(maxYawPart, motorForce[3]);
+  minYawPart = fminf(minYawPart, motorForce[1] - maxThrust);
+  minYawPart = fminf(minYawPart, 0 - motorForce[2]);
+  minYawPart = fminf(minYawPart, motorForce[3] - maxThrust);
+
+  float clamped_yawpart = 0;
+  if (minYawPart < maxYawPart) {
+    clamped_yawpart = clamp(yawpart, minYawPart, maxYawPart);
+  }
+
+  // c) re-mix
+  motorForce[0] = thrustpart - rollpart - pitchpart + clamped_yawpart;
+  motorForce[1] = thrustpart - rollpart + pitchpart - clamped_yawpart;
+  motorForce[2] = thrustpart + rollpart + pitchpart + clamped_yawpart;
+  motorForce[3] = thrustpart + rollpart - pitchpart - clamped_yawpart;
+
+  // collective-thrust saturation: skip for now
+#endif
+
+  motorPower->mode = motorsThrustModeForce;
+  motorPower->f1 = clamp(motorForce[0] / 9.81f * 1000.0f, 0, maxThrust);
+  motorPower->f2 = clamp(motorForce[1] / 9.81f * 1000.0f, 0, maxThrust);
+  motorPower->f3 = clamp(motorForce[2] / 9.81f * 1000.0f, 0, maxThrust);
+  motorPower->f4 = clamp(motorForce[3] / 9.81f * 1000.0f, 0, maxThrust);
+}
+
+static void powerDistributionForce(motors_thrust_t* motorPower, const control_t *control, float maxThrust)
+{
+  motorPower->mode = motorsThrustModeForce;
+  motorPower->f1 = control->normalizedForces[0] * maxThrust;
+  motorPower->f2 = control->normalizedForces[1] * maxThrust;
+  motorPower->f3 = control->normalizedForces[2] * maxThrust;
+  motorPower->f4 = control->normalizedForces[3] * maxThrust;
+}
+
+void powerDistribution(motors_thrust_t* motorPower, const control_t *control, float maxThrust)
+{
+  switch (control->controlMode)
+  {
+    case controlModeLegacy:
+      powerDistributionLegacy(motorPower, control);
+      break;
+    case controlModeForceTorque:
+      powerDistributionForceTorque(motorPower, control, maxThrust);
+      break;
+    case controlModeForce:
+      powerDistributionForce(motorPower, control, maxThrust);
+      break;
+  }
+}
+
+/**
+ * Power distribution related log variables.
+ * Use the following to compute desired motor forces:
+ * 
+ * M1 = thrustPart - rollPart - pitchPart + yawPart;
+ * M2 = thrustPart - rollPart + pitchPart - yawPart;
+ * M3 = thrustPart + rollPart + pitchPart + yawPart;
+ * M4 = thrustPart + rollPart - pitchPart - yawPart;
+ * 
+ * Note that the actual commanded motor forces will be clipped between 0 and maxThrust
+ */
+LOG_GROUP_START(powerDist)
+/**
+ * @brief Thrust part of power distribution; divide by 100 to get grams
+ */
+LOG_ADD_CORE(LOG_INT16, thrustPart, &log_thrustpart)
+/**
+ * @brief Roll part of power distribution; divide by 100 to get grams
+ */
+LOG_ADD_CORE(LOG_INT16, rollPart, &log_rollpart)
+/**
+ * @brief Pitch part of power distribution; divide by 100 to get grams
+ */
+LOG_ADD_CORE(LOG_INT16, pitchPart, &log_pitchpart)
+/**
+ * @brief Yaw part of power distribution; divide by 100 to get grams
+ */
+LOG_ADD_CORE(LOG_INT16, yawPart, &log_yawpart)
+/**
+ * @brief Maximum thrust for one motor; divide by 100 to get grams
+ */
+LOG_ADD_CORE(LOG_INT16, maxThrust, &log_maxThrust)
+LOG_GROUP_STOP(powerDist)
+
 /**
  * Power distribution parameters
  */
@@ -99,3 +326,9 @@ PARAM_GROUP_START(powerDist)
  */
 PARAM_ADD_CORE(PARAM_UINT32 | PARAM_PERSISTENT, idleThrust, &idleThrust)
 PARAM_GROUP_STOP(powerDist)
+
+
+PARAM_GROUP_START(sysId)
+PARAM_ADD(PARAM_FLOAT, thrust_to_torque, &thrust_to_torque)
+PARAM_ADD(PARAM_FLOAT, arm_length, &arm_length)
+PARAM_GROUP_STOP(sysId)
\ No newline at end of file
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/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c
index 01b1c8fd61..fdaa31f7b6 100644
--- a/src/modules/src/stabilizer.c
+++ b/src/modules/src/stabilizer.c
@@ -56,6 +56,8 @@
 #include "statsCnt.h"
 #include "static_mem.h"
 #include "rateSupervisor.h"
+#include "peer_localization.h"
+#include "math3d.h"
 
 static bool isInit;
 static bool emergencyStop = false;
@@ -98,6 +100,16 @@ static struct {
   int16_t rateRoll;
   int16_t ratePitch;
   int16_t rateYaw;
+
+  // payload position - mm
+  int16_t px;
+  int16_t py;
+  int16_t pz;
+
+  // payload velocity - mm / sec
+  int16_t pvx;
+  int16_t pvy;
+  int16_t pvz;
 } stateCompressed;
 
 static struct {
@@ -115,6 +127,11 @@ static struct {
   int16_t az;
 } setpointCompressed;
 
+// for payloads
+static float payload_alpha = 0.9; // between 0...1; 1: no filter
+static point_t payload_pos_last;         // m   (world frame)
+static velocity_t payload_vel_last;      // m/s (world frame)
+
 STATIC_MEM_TASK_ALLOC(stabilizerTask, STABILIZER_TASK_STACKSIZE);
 
 static void stabilizerTask(void* param);
@@ -150,6 +167,15 @@ static void compressState()
   stateCompressed.rateRoll = sensorData.gyro.x * deg2millirad;
   stateCompressed.ratePitch = -sensorData.gyro.y * deg2millirad;
   stateCompressed.rateYaw = sensorData.gyro.z * deg2millirad;
+
+  // payload
+  stateCompressed.px = state.payload_pos.x * 1000.0f;
+  stateCompressed.py = state.payload_pos.y * 1000.0f;
+  stateCompressed.pz = state.payload_pos.z * 1000.0f;
+
+  stateCompressed.pvx = state.payload_vel.x * 1000.0f;
+  stateCompressed.pvy = state.payload_vel.y * 1000.0f;
+  stateCompressed.pvz = state.payload_vel.z * 1000.0f;
 }
 
 static void compressSetpoint()
@@ -237,7 +263,8 @@ static void stabilizerTask(void* param)
 
   rateSupervisorInit(&rateSupervisorContext, xTaskGetTickCount(), M2T(1000), 997, 1003, 1);
 
-  DEBUG_PRINT("Ready to fly.\n");
+  payload_pos_last.timestamp = 0;
+   DEBUG_PRINT("Ready to fly.\n");
 
   while(1) {
     // The sensor should unlock at 1kHz
@@ -256,11 +283,53 @@ static void stabilizerTask(void* param)
       }
       // allow to update controller dynamically
       if (getControllerType() != controllerType) {
+        control.controlMode = controlModeLegacy;
         controllerInit(controllerType);
         controllerType = getControllerType();
       }
 
       stateEstimator(&state, tick);
+
+      // add the payload state here
+      peerLocalizationOtherPosition_t* payloadPos = peerLocalizationGetPositionByID(255);
+      if (payloadPos != NULL) {
+        
+        // if we got a new state
+        if (payload_pos_last.timestamp < payloadPos->pos.timestamp) {
+          struct vec vel_filtered = vzero();
+          // in the beginning, estimate the velocity to be zero, otherwise use
+          // numeric estimation with filter
+          if (payload_pos_last.timestamp != 0) {
+            // estimate the velocity numerically
+            const float dt = (payloadPos->pos.timestamp - payload_pos_last.timestamp) / 1000.0f; //s
+            struct vec pos = mkvec(payloadPos->pos.x, payloadPos->pos.y, payloadPos->pos.z);
+            struct vec last_pos = mkvec(payload_pos_last.x, payload_pos_last.y, payload_pos_last.z);
+            struct vec vel = vdiv(vsub(pos, last_pos), dt);
+
+            // apply a simple complementary filter
+            struct vec vel_old = mkvec(payload_vel_last.x, payload_vel_last.y, payload_vel_last.z);
+            vel_filtered = vadd(vscl(1.0f - payload_alpha, vel_old), vscl(payload_alpha, vel));
+          }
+          // update the position
+          state.payload_pos.x = payloadPos->pos.x;
+          state.payload_pos.y = payloadPos->pos.y;
+          state.payload_pos.z = payloadPos->pos.z;
+          state.payload_pos.timestamp = payloadPos->pos.timestamp;
+
+          // update the velocity
+          state.payload_vel.x = vel_filtered.x;
+          state.payload_vel.y = vel_filtered.y;
+          state.payload_vel.z = vel_filtered.z;
+          state.payload_vel.timestamp = payloadPos->pos.timestamp;
+          
+          // update state
+          payload_pos_last = state.payload_pos;
+          payload_vel_last = state.payload_vel;
+        } else {
+          state.payload_pos = payload_pos_last;
+          state.payload_vel = payload_vel_last;
+        }
+      }
       compressState();
 
       if (crtpCommanderHighLevelGetSetpoint(&tempSetpoint, &state, tick)) {
@@ -285,11 +354,19 @@ static void stabilizerTask(void* param)
       if (emergencyStop || (systemIsArmed() == false)) {
         motorsStop();
       } else {
-        powerDistribution(&motorPower, &control);
-        motorsSetRatio(MOTOR_M1, motorPower.m1);
-        motorsSetRatio(MOTOR_M2, motorPower.m2);
-        motorsSetRatio(MOTOR_M3, motorPower.m3);
-        motorsSetRatio(MOTOR_M4, motorPower.m4);
+        float maxThrust = motorsGetMaxThrust();
+        powerDistribution(&motorPower, &control, maxThrust);
+        if (motorPower.mode == motorsThrustModePWM) {
+          motorsSetRatio(MOTOR_M1, motorPower.m1);
+          motorsSetRatio(MOTOR_M2, motorPower.m2);
+          motorsSetRatio(MOTOR_M3, motorPower.m3);
+          motorsSetRatio(MOTOR_M4, motorPower.m4);
+        } else if (motorPower.mode == motorsThrustModeForce) {
+          motorsSetThrust(MOTOR_M1, motorPower.f1);
+          motorsSetThrust(MOTOR_M2, motorPower.f2);
+          motorsSetThrust(MOTOR_M3, motorPower.f3);
+          motorsSetThrust(MOTOR_M4, motorPower.f4);
+        }
       }
 
 #ifdef CONFIG_DECK_USD
@@ -350,6 +427,10 @@ PARAM_ADD_CORE(PARAM_UINT8, controller, &controllerType)
  * @brief If set to nonzero will turn off power
  */
 PARAM_ADD_CORE(PARAM_UINT8, stop, &emergencyStop)
+
+
+PARAM_ADD_CORE(PARAM_FLOAT, pAlpha, &payload_alpha)
+
 PARAM_GROUP_STOP(stabilizer)
 
 
@@ -792,4 +873,37 @@ LOG_ADD(LOG_INT16, ratePitch, &stateCompressed.ratePitch)
  * @brief Yaw rate (angular velocity) [milliradians / sec]
  */
 LOG_ADD(LOG_INT16, rateYaw, &stateCompressed.rateYaw)
+
+
+/**
+ * @brief The position of the payload in the global reference frame, X [mm]
+ */
+LOG_ADD(LOG_INT16, px, &stateCompressed.px)
+
+/**
+ * @brief The position of the payload in the global reference frame, Y [mm]
+ */
+LOG_ADD(LOG_INT16, py, &stateCompressed.py)
+
+/**
+ * @brief The position of the payload in the global reference frame, Z [mm]
+ */
+LOG_ADD(LOG_INT16, pz, &stateCompressed.pz)
+
+/**
+ * @brief The velocity of the payload in the global reference frame, X [mm/s]
+ */
+LOG_ADD(LOG_INT16, pvx, &stateCompressed.pvx)
+
+/**
+ * @brief The velocity of the payload in the global reference frame, Y [mm/s]
+ */
+LOG_ADD(LOG_INT16, pvy, &stateCompressed.pvy)
+
+/**
+ * @brief The velocity of the payload in the global reference frame, Z [mm/s]
+ */
+LOG_ADD(LOG_INT16, pvz, &stateCompressed.pvz)
+
+
 LOG_GROUP_STOP(stateEstimateZ)
diff --git a/src/utils/src/configblockeeprom.c b/src/utils/src/configblockeeprom.c
index 950bd332b5..4f363a169d 100644
--- a/src/utils/src/configblockeeprom.c
+++ b/src/utils/src/configblockeeprom.c
@@ -124,10 +124,6 @@ int configblockInit(void)
 
   eepromInit(I2C1_DEV);
 
-  // Because of strange behavior from I2C device during expansion port test
-  // the first read needs to be discarded
-  eepromTestConnection();
-
   if (eepromTestConnection())
   {
     if (eepromReadBuffer((uint8_t *)&configblock, 0, sizeof(configblock)))
@@ -197,7 +193,7 @@ int configblockInit(void)
 
 bool configblockTest(void)
 {
-  return eepromTest();
+  return cb_ok;
 }
 
 static bool configblockCheckMagic(configblock_t *configblock)
diff --git a/test_python/test_controller_lee.py b/test_python/test_controller_lee.py
new file mode 100644
index 0000000000..1b299b3777
--- /dev/null
+++ b/test_python/test_controller_lee.py
@@ -0,0 +1,47 @@
+#!/usr/bin/env python
+
+import cffirmware
+
+def test_controller_lee():
+
+    lee = cffirmware.controllerLee_t()
+
+    cffirmware.controllerLeeInit(lee)
+    # now we can change some controller parameters
+    lee.Kpos_D.z = 5
+
+    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
+
+    tick = 100
+
+    cffirmware.controllerLee(lee,control,setpoint,sensors,state,tick)
+    # control.thrust will be at a (tuned) hover-state
+    assert control.controlMode == cffirmware.controlModeForceTorque
+    assert control.torque[0] == 0
+    assert control.torque[1] == 0
+    assert control.torque[2] == 0
diff --git a/test_python/test_controller_lee_payload.py b/test_python/test_controller_lee_payload.py
new file mode 100644
index 0000000000..aacd08e99e
--- /dev/null
+++ b/test_python/test_controller_lee_payload.py
@@ -0,0 +1,50 @@
+#!/usr/bin/env python
+
+import cffirmware
+
+def test_controller_lee_payload():
+
+    cffirmware.controllerLeePayloadInit()
+
+    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
+
+    state.payload_pos.x = 0
+    state.payload_pos.y = 0
+    state.payload_pos.z = -0.784
+    state.payload_vel.x = 0
+    state.payload_vel.y = 0
+    state.payload_vel.z = 0
+
+    sensors = cffirmware.sensorData_t()
+    sensors.gyro.x = 0
+    sensors.gyro.y = 0
+    sensors.gyro.z = 0
+
+    tick = 500
+
+    cffirmware.controllerLeePayload(control, setpoint,sensors,state,tick)
+    # control.thrust will be at a (tuned) hover-state
+    assert control.controlMode == cffirmware.controlModeForceTorque
+    assert control.torque[0] == 0
+    assert control.torque[1] == 0
+    assert control.torque[2] == 0
diff --git a/test_python/test_power_distribution.py b/test_python/test_power_distribution.py
index 802392c717..584b3c65b5 100644
--- a/test_python/test_power_distribution.py
+++ b/test_python/test_power_distribution.py
@@ -13,7 +13,7 @@ def test_controller_mellinger():
     control.pitch = 0
     control.yaw = 0
 
-    cffirmware.powerDistribution(motorPower, control)
+    cffirmware.powerDistribution(motorPower, control, 0)
     # control.thrust will be at a (tuned) hover-state
     assert motorPower.m1 == control.thrust
     assert motorPower.m2 == control.thrust
diff --git a/tools/system_id/README.md b/tools/system_id/README.md
new file mode 100644
index 0000000000..0d22724dd1
--- /dev/null
+++ b/tools/system_id/README.md
@@ -0,0 +1,56 @@
+# System ID including PWM, RPM, Thrust, Voltage, Current, and Power
+
+This folder contains scripts to measure propellers and motors using the systemId deck.
+
+## Setup
+
+* Load cells:
+  * 100g: https://www.sparkfun.com/products/14727
+  * 500g: https://www.sparkfun.com/products/14728
+  * 1kg: https://www.adafruit.com/product/4540
+* SystemID board with the following ICs
+  * NAU7802 (thrust)
+  * ACS37800 (power)
+  * QRD1114 (RPM)
+* Calibration weights
+* M2/M3 Nylon screws
+* CF Mount (3D printed)
+
+Mount CF upside down (to avoid downwash)
+
+## Measurement
+
+### Common
+
+1. Mount CF
+2. Switch to crazyflie-lib-python with branch `feature-emergency-stop` in order to be able to CTRL+C a script safely.
+3. Flash firmware from dev-systemId branch
+4. Run `python3 calibscale.py --uri <URI>` and follow the prompts to calibrate the load cell. This will create an output file `calibration.yaml` with the calibration data. The other scripts will read this file (other name can be specified as command line argument).
+
+### Ramping Motors
+
+This test will ramp the motors and write collected data to `data.csv`.
+
+```
+python3 collect_data.py --uri <URI>
+```
+
+The results can be visualized with `plot_data.py`.
+
+### Find (PWM,VBat)->Force, (desired Force,VBat)->PWM, (PWM, VBat)-> MaxForce
+
+This test will randomly select sample a PWM value and average the resulting force and measured vbat over a short period of time. Then, it will output the largest possible PWM value to estimate the maximum force that the Crazyflie can create at the current state-of-charge. The mapping functions can be automatically computed.
+
+```
+python3 collect_data_max_thrust.py --uri <URI>
+python3 system_id.py
+```
+
+### Motor Delay
+
+This test uses a higher sampling rate and visualizes the delayed motor response given a step input.
+
+```
+python3 collect_data_motor_delay.py --uri <URI>
+python3 plot_motor_delay.py
+```
diff --git a/tools/system_id/calibscale.py b/tools/system_id/calibscale.py
new file mode 100644
index 0000000000..d5bb8ea222
--- /dev/null
+++ b/tools/system_id/calibscale.py
@@ -0,0 +1,145 @@
+"""
+Calibrate load cell
+"""
+import argparse
+import logging
+import time
+import yaml
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.log import LogConfig
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+
+class CalibScale:
+    def __init__(self, link_uri):
+        """ Initialize and run with the specified link_uri """
+
+        self.measurements = []
+
+        self._cf = Crazyflie(rw_cache='./cache')
+
+        # Connect some callbacks from the Crazyflie API
+        self._cf.connected.add_callback(self._connected)
+        self._cf.disconnected.add_callback(self._disconnected)
+        self._cf.connection_failed.add_callback(self._connection_failed)
+        self._cf.connection_lost.add_callback(self._connection_lost)
+
+        print('Connecting to %s' % link_uri)
+
+        # Try to connect to the Crazyflie
+        self._cf.open_link(link_uri)
+
+        # Variable used to keep main loop occupied until disconnect
+        self.is_connected = False
+
+    def _connected(self, link_uri):
+        """ This callback is called form the Crazyflie API when a Crazyflie
+        has been connected and the TOCs have been downloaded."""
+        print('Connected to %s' % link_uri)
+
+        # The definition of the logconfig can be made before connecting
+        self._lg_stab = LogConfig(name='data', period_in_ms=10)
+        self._lg_stab.add_variable('loadcell.weight', 'float')
+        self._lg_stab.add_variable('loadcell.rawWeight', 'int32_t')
+
+        # Adding the configuration cannot be done until a Crazyflie is
+        # connected, since we need to check that the variables we
+        # would like to log are in the TOC.
+        try:
+            self._cf.log.add_config(self._lg_stab)
+            # This callback will receive the data
+            self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
+            # This callback will be called on errors
+            self._lg_stab.error_cb.add_callback(self._stab_log_error)
+            # Start the logging
+            self._lg_stab.start()
+        except KeyError as e:
+            print('Could not start log configuration,'
+                  '{} not found in TOC'.format(str(e)))
+        except AttributeError:
+            print('Could not add Stabilizer log config, bad configuration.')
+
+        self.is_connected = True
+
+    def _stab_log_error(self, logconf, msg):
+        """Callback from the log API when an error occurs"""
+        print('Error when logging %s: %s' % (logconf.name, msg))
+
+    def _stab_log_data(self, timestamp, data, logconf):
+        """Callback froma the log API when data arrives"""
+        # print('[%d][%s]: %s' % (timestamp, logconf.name, data))
+        self.measurements.append(data['loadcell.rawWeight'])
+
+    def _connection_failed(self, link_uri, msg):
+        """Callback when connection initial connection fails (i.e no Crazyflie
+        at the speficied address)"""
+        print('Connection to %s failed: %s' % (link_uri, msg))
+        self.is_connected = False
+
+    def _connection_lost(self, link_uri, msg):
+        """Callback when disconnected after a connection has been made (i.e
+        Crazyflie moves out of range)"""
+        print('Connection to %s lost: %s' % (link_uri, msg))
+
+    def _disconnected(self, link_uri):
+        """Callback when the Crazyflie is disconnected (called in all cases)"""
+        print('Disconnected from %s' % link_uri)
+        self.is_connected = False
+
+    def measure(self, num_measurements = 100):
+        weights = []
+        readings = []
+
+        while True:
+            data = float(input("enter weight in grams (-1 to end): "))
+            if data < 0:
+                break
+            self.measurements = []
+            while len(self.measurements) < num_measurements:
+                time.sleep(0.1)
+            print(np.mean(self.measurements))
+            weights.append(data)
+            readings.append(np.mean(self.measurements))
+
+        self._cf.close_link()
+
+        z = np.polyfit(readings, weights, 1)
+        p = np.poly1d(z)
+
+        xp = np.linspace(readings[0], readings[-1], 100)
+
+
+        plt.plot(readings, weights, '.', xp, p(xp), '--')
+        plt.show()
+        return float(z[0]), float(z[1])
+
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--output", default="calibration.yaml", help="Output file containing the calibration")
+    parser.add_argument("--uri", default="radio://0/42/2M/E7E7E7E7E7", help="URI of Crazyflie")
+    args = parser.parse_args()
+
+    # Only output errors from the logging framework
+    logging.basicConfig(level=logging.ERROR)
+    
+    # Initialize the low-level drivers (don't list the debug drivers)
+    cflib.crtp.init_drivers(enable_debug_driver=False)
+    le = CalibScale(args.uri)
+
+    while not le.is_connected:
+        time.sleep(0.1)
+
+    a,b = le.measure()
+    result = dict()
+    result['a'] = a
+    result['b'] = b
+
+    with open(args.output, 'w') as yaml_file:
+        yaml.dump(result, yaml_file)
+
+
diff --git a/tools/system_id/collect_data.py b/tools/system_id/collect_data.py
new file mode 100644
index 0000000000..b23742573f
--- /dev/null
+++ b/tools/system_id/collect_data.py
@@ -0,0 +1,188 @@
+"""
+Ramp motors and collect data using the system-id deck
+"""
+import argparse
+import logging
+import time
+import yaml
+from threading import Thread
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.log import LogConfig
+from cflib.crazyflie.localization import Localization
+
+
+class CollectData:
+    """
+    Simple logging example class that logs the Stabilizer from a supplied
+    link uri and disconnects after 5s.
+    """
+
+    def __init__(self, link_uri, calib_a, calib_b):
+        """ Initialize and run the example with the specified link_uri """
+        self.calib_a = calib_a
+        self.calib_b = calib_b
+
+        self._cf = Crazyflie(rw_cache='./cache')
+
+        # Connect some callbacks from the Crazyflie API
+        self._cf.connected.add_callback(self._connected)
+        self._cf.disconnected.add_callback(self._disconnected)
+        self._cf.connection_failed.add_callback(self._connection_failed)
+        self._cf.connection_lost.add_callback(self._connection_lost)
+
+        print('Connecting to %s' % link_uri)
+
+        # Try to connect to the Crazyflie
+        self._cf.open_link(link_uri)
+
+        # Variable used to keep main loop occupied until disconnect
+        self.is_connected = True
+
+    def _connected(self, link_uri):
+        """ This callback is called form the Crazyflie API when a Crazyflie
+        has been connected and the TOCs have been downloaded."""
+        print('Connected to %s' % link_uri)
+
+        print(self.calib_a, self.calib_b)
+        self._cf.param.set_value('loadcell.a', str(self.calib_a))
+        self._cf.param.set_value('loadcell.b', str(self.calib_b))
+
+        self._file = open("data.csv", "w+")
+        self._file.write("weight[g],pwm,vbat[V],rpm1,rpm2,rpm3,rpm4,v[V],i[A],p[W]\n");
+
+        # The definition of the logconfig can be made before connecting
+        self._lg_stab = LogConfig(name='data', period_in_ms=10)
+        self._lg_stab.add_variable('loadcell.weight', 'float')
+        self._lg_stab.add_variable('pwm.m1_pwm', 'uint16_t')
+        self._lg_stab.add_variable('pm.vbatMV', 'uint16_t')
+        self._lg_stab.add_variable('rpm.m1', 'uint16_t')
+        self._lg_stab.add_variable('rpm.m2', 'uint16_t')
+        self._lg_stab.add_variable('rpm.m3', 'uint16_t')
+        self._lg_stab.add_variable('rpm.m4', 'uint16_t')
+        self._lg_stab.add_variable('asc37800.v_mV', 'int16_t')
+        self._lg_stab.add_variable('asc37800.i_mA', 'int16_t')
+        self._lg_stab.add_variable('asc37800.p_mW', 'int16_t')
+
+        # Adding the configuration cannot be done until a Crazyflie is
+        # connected, since we need to check that the variables we
+        # would like to log are in the TOC.
+        try:
+            self._cf.log.add_config(self._lg_stab)
+            # This callback will receive the data
+            self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
+            # This callback will be called on errors
+            self._lg_stab.error_cb.add_callback(self._stab_log_error)
+            # Start the logging
+            self._lg_stab.start()
+        except KeyError as e:
+            print('Could not start log configuration,'
+                  '{} not found in TOC'.format(str(e)))
+        except AttributeError:
+            print('Could not add Stabilizer log config, bad configuration.')
+
+        # Start a separate thread to do the motor test.
+        # Do not hijack the calling thread!
+        Thread(target=self._ramp_motors).start()
+
+    def _stab_log_error(self, logconf, msg):
+        """Callback from the log API when an error occurs"""
+        print('Error when logging %s: %s' % (logconf.name, msg))
+
+    def _stab_log_data(self, timestamp, data, logconf):
+        """Callback froma the log API when data arrives"""
+        print('[%d][%s]: %s' % (timestamp, logconf.name, data))
+        self._file.write("{},{},{},{},{},{},{},{},{},{}\n".format(
+            data['loadcell.weight'],
+            data['pwm.m1_pwm'],
+            data['pm.vbatMV']/ 1000,
+            data['rpm.m1'],
+            data['rpm.m2'],
+            data['rpm.m3'],
+            data['rpm.m4'],
+            data['asc37800.v_mV']/ 1000,
+            data['asc37800.i_mA']/ 1000,
+            data['asc37800.p_mW']/ 1000))
+
+    def _connection_failed(self, link_uri, msg):
+        """Callback when connection initial connection fails (i.e no Crazyflie
+        at the speficied address)"""
+        print('Connection to %s failed: %s' % (link_uri, msg))
+        self.is_connected = False
+
+    def _connection_lost(self, link_uri, msg):
+        """Callback when disconnected after a connection has been made (i.e
+        Crazyflie moves out of range)"""
+        print('Connection to %s lost: %s' % (link_uri, msg))
+
+    def _disconnected(self, link_uri):
+        """Callback when the Crazyflie is disconnected (called in all cases)"""
+        print('Disconnected from %s' % link_uri)
+        self.is_connected = False
+
+    def _ramp_motors(self):
+        thrust_mult = 1
+        thrust_step = 500
+        time_step = 0.1
+        thrust = 0
+        pitch = 0
+        roll = 0
+        yawrate = 0
+
+        # # Unlock startup thrust protection
+        # for i in range(0, 100):
+        #     self._cf.commander.send_setpoint(0, 0, 0, 0)
+
+        localization = Localization(self._cf)
+
+        self._cf.param.set_value('motor.batCompensation', 0)
+        self._cf.param.set_value('motorPowerSet.m1', 0)
+        self._cf.param.set_value('motorPowerSet.enable', 2)
+        self._cf.param.set_value('system.forceArm', 1)
+
+        while self.is_connected: #thrust >= 0:
+            thrust += thrust_step * thrust_mult
+            if thrust >= 65536 or thrust < 0:
+            # if thrust >= 20000 or thrust < 0:
+                thrust_mult *= -1
+                thrust += thrust_step * thrust_mult
+            print(thrust)
+            # self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
+            localization.send_emergency_stop_watchdog()
+            self._cf.param.set_value('motorPowerSet.m1', str(thrust))
+            time.sleep(time_step)
+
+        # self._cf.commander.send_setpoint(0, 0, 0, 0)
+        # Make sure that the last packet leaves before the link is closed
+        # since the message queue is not flushed before closing
+        # time.sleep(0.1)
+        # self._cf.close_link()
+
+
+if __name__ == '__main__':
+
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--calibration", default="calibration.yaml", help="Input file containing the calibration")
+    parser.add_argument("--uri", default="radio://0/42/2M/E7E7E7E7E7", help="URI of Crazyflie")
+    args = parser.parse_args()
+
+    # Only output errors from the logging framework
+    logging.basicConfig(level=logging.ERROR)
+
+    # Initialize the low-level drivers (don't list the debug drivers)
+    cflib.crtp.init_drivers(enable_debug_driver=False)
+
+    with open(args.calibration, 'r') as f:
+        r = yaml.safe_load(f)
+        a = r['a']
+        b = r['b']
+
+    # collect data
+    le = CollectData(args.uri, a, b)
+
+    # The Crazyflie lib doesn't contain anything to keep the application alive,
+    # so this is where your application should do something. In our case we
+    # are just waiting until we are disconnected.
+    while le.is_connected:
+        time.sleep(1)
diff --git a/tools/system_id/collect_data_max_thrust.py b/tools/system_id/collect_data_max_thrust.py
new file mode 100644
index 0000000000..2c11cbe3ec
--- /dev/null
+++ b/tools/system_id/collect_data_max_thrust.py
@@ -0,0 +1,203 @@
+"""
+Example that uses a load cell to measure thrust using different RPMs
+"""
+import argparse
+import logging
+import time
+from threading import Thread
+import numpy as np
+import yaml
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.log import LogConfig
+from cflib.crazyflie.localization import Localization
+
+
+# Only output errors from the logging framework
+logging.basicConfig(level=logging.ERROR)
+
+
+class CollectData:
+    """
+    Simple logging example class that logs the Stabilizer from a supplied
+    link uri and disconnects after 5s.
+    """
+
+    def __init__(self, link_uri, calib_a, calib_b):
+        """ Initialize and run the example with the specified link_uri """
+        self.measurements = []
+        self.desiredThrust = 0
+        self.calib_a = calib_a
+        self.calib_b = calib_b
+
+        self._cf = Crazyflie(rw_cache='./cache')
+
+        # Connect some callbacks from the Crazyflie API
+        self._cf.connected.add_callback(self._connected)
+        self._cf.disconnected.add_callback(self._disconnected)
+        self._cf.connection_failed.add_callback(self._connection_failed)
+        self._cf.connection_lost.add_callback(self._connection_lost)
+
+        print('Connecting to %s' % link_uri)
+
+        # Try to connect to the Crazyflie
+        self._cf.open_link(link_uri)
+
+        # Variable used to keep main loop occupied until disconnect
+        self.is_connected = True
+
+    def _connected(self, link_uri):
+        """ This callback is called form the Crazyflie API when a Crazyflie
+        has been connected and the TOCs have been downloaded."""
+        print('Connected to %s' % link_uri)
+
+        print(self.calib_a, self.calib_b)
+        self._cf.param.set_value('loadcell.a', str(self.calib_a))
+        self._cf.param.set_value('loadcell.b', str(self.calib_b))
+
+        self._file = open("data.csv", "w+")
+        self._file.write("thrust[g],pwm,vbat[V],maxThrust[g],maxThrustVbat[V],rpm\n");
+
+        # The definition of the logconfig can be made before connecting
+        self._lg_stab = LogConfig(name='data', period_in_ms=10)
+        self._lg_stab.add_variable('loadcell.weight', 'float')
+        self._lg_stab.add_variable('pwm.m1_pwm', 'uint16_t')
+        self._lg_stab.add_variable('pm.vbat', 'float')
+        self._lg_stab.add_variable('rpm.m1', 'uint16_t')
+        self._lg_stab.add_variable('rpm.m2', 'uint16_t')
+        self._lg_stab.add_variable('rpm.m3', 'uint16_t')
+        self._lg_stab.add_variable('rpm.m4', 'uint16_t')
+
+        # Adding the configuration cannot be done until a Crazyflie is
+        # connected, since we need to check that the variables we
+        # would like to log are in the TOC.
+        try:
+            self._cf.log.add_config(self._lg_stab)
+            # This callback will receive the data
+            self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
+            # This callback will be called on errors
+            self._lg_stab.error_cb.add_callback(self._stab_log_error)
+            # Start the logging
+            self._lg_stab.start()
+        except KeyError as e:
+            print('Could not start log configuration,'
+                  '{} not found in TOC'.format(str(e)))
+        except AttributeError:
+            print('Could not add Stabilizer log config, bad configuration.')
+
+        self._localization = Localization(self._cf)
+
+        # Start a separate thread to do the motor test.
+        # Do not hijack the calling thread!
+        Thread(target=self._ramp_motors).start()
+
+        # # Start a timer to disconnect in 10s
+        # t = Timer(5, self._cf.close_link)
+        # t.start()
+
+    def _stab_log_error(self, logconf, msg):
+        """Callback from the log API when an error occurs"""
+        print('Error when logging %s: %s' % (logconf.name, msg))
+
+    def _stab_log_data(self, timestamp, data, logconf):
+        """Callback froma the log API when data arrives"""
+        # print('[%d][%s]: %s' % (timestamp, logconf.name, data))
+        # self._file.write("{},{},{}\n".format(data['loadcell.weight'], data['pwm.m1_pwm'], data['pm.vbat']))
+        if self.desiredThrust == data['pwm.m1_pwm']:
+            self.measurements.append(np.array([data['loadcell.weight'], data['pm.vbat']]))
+        # pass
+
+    def _connection_failed(self, link_uri, msg):
+        """Callback when connection initial connection fails (i.e no Crazyflie
+        at the speficied address)"""
+        print('Connection to %s failed: %s' % (link_uri, msg))
+        self.is_connected = False
+
+    def _connection_lost(self, link_uri, msg):
+        """Callback when disconnected after a connection has been made (i.e
+        Crazyflie moves out of range)"""
+        print('Connection to %s lost: %s' % (link_uri, msg))
+
+    def _disconnected(self, link_uri):
+        """Callback when the Crazyflie is disconnected (called in all cases)"""
+        print('Disconnected from %s' % link_uri)
+        self.is_connected = False
+
+    def _measure(self, thrust, min_samples = 50):
+        self.desiredThrust = thrust
+        self.measurements = []
+        self._cf.param.set_value('motorPowerSet.m1', str(thrust))
+        while len(self.measurements) < min_samples:
+            self._localization.send_emergency_stop_watchdog()
+            time.sleep(0.1)
+        m = np.array(self.measurements)
+        # only return the last few samples
+        return m[40:]
+
+    def _ramp_motors(self):
+
+        self._cf.param.set_value('motor.batCompensation', 0)
+        self._cf.param.set_value('motorPowerSet.m1', 0)
+        self._cf.param.set_value('motorPowerSet.enable', 2)
+        self._cf.param.set_value('system.forceArm', 1)
+
+        while self.is_connected: #thrust >= 0:
+            # randomply sample PWM
+            thrust = int(np.random.uniform(15000, 65535))
+            measurements = self._measure(thrust)
+            # print(measurements)
+            achievedThrust = np.mean(measurements[:,0])
+            vbat = np.mean(measurements[:,1])
+
+            # go to full thrust
+            measurements = self._measure(65535)
+            # print(measurements)
+            maxThrust = np.mean(measurements[:,0])
+            vbatAtMaxThrust = np.mean(measurements[:,1])
+
+            # write result
+            print(achievedThrust, thrust, vbat, maxThrust, vbatAtMaxThrust)
+            self._file.write("{},{},{},{},{}\n".format(
+                achievedThrust,
+                thrust,
+                vbat,
+                maxThrust,
+                vbatAtMaxThrust))
+
+            if vbatAtMaxThrust < 2.8:
+                break
+
+        # self._cf.commander.send_setpoint(0, 0, 0, 0)
+        # Make sure that the last packet leaves before the link is closed
+        # since the message queue is not flushed before closing
+        time.sleep(0.1)
+        self._cf.close_link()
+        self._file.close()
+
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--calibration", default="calibration.yaml", help="Input file containing the calibration")
+    parser.add_argument("--uri", default="radio://0/42/2M/E7E7E7E7E7", help="URI of Crazyflie")
+    args = parser.parse_args()
+
+    # Only output errors from the logging framework
+    logging.basicConfig(level=logging.ERROR)
+
+    # Initialize the low-level drivers (don't list the debug drivers)
+    cflib.crtp.init_drivers(enable_debug_driver=False)
+
+    with open(args.calibration, 'r') as f:
+        r = yaml.safe_load(f)
+        a = r['a']
+        b = r['b']
+
+    # collect data
+    le = CollectData(args.uri, a, b)
+
+    # The Crazyflie lib doesn't contain anything to keep the application alive,
+    # so this is where your application should do something. In our case we
+    # are just waiting until we are disconnected.
+    while le.is_connected:
+        time.sleep(1)
diff --git a/tools/system_id/collect_data_motor_delay.py b/tools/system_id/collect_data_motor_delay.py
new file mode 100644
index 0000000000..3ba6964dd9
--- /dev/null
+++ b/tools/system_id/collect_data_motor_delay.py
@@ -0,0 +1,194 @@
+"""
+Example that uses a load cell to measure thrust using different RPMs
+"""
+import argparse
+import logging
+import time
+from threading import Thread
+import yaml
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.log import LogConfig
+from cflib.crazyflie.localization import Localization
+
+
+# Only output errors from the logging framework
+logging.basicConfig(level=logging.ERROR)
+
+
+class CollectData:
+    """
+    Simple logging example class that logs the Stabilizer from a supplied
+    link uri and disconnects after 5s.
+    """
+
+    def __init__(self, link_uri, calib_a, calib_b):
+        """ Initialize and run the example with the specified link_uri """
+        self.calib_a = calib_a
+        self.calib_b = calib_b
+
+        self._cf = Crazyflie(rw_cache='./cache')
+
+        # Connect some callbacks from the Crazyflie API
+        self._cf.connected.add_callback(self._connected)
+        self._cf.disconnected.add_callback(self._disconnected)
+        self._cf.connection_failed.add_callback(self._connection_failed)
+        self._cf.connection_lost.add_callback(self._connection_lost)
+
+        print('Connecting to %s' % link_uri)
+
+        # Try to connect to the Crazyflie
+        self._cf.open_link(link_uri)
+
+        # Variable used to keep main loop occupied until disconnect
+        self.is_connected = True
+
+    def _connected(self, link_uri):
+        """ This callback is called form the Crazyflie API when a Crazyflie
+        has been connected and the TOCs have been downloaded."""
+        print('Connected to %s' % link_uri)
+
+        print(self.calib_a, self.calib_b)
+        self._cf.param.set_value('loadcell.a', str(self.calib_a))
+        self._cf.param.set_value('loadcell.b', str(self.calib_b))
+        self._cf.param.set_value('loadcell.sampleRate', 7) # fastest sample rate: 320 Hz;
+        # self._cf.param.set_value('loadcell.sampleRate', 4)
+
+        self._file = open("data.csv", "w+")
+        self._file.write("time[ms],weight[g],pwm,vbat[V],rpm1,rpm2,rpm3,rpm4,v[V],i[A]\n");
+
+        # The definition of the logconfig can be made before connecting
+        self._lg_stab = LogConfig(name='data', period_in_ms=5)
+        self._lg_stab.add_variable('loadcell.weight', 'float')
+        self._lg_stab.add_variable('pwm.m1_pwm', 'uint16_t')
+        self._lg_stab.add_variable('pm.vbatMV', 'uint16_t')
+        self._lg_stab.add_variable('rpm.m1', 'uint16_t')
+        self._lg_stab.add_variable('rpm.m2', 'uint16_t')
+        self._lg_stab.add_variable('rpm.m3', 'uint16_t')
+        self._lg_stab.add_variable('rpm.m4', 'uint16_t')
+        self._lg_stab.add_variable('asc37800.v_avg', 'float')
+        self._lg_stab.add_variable('asc37800.i_avg', 'float')
+
+        # Adding the configuration cannot be done until a Crazyflie is
+        # connected, since we need to check that the variables we
+        # would like to log are in the TOC.
+        try:
+            self._cf.log.add_config(self._lg_stab)
+            # This callback will receive the data
+            self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
+            # This callback will be called on errors
+            self._lg_stab.error_cb.add_callback(self._stab_log_error)
+            # Start the logging
+            self._lg_stab.start_v2() # Use special v2 version to be able to sample > 100 Hz
+        except KeyError as e:
+            print('Could not start log configuration,'
+                  '{} not found in TOC'.format(str(e)))
+        except AttributeError:
+            print('Could not add Stabilizer log config, bad configuration.')
+
+        self._localization = Localization(self._cf)
+
+        # Start a separate thread to do the motor test.
+        # Do not hijack the calling thread!
+        Thread(target=self._ramp_motors).start()
+
+        # # Start a timer to disconnect in 10s
+        # t = Timer(5, self._cf.close_link)
+        # t.start()
+
+    def _stab_log_error(self, logconf, msg):
+        """Callback from the log API when an error occurs"""
+        print('Error when logging %s: %s' % (logconf.name, msg))
+
+    def _stab_log_data(self, timestamp, data, logconf):
+        """Callback froma the log API when data arrives"""
+        print('[%d][%s]: %s' % (timestamp, logconf.name, data))
+        self._file.write("{},{},{},{},{},{},{},{},{},{}\n".format(
+            timestamp,
+            data['loadcell.weight'],
+            data['pwm.m1_pwm'],
+            data['pm.vbatMV'] / 1000,
+            data['rpm.m1'],
+            data['rpm.m2'],
+            data['rpm.m3'],
+            data['rpm.m4'],
+            data['asc37800.v_avg'],
+            data['asc37800.i_avg']))
+
+    def _connection_failed(self, link_uri, msg):
+        """Callback when connection initial connection fails (i.e no Crazyflie
+        at the speficied address)"""
+        print('Connection to %s failed: %s' % (link_uri, msg))
+        self.is_connected = False
+
+    def _connection_lost(self, link_uri, msg):
+        """Callback when disconnected after a connection has been made (i.e
+        Crazyflie moves out of range)"""
+        print('Connection to %s lost: %s' % (link_uri, msg))
+
+    def _disconnected(self, link_uri):
+        """Callback when the Crazyflie is disconnected (called in all cases)"""
+        print('Disconnected from %s' % link_uri)
+        self.is_connected = False
+
+    def _applyThrust(self, thrust, duration):
+        start = time.time()
+        self._cf.param.set_value('motorPowerSet.m1', int(thrust))
+        while time.time() - start < duration:
+            self._localization.send_emergency_stop_watchdog()
+            print(time.time() - start)
+            time.sleep(0.1)
+
+    def _ramp_motors(self):
+        self._cf.param.set_value('motor.batCompensation', 0)
+        self._cf.param.set_value('motorPowerSet.m1', 0)
+        self._cf.param.set_value('motorPowerSet.enable', 2)
+        self._cf.param.set_value('system.forceArm', 1)
+
+        while self.is_connected:
+            # base speed
+            self._applyThrust(10000, 3.0)
+
+            # 0 -> 1
+            self._applyThrust(1.0 * 65535, 3.0)
+            # 1 -> 0
+            self._applyThrust(10000, 3.0)
+
+            # 0 -> 0.5
+            self._applyThrust(0.5 * 65535, 3.0)
+            # 0.5 -> 0
+            self._applyThrust(10000, 3.0)
+
+            break
+
+        self._cf.param.set_value('motorPowerSet.enable', 0)
+        time.sleep(1)
+        self._cf.close_link()
+
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--calibration", default="calibration.yaml", help="Input file containing the calibration")
+    parser.add_argument("--uri", default="radio://0/42/2M/E7E7E7E7E7", help="URI of Crazyflie")
+    args = parser.parse_args()
+
+    # Only output errors from the logging framework
+    logging.basicConfig(level=logging.ERROR)
+
+    # Initialize the low-level drivers (don't list the debug drivers)
+    cflib.crtp.init_drivers(enable_debug_driver=False)
+
+    with open(args.calibration, 'r') as f:
+        r = yaml.safe_load(f)
+        a = r['a']
+        b = r['b']
+
+    # collect data
+    le = CollectData(args.uri, a, b)
+
+    # The Crazyflie lib doesn't contain anything to keep the application alive,
+    # so this is where your application should do something. In our case we
+    # are just waiting until we are disconnected.
+    while le.is_connected:
+        time.sleep(1)
diff --git a/tools/system_id/plot_data.py b/tools/system_id/plot_data.py
new file mode 100644
index 0000000000..d106d9c8dc
--- /dev/null
+++ b/tools/system_id/plot_data.py
@@ -0,0 +1,48 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+def loadFile(filename):
+    fileData = np.loadtxt(filename, delimiter=',', skiprows=1, ndmin=2)
+    return fileData
+
+if __name__ == '__main__':
+    data = loadFile("data.csv")
+
+    thrust = data[:,0] / 4  # g, per motor
+    pwm = data[:,1]         # PWM value
+    vbat = data[:,2]        # V, battery voltage, 
+    rpm = np.mean(data[:,3:7],axis=1) # average over all motors
+
+    fig,ax = plt.subplots(2)
+
+    ax[0].plot(data[:,4] - data[:,3], label='M2-M1')
+    ax[0].plot(data[:,5] - data[:,3], label='M3-M1')
+    ax[0].plot(data[:,6] - data[:,3], label='M4-M1')
+    ax[0].set_xlabel('Time')
+    ax[0].set_ylabel('Relative RPM')
+    ax[0].legend()
+
+    ax[1].plot(rpm, thrust)
+    ax[1].set_xlabel('RPM')
+    ax[1].set_ylabel('Thrust [g]')
+
+    z = np.polyfit(rpm, thrust, 2)
+    p = np.poly1d(z)
+
+    xp = np.linspace(np.min(rpm), np.max(rpm), 100)
+
+    ax[1].plot(xp, p(xp), '--')
+    ax[1].set_title("thrust [g] = {} * rpm^2 + {} * rpm + {}".format(z[0], z[1], z[2]))
+
+    plt.show()
+
+    fig, ax = plt.subplots()
+
+    print(data[0, 7]/vbat[0])
+
+    # ax.plot(thrust, vbat, label='pm.vbat')
+    # ax.plot(thrust, data[:, 7]/1.0955852742562724, label='i2c.v_avg')
+    # ax.plot(rpm, data[:, 8], label='i2c.i_avg')
+    ax.plot(thrust, data[:, 7] * data[:, 8], label='i2c.p_avg')
+    ax.legend()
+    plt.show()
diff --git a/tools/system_id/plot_motor_delay.py b/tools/system_id/plot_motor_delay.py
new file mode 100644
index 0000000000..0f997df759
--- /dev/null
+++ b/tools/system_id/plot_motor_delay.py
@@ -0,0 +1,64 @@
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+import numpy as np
+import argparse
+
+parser = argparse.ArgumentParser()
+parser.add_argument("file", help="csv file")
+args = parser.parse_args()
+
+data = np.loadtxt(args.file, delimiter=',', skiprows=1)
+
+# default
+c00 = 11.093358483549203
+c10 = -39.08104165843915
+c01 = -9.525647087583181
+c20 = 20.573302305476638
+c11 = 38.42885066644033
+
+def pwm2force(pwm, v):
+	pwm = pwm / 65536
+	v = v / 4.2
+	return c00 + c10 * pwm + c01 * v + c20 * pwm * pwm + c11 * pwm * v
+
+
+# u dot = lambda (u_des - u)
+l = 16
+
+u_pred = [0]
+dts = np.diff(data[:,0]) / 1e3
+for i, dt in enumerate(dts):
+	u = u_pred[-1]
+	pwm_des = data[i,2]
+	v = data[i,3]
+	# convert pwm -> force
+	f_des = pwm2force(pwm_des, v) * 4
+
+	u_dot = l * (f_des - u)
+	u_pred.append(u + u_dot * dt)
+u_pred = np.array(u_pred)
+
+fig, ax1 = plt.subplots()
+
+ax1.plot((data[:,0] - data[0,0])/1e3, data[:,1], 'b', label="Load cell")
+ax2 = ax1.twinx()
+ax2.plot((data[:,0] - data[0,0])/1e3, data[:,2], 'g', label="PWM")
+
+# ax1.plot((data[:,0] - data[0,0])/1e3, u_pred, 'r', label="Prediction")
+
+# ax1.plot((data[:,0] - data[0,0])/1e3, data[:,1], 'b', label="Load cell")
+
+# # compute f desired from pwm
+# f_des = []
+# for i in range(0, data.shape[0]):
+# 	f_des.append(pwm2force(data[i,2], data[i,3]) * 4)
+# f_des = np.array(f_des)
+
+# ax1.plot((data[:,0] - data[0,0])/1e3, f_des, 'g', label="Desired")
+# ax1.plot((data[:,0] - data[0,0])/1e3, u_pred, 'r', label="Prediction")
+# ax1.plot((data[:, 0] - data[0, 0])/1e3, data[:, 2], 'r', label="PWM")
+
+
+ax1.legend()
+
+plt.show()
diff --git a/tools/system_id/system_id.py b/tools/system_id/system_id.py
new file mode 100644
index 0000000000..a7d7e3247e
--- /dev/null
+++ b/tools/system_id/system_id.py
@@ -0,0 +1,114 @@
+import numpy as np
+from sklearn.linear_model import LinearRegression
+import matplotlib.pyplot as plt
+
+def loadFiles(filenames):
+    if len(filenames) == 0:
+        return None
+
+    Data = np.empty((0,5))
+    for filename in filenames:
+        fileData = np.loadtxt(filename, delimiter=',', skiprows=1)
+        lastValidIdx = np.argwhere( (fileData[:,2] < 3.7) & (fileData[:,1] == 0))
+        if len(lastValidIdx) > 0:
+            lastValidIdx = lastValidIdx[0][0]
+        else:
+            lastValidIdx = -1
+        print(lastValidIdx)
+        fileData = fileData[0:lastValidIdx]
+        print(fileData)
+        Data = np.vstack((Data, fileData))
+
+    filteredData = Data[ (Data[:,1] >= 8000) & (Data[:,1] <= 0.96 * 65536) ]
+
+    X = filteredData[:, 1] / 65536 # PWM
+    Y = filteredData[:, 2] / 4.2 # Vol
+    Z = filteredData[:, 0] / 4 # Thrust (gram)
+    maxThrust = filteredData[:, 3] / 4
+
+    return X, Y, Z, maxThrust
+
+def system_id(filenames, validations=[]):
+    X, Y, Z, maxThrust = loadFiles(filenames)
+    if len(validations) > 0:
+        X_val, Y_val, Z_val, maxThrust_val = loadFiles(validations)
+
+    plt.scatter(X, Z, label="training data")
+    plt.xlabel("PWM")
+    plt.ylabel("Thrust")
+    
+    if len(validations) > 0:
+        plt.scatter(X_val, Z_val, label="validation data")
+        plt.xlabel("PWM")
+        plt.ylabel("Thrust")
+    plt.legend()
+    plt.show()
+    
+    Input = np.zeros((X.shape[0], 4))
+    Input[:, 0] = X
+    Input[:, 1] = Y
+    Input[:, 2] = X ** 2
+    Input[:, 3] = X * Y
+    
+    if len(validations) > 0:
+        Input_val = np.zeros((X_val.shape[0], 4))
+        Input_val[:, 0] = X_val
+        Input_val[:, 1] = Y_val
+        Input_val[:, 2] = X_val ** 2
+        Input_val[:, 3] = X_val * Y_val
+    
+    reg = LinearRegression().fit(Input, Z)
+    print("******************************")
+    print("C_00 = " + str(reg.intercept_))
+    print("C_10 = " + str(reg.coef_[0]))
+    print("C_01 = " + str(reg.coef_[1]))
+    print("C_20 = " + str(reg.coef_[2]))
+    print("C_11 = " + str(reg.coef_[3]))
+    print("score = " + str(reg.score(Input, Z)))
+    if len(validations) > 0:
+        Error = reg.predict(Input_val) - Z_val.reshape(X_val.shape[0])
+        print("validation score = " + str(reg.score(Input_val, Z_val)))
+        print("validation max error = " + str(np.max(np.abs(Error))) + " gram")
+        print("validation mean error = " + str(np.mean(np.abs(Error))) + " gram")
+    
+    Input = np.zeros((X.shape[0], 4))
+    Input[:, 0] = Z
+    Input[:, 1] = Y
+    Input[:, 2] = Z ** 2
+    Input[:, 3] = Z * Y
+    
+    if len(validations) > 0:
+        Input_val = np.zeros((X_val.shape[0], 4))
+        Input_val[:, 0] = Z_val
+        Input_val[:, 1] = Y_val
+        Input_val[:, 2] = Z_val ** 2
+        Input_val[:, 3] = Z_val * Y_val
+    
+    reg = LinearRegression().fit(Input, X)
+    print("******************************")
+    print("d00: " + str(reg.intercept_))
+    print("d10: " + str(reg.coef_[0]))
+    print("d01: " + str(reg.coef_[1]))
+    print("d20: " + str(reg.coef_[2]))
+    print("d11: " + str(reg.coef_[3]))
+    print("score = " + str(reg.score(Input, X)))
+    if len(validations) > 0:
+        Error = reg.predict(Input_val) - X_val.reshape(X_val.shape[0])
+        print("validation score = " + str(reg.score(Input_val, X_val)))
+        print("validation max error = " + str(np.max(np.abs(Error))) + " pwm")
+        print("validation mean error = " + str(np.mean(np.abs(Error))) + " pwm")
+
+    Input = np.zeros((X.shape[0], 2))
+    Input[:, 0] = X
+    Input[:, 1] = Y
+
+    reg = LinearRegression().fit(Input, maxThrust)
+    print("******************************")
+    print("e00: " + str(reg.intercept_))
+    print("e10: " + str(reg.coef_[0]))
+    print("e01: " + str(reg.coef_[1]))
+    print("score = " + str(reg.score(Input, maxThrust)))
+
+
+system_id(["data_max_thrust_brushless01.csv", "data_max_thrust_brushless03.csv"],
+          validations=["data_max_thrust_brushless01.csv"])
diff --git a/tools/usdlog/plot_events.py b/tools/usdlog/plot_events.py
index d707d5d8e3..64e0b5904e 100644
--- a/tools/usdlog/plot_events.py
+++ b/tools/usdlog/plot_events.py
@@ -21,6 +21,8 @@ def showAnnotation(data, sel):
 
     # decode binary log data
     data_usd = cfusdlog.decode(args.file_usd)
+    # print(data_usd)
+    # exit()
 
     # find start time
     start_time = None
@@ -32,9 +34,10 @@ def showAnnotation(data, sel):
 
     # new figure
     fig, ax = plt.subplots(len(data_usd.keys()),1,sharex=True)
+    ax = [ax]
 
     for k, (event_name, data) in enumerate(data_usd.items()):
-        # print(k, event_name)
+        print(k, event_name)
         t = (data['timestamp'] - start_time) / 1000
         ax[k].scatter(t, t*0)
         ax[k].set_title(event_name)