diff --git a/src/deck/drivers/interface/servo.h b/src/deck/drivers/interface/servo.h
new file mode 100644
index 0000000000..bc58675d09
--- /dev/null
+++ b/src/deck/drivers/interface/servo.h
@@ -0,0 +1,71 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 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/>.
+ *
+ * servo.h - servo header file
+ *
+ * The motors PWM ratio is a value on 16 bits (from 0 to 0xFFFF)
+ * the functions of the driver will make the conversions to the actual PWM
+ * precision (ie. if the precision is 8bits 0xFFFF and 0xFF00 are equivalents).
+ *
+ * The precision is set in number of bits by the define MOTORS_PWM_BITS
+ * The timer prescaler is set by MOTORS_PWM_PRESCALE
+ */
+#ifndef __SERVO_H__
+#define __SERVO_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "config.h"
+
+/******** Defines ********/
+#define SERVO_PWM_PERIOD         1000  // ARR register content
+#define SERVO_PWM_FREQUENCY_HZ   50 // target servo pwm frequency
+#define SERVO_PWM_PRESCALE       (uint16_t) (1680) // 84mhz / (50hz * ARR)
+
+/**
+ * Servo Initialization
+ */
+void servoInit();
+
+bool servoTest(void);
+
+/**
+ *
+ * @brief Set servo angle.
+ * @param: angle: desired servo angle in degrees
+ */
+void servoSetAngle(uint8_t angle);
+
+/**
+ * Servo angle parameter callback. When servo angle is changed, call
+ * function to change servo angle automatically
+ *  */
+void servoAngleCallBack(void);
+
+/**
+ * Saturate servo angle. Min is 0, max is defined by parameter 'servoRange'
+ * @param angle: pointer to desired angle
+ * */
+uint8_t saturateAngle(uint8_t angle);
+
+#endif /* __SERVO_H__ */
diff --git a/src/deck/drivers/src/Kbuild b/src/deck/drivers/src/Kbuild
index 79ff5d316b..52765dfab6 100644
--- a/src/deck/drivers/src/Kbuild
+++ b/src/deck/drivers/src/Kbuild
@@ -15,6 +15,7 @@ obj-$(CONFIG_DECK_LOCO)                 += lpsTdoa3Tag.o
 obj-$(CONFIG_DECK_LOCO)                 += lpsTwrTag.o
 obj-$(CONFIG_DECK_MULTIRANGER)          += multiranger.o
 obj-$(CONFIG_DECK_OA)                   += oa.o
+obj-$(CONFIG_DECK_SERVO)                += servo.o
 obj-$(CONFIG_DECK_USD)                  += usddeck.o
 obj-$(CONFIG_DECK_ZRANGER)              += zranger.o
 obj-$(CONFIG_DECK_ZRANGER2)             += zranger2.o
diff --git a/src/deck/drivers/src/Kconfig b/src/deck/drivers/src/Kconfig
index 1db2aa01b4..1c4ee4ff58 100644
--- a/src/deck/drivers/src/Kconfig
+++ b/src/deck/drivers/src/Kconfig
@@ -390,6 +390,37 @@ config DECK_OA
     help
         Codename: Obstacle Avoidance driver, obsolete deck.
 
+config DECK_SERVO
+    bool "Support the servo deck (prototype/via BigQuad deck)"
+    default n
+    help
+      Servo PWM driver.
+
+choice
+    prompt "Servo pin"
+    depends on DECK_SERVO
+    default DECK_SERVO_USE_TX2
+
+    config DECK_SERVO_USE_TX2
+        bool "PA2/TX2"
+    
+    config DECK_SERVO_USE_RX2
+        bool "PA3/RX2"
+
+    config DECK_SERVO_USE_IO1
+        bool "PB8/IO_1"
+
+    config DECK_SERVO_USE_IO2
+        bool "PB5/IO_2"
+
+    config DECK_SERVO_USE_IO3
+        bool "PB4/IO_3"
+
+    config DECK_SERVO_USE_MOSI
+        bool "PA7/MOSI"
+
+endchoice
+
 config DECK_USD
     bool "Support the Micro SD card deck"
     default y
diff --git a/src/deck/drivers/src/servo.c b/src/deck/drivers/src/servo.c
new file mode 100644
index 0000000000..158557c9eb
--- /dev/null
+++ b/src/deck/drivers/src/servo.c
@@ -0,0 +1,259 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2024 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/>.
+ *
+ * servo.c - Servo driver
+ *
+ * Using contributions from: Eric Ewing, Will Wu
+ */
+#define DEBUG_MODULE "SERVO"
+
+#include <stdbool.h>
+
+/* ST includes */
+#include "stm32fxxx.h"
+
+//FreeRTOS includes
+#include "FreeRTOS.h"
+#include "task.h"
+#include "debug.h"
+#include <string.h>
+#include <inttypes.h>
+#include "motors.h"
+#include "param.h"
+#include "deck.h"
+
+static uint16_t servo_MIN_us = 1000;
+static uint16_t servo_MAX_us = 2000;
+
+#include "servo.h"
+
+// #define DEBUG_SERVO
+
+static bool isInit = false;
+
+const MotorPerifDef* servoMap;
+extern const MotorPerifDef* servoMapIO1;
+extern const MotorPerifDef* servoMapIO2;
+extern const MotorPerifDef* servoMapIO3;
+extern const MotorPerifDef* servoMapRX2;
+extern const MotorPerifDef* servoMapTX2;
+extern const MotorPerifDef* servoMapMOSI;
+
+/* Public functions */
+static uint8_t servo_idle = 90;
+static uint8_t s_servo_angle;
+static uint8_t servo_range = 180; // in degrees
+
+void servoMapInit(const MotorPerifDef* servoMapSelect)
+{
+  servoMap = servoMapSelect;
+
+  GPIO_InitTypeDef GPIO_InitStructure;
+  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
+  TIM_OCInitTypeDef  TIM_OCInitStructure;
+
+  //clock the servo pin and the timers
+  RCC_AHB1PeriphClockCmd(servoMap->gpioPerif, ENABLE);
+  RCC_APB1PeriphClockCmd(servoMap->timPerif, ENABLE);
+
+  //configure gpio for timer out
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+  GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+  GPIO_InitStructure.GPIO_Pin = servoMap->gpioPin;
+  GPIO_Init(servoMap->gpioPort, &GPIO_InitStructure);
+
+  //map timer to alternate function
+  GPIO_PinAFConfig(servoMap->gpioPort, servoMap->gpioPinSource, servoMap->gpioAF);
+
+  //Timer configuration
+  TIM_TimeBaseStructure.TIM_Period = SERVO_PWM_PERIOD;
+  TIM_TimeBaseStructure.TIM_Prescaler = SERVO_PWM_PRESCALE;
+  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+  TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
+  TIM_TimeBaseInit(servoMap->tim, &TIM_TimeBaseStructure);
+
+  // PWM channels configuration
+  TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
+  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+  TIM_OCInitStructure.TIM_Pulse = 0;
+  TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
+  TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
+
+  // Configure OC1
+  servoMap->ocInit(servoMap->tim, &TIM_OCInitStructure);
+  servoMap->preloadConfig(servoMap->tim, TIM_OCPreload_Enable);
+
+
+  //Enable the timer PWM outputs
+  TIM_CtrlPWMOutputs(servoMap->tim, ENABLE);
+  servoMap->setCompare(servoMap->tim, 0x00);
+
+  //Enable the timer
+  TIM_Cmd(servoMap->tim, ENABLE);
+}
+
+void servoInit()
+{
+  if (isInit){
+    return;
+  }
+
+  #ifdef CONFIG_DECK_SERVO_USE_IO1
+    servoMapInit(servoMapIO1);
+    DEBUG_PRINT("Init on IO1 [OK]\n");
+  #elif CONFIG_DECK_SERVO_USE_IO2
+    servoMapInit(servoMapIO2);
+    DEBUG_PRINT("Init on IO2 [OK]\n");
+  #elif CONFIG_DECK_SERVO_USE_IO3
+    servoMapInit(servoMapIO3);
+    DEBUG_PRINT("Init on IO3 [OK]\n");
+  #elif CONFIG_DECK_SERVO_USE_RX2
+    servoMapInit(servoMapRX2);
+    DEBUG_PRINT("Init on RX2 [OK]\n"); // not working on Bolt 1.1...
+  #elif CONFIG_DECK_SERVO_USE_MOSI
+    servoMapInit(servoMapMOSI);
+    DEBUG_PRINT("Init on MOSI [OK]\n");
+  #elif CONFIG_DECK_SERVO_USE_TX2
+    servoMapInit(servoMapTX2);
+    DEBUG_PRINT("Init on TX2 [OK]\n"); // not working on Bolt 1.1...
+  #else
+    isInit = false
+    DEBUG_PRINT("Failed to configure servo pin!\n");
+    return;
+  #endif
+  
+  servoSetAngle(saturateAngle(servo_idle));
+
+  s_servo_angle = servo_idle;
+
+  isInit = true;
+}
+
+bool servoTest(void)
+{
+  return isInit;
+}
+
+void servoSetAngle(uint8_t angle)
+{
+  // set CCR register
+  // Duty% = CCR/ARR*100, so CCR = Duty%/100 * ARR
+
+  double pulse_length_us = (double)(angle) / servo_range * (servo_MAX_us - servo_MIN_us) + servo_MIN_us;
+  double pulse_length_s = pulse_length_us / 1000000;
+  const uint32_t ccr_val = (uint32_t)(pulse_length_s * SERVO_PWM_PERIOD * SERVO_PWM_FREQUENCY_HZ);
+  servoMap->setCompare(servoMap->tim, ccr_val);
+  
+  #ifdef DEBUG_SERVO
+    DEBUG_PRINT("Set Angle: %u deg, pulse width: %f us \n", angle, pulse_length_us);
+  #endif
+}
+
+uint8_t saturateAngle(uint8_t angle)
+{
+  if (angle > servo_range) {
+    return servo_range;
+  }
+  else if (angle < 0) {
+    return 0;
+  }
+  else {
+    return angle;
+  }
+
+}
+
+void servoAngleCallBack(void)
+{
+  servoSetAngle(saturateAngle(s_servo_angle));
+}
+
+static const DeckDriver servo_deck = {
+  .vid = 0x00,
+  .pid = 0x00,
+  .name = "bcServo",
+
+  #ifdef CONFIG_DECK_SERVO_USE_IO1
+    .usedPeriph = DECK_USING_TIMER4,
+    .usedGpio = DECK_USING_IO_1,
+  #elif CONFIG_DECK_SERVO_USE_IO2
+    .usedPeriph = DECK_USING_TIMER3,
+    .usedGpio = DECK_USING_IO_2,
+  #elif CONFIG_DECK_SERVO_USE_IO3
+    .usedPeriph = DECK_USING_TIMER3,
+    .usedGpio = DECK_USING_IO_3,
+  #elif CONFIG_DECK_SERVO_USE_RX2
+    .usedPeriph = DECK_USING_TIMER5,
+    .usedGpio = DECK_USING_PA3,
+  #elif CONFIG_DECK_SERVO_USE_MOSI
+    .usedPeriph = DECK_USING_TIMER14,
+    .usedGpio = DECK_USING_PA7,
+  #elif CONFIG_DECK_SERVO_USE_TX2
+    .usedPeriph = DECK_USING_TIMER5,
+    .usedGpio = DECK_USING_PA2,
+  #else
+    .usedPeriph = 0,
+    .usedGpio = 0,
+  #endif
+  
+  .init = servoInit,
+  .test = servoTest,
+};
+
+DECK_DRIVER(servo_deck);
+
+PARAM_GROUP_START(deck)
+
+PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcServo, &isInit)
+PARAM_GROUP_STOP(deck)
+
+/**
+ * "Servo" deck parameters
+ */
+PARAM_GROUP_START(servo)
+
+/**
+ * @brief PWM pulse width for minimal servo position (in microseconds)
+ */
+PARAM_ADD(PARAM_UINT16 | PARAM_PERSISTENT, servoMINus, &servo_MIN_us)
+/**
+ * @brief PWM pulse width for maximal servo position (in microseconds)
+ */
+PARAM_ADD(PARAM_UINT16 | PARAM_PERSISTENT, servoMAXus, &servo_MAX_us)
+/**
+ * @brief Servo range, i.e. angle between the min and max positions (in degrees)
+ */
+PARAM_ADD(PARAM_UINT8 | PARAM_PERSISTENT, servoRange, &servo_range)
+/**
+ * @brief Servo idle (startup) angular position (in degrees, min = 0, max = servoRange)
+ */
+PARAM_ADD(PARAM_UINT8 | PARAM_PERSISTENT, servoIdle, &servo_idle)
+/**
+ * @brief Servo angular position (in degrees, min = 0, max = servoRange)
+ */
+PARAM_ADD_WITH_CALLBACK(PARAM_UINT8 , servoAngle, &s_servo_angle, &servoAngleCallBack)
+
+PARAM_GROUP_STOP(servo)
diff --git a/src/deck/interface/deck_core.h b/src/deck/interface/deck_core.h
index c13deaccd5..47dc23d867 100644
--- a/src/deck/interface/deck_core.h
+++ b/src/deck/interface/deck_core.h
@@ -69,6 +69,7 @@ bool deckTest(void);
 #define DECK_USING_TIMER10 (1 << 16)
 #define DECK_USING_TIMER14 (1 << 15)
 #define DECK_USING_TIMER9  (1 << 17)
+#define DECK_USING_TIMER4  (1 << 18)
 
 struct deckInfo_s;
 struct deckFwUpdate_s;
diff --git a/src/drivers/src/motors_def.c b/src/drivers/src/motors_def.c
index 0e334efdcd..d9ef9658c1 100644
--- a/src/drivers/src/motors_def.c
+++ b/src/drivers/src/motors_def.c
@@ -623,7 +623,7 @@ static const MotorPerifDef MOTORS_PB5_TIM3_CH2_BRUSHLESS_OD =
 };
 
 // Deck IO3, PB4, TIM3_CH1
-static const MotorPerifDef MOTORS_PB4_TIM2_CH1_BRUSHLESS_OD =
+static const MotorPerifDef MOTORS_PB4_TIM3_CH1_BRUSHLESS_OD =
 {
     .drvType       = BRUSHLESS,
     .gpioPerif     = RCC_AHB1Periph_GPIOB,
@@ -743,7 +743,7 @@ const MotorPerifDef* motorMapDefaultBrushed[NBR_OF_MOTORS] =
 const MotorPerifDef* motorMapBigQuadDeck[NBR_OF_MOTORS] =
 {
   &MOTORS_PA2_TIM2_CH3_BRUSHLESS_OD,
-  &MOTORS_PB4_TIM2_CH1_BRUSHLESS_OD,
+  &MOTORS_PB4_TIM3_CH1_BRUSHLESS_OD,
   &MOTORS_PB5_TIM3_CH2_BRUSHLESS_OD,
   &MOTORS_PA3_TIM2_CH4_BRUSHLESS_OD
 };
@@ -805,3 +805,32 @@ const MotorPerifDef* motorMapCF21Brushless[NBR_OF_MOTORS] =
     &MOTORS_PB10_TIM2_CH3_BRUSHLESS_OD
 };
 
+/**
+ * Servo mapped to the Bigquad CPPM (MOSI) port
+ */
+const MotorPerifDef* servoMapMOSI = &MOTORS_PA7_TIM14_CH1_BRUSHLESS_OD;
+
+/**
+ * Servo mapped to the Bigquad M1 / TX2 port
+ */
+const MotorPerifDef* servoMapTX2 = &MOTORS_PA2_TIM5_CH3_BRUSHLESS_OD;
+
+/**
+ * Servo mapped to the Bigquad M3 / IO2 port
+ */
+const MotorPerifDef* servoMapIO2 = &MOTORS_PB5_TIM3_CH2_BRUSHLESS_OD;
+
+/**
+ * Servo mapped to the Bigquad M2 / IO3 port
+ */
+const MotorPerifDef* servoMapIO3 = &MOTORS_PB4_TIM3_CH1_BRUSHLESS_OD;
+
+/**
+ * Servo mapped to the Bigquad M4 / RX2 port
+ */
+const MotorPerifDef* servoMapRX2 = &MOTORS_PA3_TIM5_CH4_BRUSHLESS_OD;
+
+/**
+ * Servo mapped to IO1 port
+ */
+const MotorPerifDef* servoMapIO1 = &MOTORS_PB8_TIM4_CH3_BRUSHLESS_OD;
diff --git a/src/modules/interface/supervisor.h b/src/modules/interface/supervisor.h
index 48de77d8ed..46e2be9b4c 100644
--- a/src/modules/interface/supervisor.h
+++ b/src/modules/interface/supervisor.h
@@ -110,9 +110,26 @@ bool supervisorCanArm();
 bool supervisorIsArmed();
 
 /**
- * @brief Query if the system is locked (due to a crash)
+ * @brief Query if the system is locked (due to an emergency stop or watchdog timeout)
  *
  * @return true
  * @return false
  */
 bool supervisorIsLocked();
+
+/**
+ * @brief Query if the system is crashed
+ *
+ * @return true
+ * @return false
+ */
+bool supervisorIsCrashed();
+
+/**
+ * @brief Request the system to be recover if crashed.
+ *
+ * @param doRecover true - request recovery. false - request crashed.
+ * @return true   The request was granted
+ * @return false  The request could not be granted
+ */
+bool supervisorRequestCrashRecovery(const bool doRecover);
\ No newline at end of file
diff --git a/src/modules/interface/supervisor_state_machine.h b/src/modules/interface/supervisor_state_machine.h
index 4c62a9d590..164284d572 100644
--- a/src/modules/interface/supervisor_state_machine.h
+++ b/src/modules/interface/supervisor_state_machine.h
@@ -37,6 +37,7 @@ typedef enum {
     supervisorStateWarningLevelOut,
     supervisorStateExceptFreeFall,
     supervisorStateLocked,
+    supervisorStateCrashed,
     supervisorState_NrOfStates,
 } supervisorState_t;
 
@@ -48,6 +49,8 @@ typedef enum {
   supervisorConditionCommanderWdtWarning,
   supervisorConditionCommanderWdtTimeout,
   supervisorConditionEmergencyStop,
+  supervisorConditionIsCrashed,
+  supervisorConditionLandingTimeout,
   supervisorCondition_NrOfConditions,
 } supervisorConditions_t;
 
@@ -61,6 +64,8 @@ typedef uint32_t supervisorConditionBits_t;
 #define SUPERVISOR_CB_COMMANDER_WDT_WARNING (1 << supervisorConditionCommanderWdtWarning)
 #define SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT (1 << supervisorConditionCommanderWdtTimeout)
 #define SUPERVISOR_CB_EMERGENCY_STOP (1 << supervisorConditionEmergencyStop)
+#define SUPERVISOR_CB_CRASHED (1 << supervisorConditionIsCrashed)
+#define SUPERVISOR_CB_LANDING_TIMEOUT (1 << supervisorConditionLandingTimeout)
 
 
 // Enum that is used to describe how to combine the bits in the required field
diff --git a/src/modules/src/controller/controller_brescianini.c b/src/modules/src/controller/controller_brescianini.c
index d17d97ab6b..905f9ba300 100644
--- a/src/modules/src/controller/controller_brescianini.c
+++ b/src/modules/src/controller/controller_brescianini.c
@@ -38,6 +38,7 @@
 #include "num.h"
 #include "math3d.h"
 #include "physicalConstants.h"
+#include "platform_defaults.h"
 
 static struct mat33 CRAZYFLIE_INERTIA =
     {{{16.6e-6f, 0.83e-6f, 0.72e-6f},
diff --git a/src/modules/src/controller/controller_mellinger.c b/src/modules/src/controller/controller_mellinger.c
index f15f5e41c0..40bc2c285f 100644
--- a/src/modules/src/controller/controller_mellinger.c
+++ b/src/modules/src/controller/controller_mellinger.c
@@ -42,6 +42,7 @@ We added the following:
 #include "position_controller.h"
 #include "controller_mellinger.h"
 #include "physicalConstants.h"
+#include "platform_defaults.h"
 
 // Global state variable used in the
 // firmware as the only instance and in bindings
diff --git a/src/modules/src/platformservice.c b/src/modules/src/platformservice.c
index 28a3846c86..9a9972a852 100644
--- a/src/modules/src/platformservice.c
+++ b/src/modules/src/platformservice.c
@@ -54,6 +54,7 @@ typedef enum {
 typedef enum {
   setContinuousWave  = 0x00,
   armSystem          = 0x01,
+  recoverSystem     = 0x02, 
 } PlatformCommand;
 
 typedef enum {
@@ -135,6 +136,14 @@ static void platformCommandProcess(CRTPPacket *p)
       p->size = 2;
       break;
     }
+    case recoverSystem:
+    {
+      const bool success = supervisorRequestCrashRecovery(true);
+      data[0] = success;
+      data[1] = !supervisorIsCrashed();
+      p->size = 2;
+      break;
+    }    
     default:
       break;
   }
diff --git a/src/modules/src/power_distribution_quadrotor.c b/src/modules/src/power_distribution_quadrotor.c
index c37b7448ed..a58e1e4155 100644
--- a/src/modules/src/power_distribution_quadrotor.c
+++ b/src/modules/src/power_distribution_quadrotor.c
@@ -33,6 +33,7 @@
 #include "autoconf.h"
 #include "config.h"
 #include "math.h"
+#include "platform_defaults.h"
 
 #ifndef CONFIG_MOTORS_DEFAULT_IDLE_THRUST
 #  define DEFAULT_IDLE_THRUST 0
@@ -41,7 +42,7 @@
 #endif
 
 static uint32_t idleThrust = DEFAULT_IDLE_THRUST;
-static float armLength = 0.046f; // m;
+static float armLength = ARM_LENGTH; // m
 static float thrustToTorque = 0.005964552f;
 
 // thrust = a * pwm^2 + b * pwm
diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c
index 2e209829ab..66649ca353 100644
--- a/src/modules/src/supervisor.c
+++ b/src/modules/src/supervisor.c
@@ -61,11 +61,14 @@
   #define AUTO_ARMING 0
 #endif
 
+static uint16_t landingTimeoutDuration = LANDING_TIMEOUT_MS;
+
 typedef struct {
   bool canFly;
   bool isFlying;
   bool isTumbled;
   bool isArmingActivated;
+  bool isCrashed;
   uint16_t infoBitfield;
   uint8_t paramEmergencyStop;
 
@@ -78,6 +81,9 @@ typedef struct {
   // The time (in ticks) of the latest high thrust event. 0=no high thrust event yet
   uint32_t latestThrustTick;
 
+  // The time (in ticks) of the latest landing event. 0=no landing event yet
+  uint32_t latestLandingTick;
+
   supervisorState_t state;
 
   // Copy of latest conditions, for logging
@@ -115,6 +121,38 @@ bool supervisorIsLocked() {
   return supervisorStateLocked == supervisorMem.state;
 }
 
+bool supervisorIsCrashed() {
+  return supervisorMem.isCrashed;
+}
+
+static void supervisorSetLatestLandingTime(SupervisorMem_t* this, const uint32_t currentTick) {
+  this->latestLandingTick = currentTick;
+}
+
+bool supervisorIsLandingTimeout(SupervisorMem_t* this, const uint32_t currentTick) {
+  if (0 == this->latestLandingTick) {
+    return false;
+  }
+
+  const uint32_t landingTime = currentTick - this->latestLandingTick;
+  return landingTime > M2T(landingTimeoutDuration);
+}
+
+bool supervisorRequestCrashRecovery(const bool doRecovery) {
+
+  if (doRecovery && !supervisorIsCrashed()) {
+    return true;
+  } else if (doRecovery && supervisorIsCrashed() && !supervisorIsTumbled()) {
+    supervisorMem.isCrashed = false;
+    return true;
+  } else if (!doRecovery) {
+    supervisorMem.isCrashed = true;
+    return true;
+  }
+
+  return false;
+}
+
 bool supervisorRequestArming(const bool doArm) {
   if (doArm == supervisorMem.isArmingActivated) {
     return true;
@@ -215,25 +253,39 @@ static bool checkEmergencyStopWatchdog(const uint32_t tick) {
   return isOk;
 }
 
-static void postTransitionActions(SupervisorMem_t* this, const supervisorState_t previousState) {
+static void postTransitionActions(SupervisorMem_t* this, const supervisorState_t previousState, const uint32_t currentTick) {
   const supervisorState_t newState = this->state;
 
   if (newState == supervisorStateReadyToFly) {
     DEBUG_PRINT("Ready to fly\n");
   }
 
+  if (newState == supervisorStateLanded) {
+    supervisorSetLatestLandingTime(this, currentTick);
+  }
+  
+  if ((previousState == supervisorStateLanded) && (newState == supervisorStateReset)) {
+    DEBUG_PRINT("Landing timeout, disarming\n");
+  }
+
   if (newState == supervisorStateLocked) {
     DEBUG_PRINT("Locked, reboot required\n");
   }
 
+  if (newState == supervisorStateCrashed) {
+    DEBUG_PRINT("Crashed, recovery required\n");
+    supervisorRequestCrashRecovery(false);
+  }
+
   if ((previousState == supervisorStateNotInitialized || previousState == supervisorStateReadyToFly || previousState == supervisorStateFlying) &&
-      newState != supervisorStateReadyToFly && newState != supervisorStateFlying) {
+      newState != supervisorStateReadyToFly && newState != supervisorStateFlying && newState != supervisorStateLanded) {
     DEBUG_PRINT("Can not fly\n");
   }
 
   if (newState != supervisorStateReadyToFly &&
       newState != supervisorStateFlying &&
-      newState != supervisorStateWarningLevelOut) {
+      newState != supervisorStateWarningLevelOut &&
+      newState != supervisorStateLanded) {
     supervisorRequestArming(false);
   }
 
@@ -282,6 +334,14 @@ static supervisorConditionBits_t updateAndPopulateConditions(SupervisorMem_t* th
     conditions |= SUPERVISOR_CB_EMERGENCY_STOP;
   }
 
+  if (supervisorIsCrashed()) {
+    conditions |= SUPERVISOR_CB_CRASHED;
+  }
+
+  if (supervisorIsLandingTimeout(this, currentTick)) {
+    conditions |= SUPERVISOR_CB_LANDING_TIMEOUT;
+  }
+
   return conditions;
 }
 
@@ -312,6 +372,9 @@ static void updateLogData(SupervisorMem_t* this, const supervisorConditionBits_t
   if (supervisorStateLocked == this->state) {
     this->infoBitfield |= 0x0040;
   }
+  if (this->isCrashed) {
+    this->infoBitfield |= 0x0080;
+  }
 }
 
 void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint, stabilizerStep_t stabilizerStep) {
@@ -327,7 +390,7 @@ void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint, s
   if (this->state != newState) {
     const supervisorState_t previousState = this->state;
     this->state = newState;
-    postTransitionActions(this, previousState);
+    postTransitionActions(this, previousState, currentTick);
   }
 
   this->latestConditions = conditions;
@@ -343,6 +406,8 @@ void supervisorOverrideSetpoint(setpoint_t* setpoint) {
   switch(this->state){
     case supervisorStateReadyToFly:
       // Fall through
+    case supervisorStateLanded:
+      // Fall through
     case supervisorStateFlying:
       // Do nothing
       break;
@@ -370,7 +435,8 @@ bool supervisorAreMotorsAllowedToRun() {
   SupervisorMem_t* this = &supervisorMem;
   return (this->state == supervisorStateReadyToFly) ||
          (this->state == supervisorStateFlying) ||
-         (this->state == supervisorStateWarningLevelOut);
+         (this->state == supervisorStateWarningLevelOut) ||
+         (this->state == supervisorStateLanded);
 }
 
 void infoDump(const SupervisorMem_t* this) {
@@ -462,4 +528,10 @@ PARAM_GROUP_START(supervisor)
  * @brief Set to nonzero to dump information about the current supervisor state to the console log
  */
 PARAM_ADD(PARAM_UINT8, infdmp, &supervisorMem.doinfodump)
+
+/**
+ * @brief Landing timeout duration (ms)
+ */
+PARAM_ADD(PARAM_UINT16 | PARAM_PERSISTENT, landedTimeout, &landingTimeoutDuration)
+
 PARAM_GROUP_STOP(supervisor)
diff --git a/src/modules/src/supervisor_state_machine.c b/src/modules/src/supervisor_state_machine.c
index b3cd69ba75..2ccb2c7e65 100644
--- a/src/modules/src/supervisor_state_machine.c
+++ b/src/modules/src/supervisor_state_machine.c
@@ -47,6 +47,7 @@ static const char* const stateNames[] = {
   "Warning, level out",
   "Exception, free fall",
   "Locked",
+  "Crashed",
 };
 static_assert(sizeof(stateNames) / sizeof(stateNames[0]) == supervisorState_NrOfStates);
 
@@ -57,6 +58,8 @@ static const char* const conditionNames[] = {
   "commanderWdtWarning",
   "commanderWdtTimeout",
   "emergencyStop",
+  "isCrashed",
+  "landingTimeout",
 };
 static_assert(sizeof(conditionNames) / sizeof(conditionNames[0]) == supervisorCondition_NrOfConditions);
 
@@ -162,7 +165,16 @@ static SupervisorStateTransition_t transitionsFlying[] = {
   {
     .newState = supervisorStateExceptFreeFall,
 
-    .triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_CONF_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP,
+    .triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_EMERGENCY_STOP,
+    .negatedTriggers = SUPERVISOR_CB_ARMED,
+    .triggerCombiner = supervisorAny,
+
+    .blockerCombiner = supervisorNever,
+  },
+  {
+    .newState = supervisorStateCrashed,
+
+    .triggers = SUPERVISOR_CB_CONF_IS_TUMBLED,
     .negatedTriggers = SUPERVISOR_CB_ARMED,
     .triggerCombiner = supervisorAny,
 
@@ -192,10 +204,21 @@ static SupervisorStateTransition_t transitionsLanded[] = {
   {
     .newState = supervisorStateReset,
 
-    .triggerCombiner = supervisorAlways,
+    .triggers = SUPERVISOR_CB_LANDING_TIMEOUT,
+    .negatedTriggers = SUPERVISOR_CB_NONE,
+    .triggerCombiner = supervisorAll,
 
     .blockerCombiner = supervisorNever,
   },
+  {
+    .newState = supervisorStateFlying,
+
+    .triggers = SUPERVISOR_CB_IS_FLYING,
+    .negatedTriggers = SUPERVISOR_CB_NONE,
+    .triggerCombiner = supervisorAll,
+
+    .blockerCombiner = supervisorNever,
+  }
 };
 
 static SupervisorStateTransition_t transitionsReset[] = {
@@ -249,6 +272,28 @@ static SupervisorStateTransition_t transitionsLocked[] = {
   },
 };
 
+
+static SupervisorStateTransition_t transitionsTumbled[] = {
+  {
+    .newState = supervisorStatePreFlChecksNotPassed,
+
+    .triggers = SUPERVISOR_CB_NONE,
+    .negatedTriggers = SUPERVISOR_CB_CRASHED | SUPERVISOR_CB_IS_TUMBLED,
+    .triggerCombiner = supervisorAll,
+
+    .blockerCombiner = supervisorNever
+  },
+  {
+    .newState = supervisorStateLocked,
+
+    .triggers = SUPERVISOR_CB_EMERGENCY_STOP,
+    .negatedTriggers = SUPERVISOR_CB_NONE,
+    .triggerCombiner = supervisorAll,
+
+    .blockerCombiner = supervisorNever
+  },
+};
+
 SupervisorStateTransitionList_t transitionLists[] = {
   {SUPERVISOR_TRANSITION_ENTRY(transitionsNotInitialized)},
   {SUPERVISOR_TRANSITION_ENTRY(transitionsPreFlChecksNotPassed)},
@@ -260,6 +305,7 @@ SupervisorStateTransitionList_t transitionLists[] = {
   {SUPERVISOR_TRANSITION_ENTRY(transitionsWarningLevelOut)},
   {SUPERVISOR_TRANSITION_ENTRY(transitionsExceptFreeFall)},
   {SUPERVISOR_TRANSITION_ENTRY(transitionsLocked)},
+  {SUPERVISOR_TRANSITION_ENTRY(transitionsTumbled)},
 };
 static_assert(sizeof(transitionLists) / sizeof(transitionLists[0]) == supervisorState_NrOfStates);
 
diff --git a/src/platform/interface/platform_defaults.h b/src/platform/interface/platform_defaults.h
index af8d5d0f86..e85abfb7de 100644
--- a/src/platform/interface/platform_defaults.h
+++ b/src/platform/interface/platform_defaults.h
@@ -44,6 +44,17 @@
     #include "platform_defaults_flapper.h"
 #endif
 
+// Drone physical constants
+// TODO provide those in each config. Those are values of cf2 platform
+#ifndef ARM_LENGTH
+    // m
+    #define ARM_LENGTH  0.046f
+#endif
+#ifndef CF_MASS
+    // kg
+    #define CF_MASS     0.027f
+#endif
+
 // IMU alignment on the airframe 
 #ifndef IMU_PHI
     #define IMU_PHI     0.0f
@@ -114,6 +125,11 @@
     #define SUPERVISOR_TUMBLE_CHECK_ENABLE true
 #endif
 
+// Landing timeout before disarming
+#ifndef LANDING_TIMEOUT_MS
+    #define LANDING_TIMEOUT_MS 3000
+#endif
+
 
 // Health test parameters
 #ifndef HEALTH_BRUSHED_ON_PERIOD_MSEC
diff --git a/src/platform/interface/platform_defaults_cf2.h b/src/platform/interface/platform_defaults_cf2.h
index 147ac41aa4..9c906585f2 100644
--- a/src/platform/interface/platform_defaults_cf2.h
+++ b/src/platform/interface/platform_defaults_cf2.h
@@ -42,6 +42,12 @@
 // Requires kbuild config ENABLE_AUTO_SHUTDOWN to be activated.
 #define DEFAULT_SYSTEM_SHUTDOWN_TIMEOUT_MIN       5
 
+// Drone physical constants
+// m
+#define ARM_LENGTH  0.046f
+// kg
+#define CF_MASS     0.027f
+
 // Default PID gains
 #define PID_ROLL_RATE_KP  250.0
 #define PID_ROLL_RATE_KI  500.0
diff --git a/src/utils/interface/physicalConstants.h b/src/utils/interface/physicalConstants.h
index 7b94316ce4..d107c25b3e 100644
--- a/src/utils/interface/physicalConstants.h
+++ b/src/utils/interface/physicalConstants.h
@@ -47,7 +47,3 @@
 #ifndef ARCMINUTE
   #define ARCMINUTE (M_PI_F / 10800.0f)
 #endif
-
-#ifndef CF_MASS
-  #define CF_MASS (0.027f) // in kg
-#endif
diff --git a/src/utils/interface/pid.h b/src/utils/interface/pid.h
index 50248f0aec..cbdfb828d4 100644
--- a/src/utils/interface/pid.h
+++ b/src/utils/interface/pid.h
@@ -38,7 +38,7 @@ typedef struct
 {
   float desired;      //< set point
   float error;        //< error
-  float prevError;    //< previous error
+  float prevMeasured; //< previous measurement
   float integ;        //< integral
   float deriv;        //< derivative
   float kp;           //< proportional gain
diff --git a/src/utils/src/pid.c b/src/utils/src/pid.c
index 67b00ba9a0..7152e9fad8 100644
--- a/src/utils/src/pid.c
+++ b/src/utils/src/pid.c
@@ -36,7 +36,7 @@ void pidInit(PidObject* pid, const float desired, const float kp,
              bool enableDFilter)
 {
   pid->error         = 0;
-  pid->prevError     = 0;
+  pid->prevMeasured  = 0;
   pid->integ         = 0;
   pid->deriv         = 0;
   pid->desired       = desired;
@@ -56,86 +56,82 @@ void pidInit(PidObject* pid, const float desired, const float kp,
 
 float pidUpdate(PidObject* pid, const float measured, const bool updateError)
 {
-    float output = 0.0f;
+  float output = 0.0f;
 
-    if (updateError)
-    {
-        pid->error = pid->desired - measured;
-    }
-
-    pid->outP = pid->kp * pid->error;
-    output += pid->outP;
+  if (updateError)
+  {
+      pid->error = pid->desired - measured;
+  }
 
-    float deriv = (pid->error - pid->prevError) / pid->dt;
-    
-    #if CONFIG_CONTROLLER_PID_FILTER_ALL
+  pid->outP = pid->kp * pid->error;
+  output += pid->outP;
+
+  float deriv = -(measured - pid->prevMeasured) / pid->dt;
+  
+  #if CONFIG_CONTROLLER_PID_FILTER_ALL
+    pid->deriv = deriv;
+  #else
+    if (pid->enableDFilter){
+      pid->deriv = lpf2pApply(&pid->dFilter, deriv);
+    } else {
       pid->deriv = deriv;
-    #else
-      if (pid->enableDFilter){
-        pid->deriv = lpf2pApply(&pid->dFilter, deriv);
-      } else {
-        pid->deriv = deriv;
-      }
-    #endif
-    if (isnan(pid->deriv)) {
-      pid->deriv = 0;
     }
-    pid->outD = pid->kd * pid->deriv;
-    output += pid->outD;
+  #endif
+  if (isnan(pid->deriv)) {
+    pid->deriv = 0;
+  }
+  pid->outD = pid->kd * pid->deriv;
+  output += pid->outD;
 
-    pid->integ += pid->error * pid->dt;
+  pid->integ += pid->error * pid->dt;
 
-    // Constrain the integral (unless the iLimit is zero)
-    if(pid->iLimit != 0)
-    {
-    	pid->integ = constrain(pid->integ, -pid->iLimit, pid->iLimit);
-    }
+  // Constrain the integral (unless the iLimit is zero)
+  if(pid->iLimit != 0)
+  {
+    pid->integ = constrain(pid->integ, -pid->iLimit, pid->iLimit);
+  }
+
+  pid->outI = pid->ki * pid->integ;
+  output += pid->outI;
 
-    pid->outI = pid->ki * pid->integ;
-    output += pid->outI;
-
-    pid->outFF = pid->kff * pid->desired;
-    output += pid->outFF;
-    
-    #if CONFIG_CONTROLLER_PID_FILTER_ALL
-      //filter complete output instead of only D component to compensate for increased noise from increased barometer influence
-      if (pid->enableDFilter)
-      {
-        output = lpf2pApply(&pid->dFilter, output);
-      }
-      else {
-        output = output;
-      }
-      if (isnan(output)) {
-        output = 0;
-      }
-     #endif
-      
-    
-
-    // Constrain the total PID output (unless the outputLimit is zero)
-    if(pid->outputLimit != 0)
+  pid->outFF = pid->kff * pid->desired;
+  output += pid->outFF;
+  
+  #if CONFIG_CONTROLLER_PID_FILTER_ALL
+    //filter complete output instead of only D component to compensate for increased noise from increased barometer influence
+    if (pid->enableDFilter)
     {
-      output = constrain(output, -pid->outputLimit, pid->outputLimit);
+      output = lpf2pApply(&pid->dFilter, output);
+    }
+    else {
+      output = output;
+    }
+    if (isnan(output)) {
+      output = 0;
     }
+    #endif
 
+  // Constrain the total PID output (unless the outputLimit is zero)
+  if(pid->outputLimit != 0)
+  {
+    output = constrain(output, -pid->outputLimit, pid->outputLimit);
+  }
 
-    pid->prevError = pid->error;
+  pid->prevMeasured = measured;
 
-    return output;
+  return output;
 }
 
 void pidSetIntegralLimit(PidObject* pid, const float limit) {
     pid->iLimit = limit;
 }
 
-
 void pidReset(PidObject* pid)
 {
-  pid->error     = 0;
-  pid->prevError = 0;
-  pid->integ     = 0;
-  pid->deriv     = 0;
+  pid->error        = 0;
+  pid->prevMeasured = 0;
+  pid->integ        = 0;
+  pid->deriv        = 0;
 }
 
 void pidSetError(PidObject* pid, const float error)