From 9afa86249699e113fedbc27420ff7bc4c4aacd7c Mon Sep 17 00:00:00 2001
From: Will Wu <williamwu8-8@outlook.com>
Date: Mon, 11 Sep 2023 17:23:40 -0400
Subject: [PATCH 01/17] Remap Servo to CPPM port

---
 src/deck/drivers/src/bigquad.c |   4 +-
 src/drivers/interface/servo.h  |  57 ++++++++++++++
 src/drivers/src/Kbuild         |   1 +
 src/drivers/src/motors_def.c   |   4 +
 src/drivers/src/servo.c        | 135 +++++++++++++++++++++++++++++++++
 5 files changed, 200 insertions(+), 1 deletion(-)
 create mode 100644 src/drivers/interface/servo.h
 create mode 100644 src/drivers/src/servo.c

diff --git a/src/deck/drivers/src/bigquad.c b/src/deck/drivers/src/bigquad.c
index 00bcd20f33..d9187683bb 100644
--- a/src/deck/drivers/src/bigquad.c
+++ b/src/deck/drivers/src/bigquad.c
@@ -42,6 +42,7 @@
 
 #include "FreeRTOS.h"
 #include "task.h"
+#include "servo.h"
 
 #define BIGQUAD_BAT_VOLT_PIN       DECK_GPIO_MISO
 #define BIGQUAD_BAT_VOLT_MULT      (CONFIG_DECK_BIGQUAD_BAT_VOLT_MULT_MV / 1000.0)
@@ -99,6 +100,7 @@ static void bigquadInit(DeckInfo *info)
               BQ_OSD_TASK_STACKSIZE, NULL, BQ_OSD_TASK_PRI, NULL);
 #endif
 
+  servoInit();
   isInit = true;
 }
 
@@ -121,7 +123,7 @@ static const DeckDriver bigquad_deck = {
 
   .usedPeriph = DECK_USING_TIMER3 | DECK_USING_TIMER14,
   .usedGpio = DECK_USING_PA2 | DECK_USING_PA3 | DECK_USING_IO_3 |
-              DECK_USING_IO_2 | DECK_USING_PA7,
+              DECK_USING_IO_2 | DECK_USING_PA7 | DECK_USING_IO_4,
   .init = bigquadInit,
   .test = bigquadTest,
 };
diff --git a/src/drivers/interface/servo.h b/src/drivers/interface/servo.h
new file mode 100644
index 0000000000..4d220a9c94
--- /dev/null
+++ b/src/drivers/interface/servo.h
@@ -0,0 +1,57 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 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 ********/
+
+/*** Public interface ***/
+
+/**
+ * Servo Initialization. Configures two output compare channels in PWM mode
+ * with one of them inverted to increase power output.
+ */
+void servoInit();
+
+bool servoTest(void);
+
+/**
+ * Set servo ratio/power.
+ */
+void servoSetAngle(uint8_t ratio);
+
+#endif /* __MOTORS_H__ */
diff --git a/src/drivers/src/Kbuild b/src/drivers/src/Kbuild
index 18bc093636..b496872bfb 100644
--- a/src/drivers/src/Kbuild
+++ b/src/drivers/src/Kbuild
@@ -17,6 +17,7 @@ obj-y += nvic.o
 obj-y += pca9685.o
 obj-y += piezo.o
 obj-y += pmw3901.o
+obj-y += servo.o
 obj-y += swd.o
 obj-y += uart1.o
 obj-y += uart2.o
diff --git a/src/drivers/src/motors_def.c b/src/drivers/src/motors_def.c
index 143a44b39f..430c8e9784 100644
--- a/src/drivers/src/motors_def.c
+++ b/src/drivers/src/motors_def.c
@@ -771,3 +771,7 @@ 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;
diff --git a/src/drivers/src/servo.c b/src/drivers/src/servo.c
new file mode 100644
index 0000000000..79f3e8b1bb
--- /dev/null
+++ b/src/drivers/src/servo.c
@@ -0,0 +1,135 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 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.c - Servo driver
+ *
+ * This code mainly interfacing the PWM peripheral lib of ST.
+ * Author: Eric Ewing
+ */
+
+#include <stdbool.h>
+
+/* ST includes */
+#include "stm32fxxx.h"
+#include "param.h"
+
+
+//FreeRTOS includes
+#include "FreeRTOS.h"
+#include "task.h"
+#include "debug.h"
+#include <string.h>
+#include <inttypes.h>
+#include <motors.h>
+
+
+// TODO Verify PWM settings
+#define SERVO_PWM_BITS      (0)
+#define SERVO_PWM_PERIOD    1000  // 84mhz -> 50hz with 100 as a prescaler
+#define SERVO_PWM_PRESCALE  (uint16_t) (1680 * 2) //84mhz to khz
+#define SERVO_BASE_FREQ     (0) // should be calculated
+
+#include "servo.h"
+
+static bool isInit = false;
+// we use the "servoMapMOSI" struct to initialize PWM
+extern const MotorPerifDef* servoMapMOSI;
+
+/* Public functions */
+
+void servoInit()
+{
+  if (isInit){
+    DEBUG_PRINT("Already Initialized!");
+    return;
+  }
+  GPIO_InitTypeDef GPIO_InitStructure;
+  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
+  TIM_OCInitTypeDef  TIM_OCInitStructure;
+
+  //clock the servo pin and the timers
+  RCC_AHB1PeriphClockCmd(servoMapMOSI->gpioPerif, ENABLE);
+  RCC_APB1PeriphClockCmd(servoMapMOSI->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_DOWN;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; // TODO: verify this
+  GPIO_InitStructure.GPIO_Pin = servoMapMOSI->gpioPin;
+  GPIO_Init(servoMapMOSI->gpioPort, &GPIO_InitStructure);
+
+  //map timer to alternate function
+  GPIO_PinAFConfig(servoMapMOSI->gpioPort, servoMapMOSI->gpioPin, servoMapMOSI->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(servoMapMOSI->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_Low;
+  TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
+
+  // Configure OC3
+  servoMapMOSI->ocInit(servoMapMOSI->tim, &TIM_OCInitStructure);
+  servoMapMOSI->preloadConfig(servoMapMOSI->tim, TIM_OCPreload_Enable);
+
+
+  //Enable the timer PWM outputs
+  TIM_CtrlPWMOutputs(servoMapMOSI->tim, ENABLE);
+  servoMapMOSI->setCompare(servoMapMOSI->tim, 0x00);
+
+  //Enable the timer
+  TIM_Cmd(servoMapMOSI->tim, ENABLE);
+
+  servoSetAngle(90);
+  isInit = true;
+}
+
+bool servoTest(void)
+{
+  return isInit;
+}
+
+void servoSetAngle(uint8_t angle)
+{
+  // Convert angle to between .025 and .125 and then to ratio of signal
+  double angle_d = (double) angle;
+  uint32_t ratio = ((angle_d / 180.)* SERVO_PWM_PERIOD) / 10. + 100;
+  servoMapMOSI->setCompare(servoMapMOSI->tim, ratio);
+  // TIM_SetCompare4(SERVO_TIM, ratio);
+  DEBUG_PRINT("Set Angle: %u, set ratio: %" PRId32 "\n", angle, ratio);
+}
+
+PARAM_GROUP_START(servo_controller)
+
+// PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, servoInit, &isInit)
+
+PARAM_GROUP_STOP(servo_controller)

From 2672d1e441a4ec0618f8ef2c54cb167e52b8c39b Mon Sep 17 00:00:00 2001
From: Will Wu <williamwu8-8@outlook.com>
Date: Fri, 15 Sep 2023 17:32:22 -0400
Subject: [PATCH 02/17] Adjust servo prescaler for 50Hz PWM

---
 src/drivers/src/servo.c | 32 +++++++++++++++++++-------------
 1 file changed, 19 insertions(+), 13 deletions(-)

diff --git a/src/drivers/src/servo.c b/src/drivers/src/servo.c
index 79f3e8b1bb..bc9a0d9611 100644
--- a/src/drivers/src/servo.c
+++ b/src/drivers/src/servo.c
@@ -24,7 +24,7 @@
  * servo.c - Servo driver
  *
  * This code mainly interfacing the PWM peripheral lib of ST.
- * Author: Eric Ewing
+ * Author: Eric Ewing, Will Wu
  */
 
 #include <stdbool.h>
@@ -44,10 +44,13 @@
 
 
 // TODO Verify PWM settings
-#define SERVO_PWM_BITS      (0)
-#define SERVO_PWM_PERIOD    1000  // 84mhz -> 50hz with 100 as a prescaler
-#define SERVO_PWM_PRESCALE  (uint16_t) (1680 * 2) //84mhz to khz
-#define SERVO_BASE_FREQ     (0) // should be calculated
+#define SERVO_PWM_BITS           (0)
+#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)
+#define SERVO_BASE_FREQ          (0) // should be calculated
+static const double SERVO_ZERO_PULSE_ms = 0.5;
+static const double SERVO_180_PULSE_ms  = 2.5;
 
 #include "servo.h"
 
@@ -94,7 +97,7 @@ void servoInit()
   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_Low;
+  TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
   TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
 
   // Configure OC3
@@ -109,7 +112,7 @@ void servoInit()
   //Enable the timer
   TIM_Cmd(servoMapMOSI->tim, ENABLE);
 
-  servoSetAngle(90);
+  servoSetAngle(130);
   isInit = true;
 }
 
@@ -120,12 +123,15 @@ bool servoTest(void)
 
 void servoSetAngle(uint8_t angle)
 {
-  // Convert angle to between .025 and .125 and then to ratio of signal
-  double angle_d = (double) angle;
-  uint32_t ratio = ((angle_d / 180.)* SERVO_PWM_PERIOD) / 10. + 100;
-  servoMapMOSI->setCompare(servoMapMOSI->tim, ratio);
-  // TIM_SetCompare4(SERVO_TIM, ratio);
-  DEBUG_PRINT("Set Angle: %u, set ratio: %" PRId32 "\n", angle, ratio);
+  // set CCR register
+  // Duty% = CCR/ARR*100, so CCR = Duty%/100 * ARR
+
+  double pulse_length_ms = (double)(angle) / 180. * (SERVO_180_PULSE_ms - SERVO_ZERO_PULSE_ms) + SERVO_ZERO_PULSE_ms;
+  double pulse_length_s = pulse_length_ms / 1000;
+  const uint32_t ccr_val = (uint32_t)(pulse_length_s * SERVO_PWM_PERIOD * SERVO_PWM_FREQUENCY_HZ);
+  servoMapMOSI->setCompare(servoMapMOSI->tim, ccr_val);
+  DEBUG_PRINT("[SERVO]: Set Angle: %u, set ratio: %" PRId32 "\n", angle, ccr_val);
+
 }
 
 PARAM_GROUP_START(servo_controller)

From c9e68e4e14407cd8bd30d56092a975d6f33a07c1 Mon Sep 17 00:00:00 2001
From: Will Wu <williamwu8-8@outlook.com>
Date: Fri, 15 Sep 2023 18:04:45 -0400
Subject: [PATCH 03/17] Add servo angle param and callback function

---
 src/drivers/interface/servo.h | 12 ++++++++++--
 src/drivers/src/servo.c       | 12 ++++++++++--
 2 files changed, 20 insertions(+), 4 deletions(-)

diff --git a/src/drivers/interface/servo.h b/src/drivers/interface/servo.h
index 4d220a9c94..5b2658ec70 100644
--- a/src/drivers/interface/servo.h
+++ b/src/drivers/interface/servo.h
@@ -50,8 +50,16 @@ void servoInit();
 bool servoTest(void);
 
 /**
- * Set servo ratio/power.
+ *
+ * @brief Set servo angle.
+ * @param: angle: desired servo angle
  */
-void servoSetAngle(uint8_t ratio);
+void servoSetAngle(uint8_t angle);
+
+/**
+ * Servo angle parameter callback. When servo angle is changed, call
+ * function to change servo angle automatically
+ *  */
+void servoAngleCallBack(void);
 
 #endif /* __MOTORS_H__ */
diff --git a/src/drivers/src/servo.c b/src/drivers/src/servo.c
index bc9a0d9611..76bf1d9469 100644
--- a/src/drivers/src/servo.c
+++ b/src/drivers/src/servo.c
@@ -51,6 +51,8 @@
 #define SERVO_BASE_FREQ          (0) // should be calculated
 static const double SERVO_ZERO_PULSE_ms = 0.5;
 static const double SERVO_180_PULSE_ms  = 2.5;
+static const uint8_t SERVO_ANGLE_ZERO = 130;
+static uint8_t s_servo_angle = SERVO_ANGLE_ZERO;
 
 #include "servo.h"
 
@@ -112,7 +114,7 @@ void servoInit()
   //Enable the timer
   TIM_Cmd(servoMapMOSI->tim, ENABLE);
 
-  servoSetAngle(130);
+  servoSetAngle(SERVO_ANGLE_ZERO);
   isInit = true;
 }
 
@@ -134,8 +136,14 @@ void servoSetAngle(uint8_t angle)
 
 }
 
+void servoAngleCallBack(void)
+{
+  servoSetAngle(s_servo_angle);
+}
+
 PARAM_GROUP_START(servo_controller)
 
-// PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, servoInit, &isInit)
+PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, servoInit, &isInit)
+PARAM_ADD_WITH_CALLBACK(PARAM_UINT8 , servoAngle, &s_servo_angle, &servoAngleCallBack)
 
 PARAM_GROUP_STOP(servo_controller)

From 8a98fb0f2d4352e071cc3168dcac310106ef9044 Mon Sep 17 00:00:00 2001
From: Will Wu <williamwu8-8@outlook.com>
Date: Mon, 18 Sep 2023 17:36:10 -0400
Subject: [PATCH 04/17] Fix servo param group bug

Move Param and callback declaration

Add angle capping and center around 0
---
 src/deck/drivers/src/bigquad.c |  8 ++++++++
 src/drivers/interface/servo.h  | 13 ++++++++++++-
 src/drivers/src/motors_def.c   |  2 +-
 src/drivers/src/servo.c        | 29 +++++++++++++++--------------
 4 files changed, 36 insertions(+), 16 deletions(-)

diff --git a/src/deck/drivers/src/bigquad.c b/src/deck/drivers/src/bigquad.c
index d9187683bb..84b8afce8e 100644
--- a/src/deck/drivers/src/bigquad.c
+++ b/src/deck/drivers/src/bigquad.c
@@ -53,6 +53,7 @@
 
 //Hardware configuration
 static bool isInit;
+static int8_t s_servo_angle = 0;
 
 #ifdef CONFIG_DECK_BIGQUAD_ENABLE_OSD
 static MspObject s_MspObject;
@@ -128,6 +129,11 @@ static const DeckDriver bigquad_deck = {
   .test = bigquadTest,
 };
 
+void servoAngleCallBack(void)
+{
+  servoSetAngle(saturateAngle(s_servo_angle));
+}
+
 DECK_DRIVER(bigquad_deck);
 
 PARAM_GROUP_START(deck)
@@ -136,6 +142,8 @@ PARAM_GROUP_START(deck)
  * @brief Nonzero if [BigQuad deck](%https://www.bitcraze.io/products/bigquad-deck) is attached
  */
 PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcBigQuad, &isInit)
+PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, servoInit, &isInit)
+PARAM_ADD_WITH_CALLBACK(PARAM_INT8 , servoAngle, &s_servo_angle, &servoAngleCallBack)
 
 PARAM_GROUP_STOP(deck)
 
diff --git a/src/drivers/interface/servo.h b/src/drivers/interface/servo.h
index 5b2658ec70..e17636a9e6 100644
--- a/src/drivers/interface/servo.h
+++ b/src/drivers/interface/servo.h
@@ -38,7 +38,13 @@
 #include "config.h"
 
 /******** Defines ********/
-
+#define SERVO_PWM_BITS           (0)
+#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)
+#define SERVO_BASE_FREQ          (0) // should be calculated
+#define SERVO_ANGLE_ZERO         130
+#define SERVO_ANGLE_LIMIT        30
 /*** Public interface ***/
 
 /**
@@ -62,4 +68,9 @@ void servoSetAngle(uint8_t angle);
  *  */
 void servoAngleCallBack(void);
 
+/**
+ * Saturate servo angle. Limit is defined by macro: SERVO_ANGLE_LIMIT
+ * @param angle: pointer to desired angle
+ * */
+uint8_t saturateAngle(int8_t angle);
 #endif /* __MOTORS_H__ */
diff --git a/src/drivers/src/motors_def.c b/src/drivers/src/motors_def.c
index 430c8e9784..7830aae479 100644
--- a/src/drivers/src/motors_def.c
+++ b/src/drivers/src/motors_def.c
@@ -774,4 +774,4 @@ const MotorPerifDef* motorMapCF21Brushless[NBR_OF_MOTORS] =
 /**
  * Servo mapped to the Bigquad CPPM (MOSI) port
  */
- const MotorPerifDef* servoMapMOSI = &MOTORS_PA7_TIM14_CH1_BRUSHLESS_OD;
+const MotorPerifDef* servoMapMOSI = &MOTORS_PA7_TIM14_CH1_BRUSHLESS_OD;
diff --git a/src/drivers/src/servo.c b/src/drivers/src/servo.c
index 76bf1d9469..0dbc27dbfe 100644
--- a/src/drivers/src/servo.c
+++ b/src/drivers/src/servo.c
@@ -42,17 +42,9 @@
 #include <inttypes.h>
 #include <motors.h>
 
-
 // TODO Verify PWM settings
-#define SERVO_PWM_BITS           (0)
-#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)
-#define SERVO_BASE_FREQ          (0) // should be calculated
 static const double SERVO_ZERO_PULSE_ms = 0.5;
 static const double SERVO_180_PULSE_ms  = 2.5;
-static const uint8_t SERVO_ANGLE_ZERO = 130;
-static uint8_t s_servo_angle = SERVO_ANGLE_ZERO;
 
 #include "servo.h"
 
@@ -136,14 +128,23 @@ void servoSetAngle(uint8_t angle)
 
 }
 
-void servoAngleCallBack(void)
+uint8_t saturateAngle(int8_t angle)
 {
-  servoSetAngle(s_servo_angle);
+  if (angle > SERVO_ANGLE_LIMIT || angle < -SERVO_ANGLE_LIMIT) {
+    DEBUG_PRINT("[SERVO]: Servo angle out of range! Capping...\n");
+    bool sign = angle > 0;
+    return sign ? SERVO_ANGLE_LIMIT + SERVO_ANGLE_ZERO : -SERVO_ANGLE_LIMIT+SERVO_ANGLE_ZERO;
+  }
+  else {
+    return angle + SERVO_ANGLE_ZERO;
+  }
+
 }
 
-PARAM_GROUP_START(servo_controller)
 
-PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, servoInit, &isInit)
-PARAM_ADD_WITH_CALLBACK(PARAM_UINT8 , servoAngle, &s_servo_angle, &servoAngleCallBack)
+//PARAM_GROUP_START(servo_controller)
+
+//PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, servoInit, &isInit)
+//PARAM_ADD_WITH_CALLBACK(PARAM_UINT8 , servoAngle, &s_servo_angle, &servoAngleCallBack)
 
-PARAM_GROUP_STOP(servo_controller)
+//PARAM_GROUP_STOP(servo_controller)

From 4f7051b638662305a3a8264bb7e75cddc4fbbb4f Mon Sep 17 00:00:00 2001
From: Eric <eric_ewing@brown.edu>
Date: Thu, 26 Oct 2023 18:33:54 -0400
Subject: [PATCH 05/17] Change servo param back to servo param group

---
 src/deck/drivers/src/bigquad.c |  8 --------
 src/drivers/src/motors_def.c   |  2 +-
 src/drivers/src/servo.c        | 20 +++++++++++++-------
 src/modules/src/supervisor.c   |  6 ++++++
 4 files changed, 20 insertions(+), 16 deletions(-)

diff --git a/src/deck/drivers/src/bigquad.c b/src/deck/drivers/src/bigquad.c
index 84b8afce8e..7282f0b818 100644
--- a/src/deck/drivers/src/bigquad.c
+++ b/src/deck/drivers/src/bigquad.c
@@ -129,11 +129,6 @@ static const DeckDriver bigquad_deck = {
   .test = bigquadTest,
 };
 
-void servoAngleCallBack(void)
-{
-  servoSetAngle(saturateAngle(s_servo_angle));
-}
-
 DECK_DRIVER(bigquad_deck);
 
 PARAM_GROUP_START(deck)
@@ -142,9 +137,6 @@ PARAM_GROUP_START(deck)
  * @brief Nonzero if [BigQuad deck](%https://www.bitcraze.io/products/bigquad-deck) is attached
  */
 PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcBigQuad, &isInit)
-PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, servoInit, &isInit)
-PARAM_ADD_WITH_CALLBACK(PARAM_INT8 , servoAngle, &s_servo_angle, &servoAngleCallBack)
-
 PARAM_GROUP_STOP(deck)
 
 #endif // CONFIG_DECK_BIGQUAD
diff --git a/src/drivers/src/motors_def.c b/src/drivers/src/motors_def.c
index da70fbdce9..551b4fe53c 100644
--- a/src/drivers/src/motors_def.c
+++ b/src/drivers/src/motors_def.c
@@ -808,4 +808,4 @@ const MotorPerifDef* motorMapCF21Brushless[NBR_OF_MOTORS] =
 /**
  * Servo mapped to the Bigquad CPPM (MOSI) port
  */
-const MotorPerifDef* servoMapMOSI = &MOTORS_PA7_TIM14_CH1_BRUSHLESS_OD;
+const MotorPerifDef* servoMapMOSI = &MOTORS_PA7_TIM14_CH1_BRUSHLESS_OD;
\ No newline at end of file
diff --git a/src/drivers/src/servo.c b/src/drivers/src/servo.c
index 0dbc27dbfe..20cbd35609 100644
--- a/src/drivers/src/servo.c
+++ b/src/drivers/src/servo.c
@@ -53,6 +53,7 @@ static bool isInit = false;
 extern const MotorPerifDef* servoMapMOSI;
 
 /* Public functions */
+static int8_t s_servo_angle = 0;
 
 void servoInit()
 {
@@ -94,16 +95,16 @@ void servoInit()
   TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
   TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
 
-  // Configure OC3
+  // Configure OC1
   servoMapMOSI->ocInit(servoMapMOSI->tim, &TIM_OCInitStructure);
   servoMapMOSI->preloadConfig(servoMapMOSI->tim, TIM_OCPreload_Enable);
 
 
   //Enable the timer PWM outputs
   TIM_CtrlPWMOutputs(servoMapMOSI->tim, ENABLE);
-  servoMapMOSI->setCompare(servoMapMOSI->tim, 0x00);
+servoMapMOSI->setCompare(servoMapMOSI->tim, 0x00);
 
-  //Enable the timer
+//Enable the timer
   TIM_Cmd(servoMapMOSI->tim, ENABLE);
 
   servoSetAngle(SERVO_ANGLE_ZERO);
@@ -141,10 +142,15 @@ uint8_t saturateAngle(int8_t angle)
 
 }
 
+void servoAngleCallBack(void)
+{
+  servoInit();
+  servoSetAngle(saturateAngle(s_servo_angle));
+}
 
-//PARAM_GROUP_START(servo_controller)
+PARAM_GROUP_START(servo)
 
-//PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, servoInit, &isInit)
-//PARAM_ADD_WITH_CALLBACK(PARAM_UINT8 , servoAngle, &s_servo_angle, &servoAngleCallBack)
+PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, servoInitialized, &isInit)
+PARAM_ADD_WITH_CALLBACK(PARAM_UINT8 , servoAngle, &s_servo_angle, &servoAngleCallBack)
 
-//PARAM_GROUP_STOP(servo_controller)
+PARAM_GROUP_STOP(servo)
\ No newline at end of file
diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c
index 2e209829ab..ec9572de07 100644
--- a/src/modules/src/supervisor.c
+++ b/src/modules/src/supervisor.c
@@ -259,26 +259,32 @@ static supervisorConditionBits_t updateAndPopulateConditions(SupervisorMem_t* th
 
   const bool isTumbled = isTumbledCheck(this, sensors, currentTick);
   if (isTumbled) {
+      DEBUG_PRINT("Is Tumbled!\n");
     conditions |= SUPERVISOR_CB_IS_TUMBLED;
   }
 
   const uint32_t setpointAge = currentTick - setpoint->timestamp;
   if (setpointAge > COMMANDER_WDT_TIMEOUT_STABILIZE) {
+      DEBUG_PRINT("Setpoint is old!\n");
     conditions |= SUPERVISOR_CB_COMMANDER_WDT_WARNING;
   }
   if (setpointAge > COMMANDER_WDT_TIMEOUT_SHUTDOWN) {
+      DEBUG_PRINT("Setpoint is old! Timeout!\n");
     conditions |= SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT;
   }
 
   if (!checkEmergencyStopWatchdog(currentTick)) {
+          DEBUG_PRINT("Emergency Stop!\n");
     conditions |= SUPERVISOR_CB_EMERGENCY_STOP;
   }
 
   if (locSrvIsEmergencyStopRequested()) {
+          DEBUG_PRINT("Loc Srv Emergency stop!\n");
     conditions |= SUPERVISOR_CB_EMERGENCY_STOP;
   }
 
   if (this->paramEmergencyStop) {
+              DEBUG_PRINT("Param Emergency stop!\n");
     conditions |= SUPERVISOR_CB_EMERGENCY_STOP;
   }
 

From 4edb9bf53c5f7499282b1029bd4be0abc5588f3a Mon Sep 17 00:00:00 2001
From: Tobias Antonson <tobbeanton@gmail.com>
Date: Sat, 4 Nov 2023 19:28:49 +0100
Subject: [PATCH 06/17] Made Servo a driver and fixed its AF mapping.

---
 src/deck/drivers/src/bigquad.c |  4 ++--
 src/drivers/src/motors_def.c   | 24 +++++++++++++++++++++-
 src/drivers/src/servo.c        | 37 ++++++++++++++++++++++++----------
 3 files changed, 51 insertions(+), 14 deletions(-)

diff --git a/src/deck/drivers/src/bigquad.c b/src/deck/drivers/src/bigquad.c
index 7282f0b818..82fa67aed3 100644
--- a/src/deck/drivers/src/bigquad.c
+++ b/src/deck/drivers/src/bigquad.c
@@ -53,7 +53,7 @@
 
 //Hardware configuration
 static bool isInit;
-static int8_t s_servo_angle = 0;
+//static int8_t s_servo_angle = 0;
 
 #ifdef CONFIG_DECK_BIGQUAD_ENABLE_OSD
 static MspObject s_MspObject;
@@ -84,7 +84,7 @@ static void bigquadInit(DeckInfo *info)
 
   DEBUG_PRINT("Switching to brushless.\n");
   motorsInit(motorMapBigQuadDeck);
-  extRxInit();
+//  extRxInit();
 
   // Ignore charging/charged state to allow low-battery warning.
   pmIgnoreChargedState(true);
diff --git a/src/drivers/src/motors_def.c b/src/drivers/src/motors_def.c
index 551b4fe53c..05659a9902 100644
--- a/src/drivers/src/motors_def.c
+++ b/src/drivers/src/motors_def.c
@@ -710,6 +710,28 @@ static const MotorPerifDef MOTORS_PA7_TIM14_CH1_BRUSHLESS_OD =
     .preloadConfig = TIM_OC1PreloadConfig,
 };
 
+// Deck MOSI, PA7, TIM14_CH1, Servo
+static const MotorPerifDef MOTORS_PA7_TIM14_CH1_SERVO =
+{
+    .drvType       = BRUSHLESS,
+    .gpioPerif     = RCC_AHB1Periph_GPIOA,
+    .gpioPort      = GPIOA,
+    .gpioPin       = GPIO_Pin_7,
+    .gpioPinSource = GPIO_PinSource7,
+    .gpioOType     = GPIO_OType_PP,
+    .gpioAF        = GPIO_AF_TIM14,
+    .timPerif      = RCC_APB1Periph_TIM14,
+    .tim           = TIM14,
+    .timPolarity   = TIM_OCPolarity_High,
+    .timDbgStop    = DBGMCU_TIM14_STOP,
+    .timPeriod     = MOTORS_BL_PWM_PERIOD,
+    .timPrescaler  = MOTORS_BL_PWM_PRESCALE,
+    .setCompare    = TIM_SetCompare1,
+    .getCompare    = TIM_GetCapture1,
+    .ocInit        = TIM_OC1Init,
+    .preloadConfig = TIM_OC1PreloadConfig,
+};
+
 /**
  * Mapping for Tags that don't have motors.
  * Actually same mapping as for CF2 but the pins are not physically connected.
@@ -808,4 +830,4 @@ const MotorPerifDef* motorMapCF21Brushless[NBR_OF_MOTORS] =
 /**
  * Servo mapped to the Bigquad CPPM (MOSI) port
  */
-const MotorPerifDef* servoMapMOSI = &MOTORS_PA7_TIM14_CH1_BRUSHLESS_OD;
\ No newline at end of file
+const MotorPerifDef* servoMapMOSI = &MOTORS_PA7_TIM14_CH1_SERVO;
\ No newline at end of file
diff --git a/src/drivers/src/servo.c b/src/drivers/src/servo.c
index 20cbd35609..e3bcb64169 100644
--- a/src/drivers/src/servo.c
+++ b/src/drivers/src/servo.c
@@ -26,13 +26,12 @@
  * This code mainly interfacing the PWM peripheral lib of ST.
  * Author: Eric Ewing, Will Wu
  */
-
+#define DEBUG_MODULE "SERVO"
+#
 #include <stdbool.h>
 
 /* ST includes */
 #include "stm32fxxx.h"
-#include "param.h"
-
 
 //FreeRTOS includes
 #include "FreeRTOS.h"
@@ -40,7 +39,9 @@
 #include "debug.h"
 #include <string.h>
 #include <inttypes.h>
-#include <motors.h>
+#include "motors.h"
+#include "param.h"
+#include "deck.h"
 
 // TODO Verify PWM settings
 static const double SERVO_ZERO_PULSE_ms = 0.5;
@@ -58,9 +59,9 @@ static int8_t s_servo_angle = 0;
 void servoInit()
 {
   if (isInit){
-    DEBUG_PRINT("Already Initialized!");
     return;
   }
+
   GPIO_InitTypeDef GPIO_InitStructure;
   TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
   TIM_OCInitTypeDef  TIM_OCInitStructure;
@@ -72,13 +73,13 @@ void servoInit()
   //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_DOWN;
+  GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; // TODO: verify this
   GPIO_InitStructure.GPIO_Pin = servoMapMOSI->gpioPin;
   GPIO_Init(servoMapMOSI->gpioPort, &GPIO_InitStructure);
 
   //map timer to alternate function
-  GPIO_PinAFConfig(servoMapMOSI->gpioPort, servoMapMOSI->gpioPin, servoMapMOSI->gpioAF);
+  GPIO_PinAFConfig(servoMapMOSI->gpioPort, servoMapMOSI->gpioPinSource, servoMapMOSI->gpioAF);
 
   //Timer configuration
   TIM_TimeBaseStructure.TIM_Period = SERVO_PWM_PERIOD;
@@ -102,12 +103,14 @@ void servoInit()
 
   //Enable the timer PWM outputs
   TIM_CtrlPWMOutputs(servoMapMOSI->tim, ENABLE);
-servoMapMOSI->setCompare(servoMapMOSI->tim, 0x00);
+  servoMapMOSI->setCompare(servoMapMOSI->tim, 0x00);
 
-//Enable the timer
+  //Enable the timer
   TIM_Cmd(servoMapMOSI->tim, ENABLE);
 
+  DEBUG_PRINT("Init [OK]\n");
   servoSetAngle(SERVO_ANGLE_ZERO);
+
   isInit = true;
 }
 
@@ -132,7 +135,7 @@ void servoSetAngle(uint8_t angle)
 uint8_t saturateAngle(int8_t angle)
 {
   if (angle > SERVO_ANGLE_LIMIT || angle < -SERVO_ANGLE_LIMIT) {
-    DEBUG_PRINT("[SERVO]: Servo angle out of range! Capping...\n");
+//    DEBUG_PRINT("[SERVO]: Servo angle out of range! Capping...\n");
     bool sign = angle > 0;
     return sign ? SERVO_ANGLE_LIMIT + SERVO_ANGLE_ZERO : -SERVO_ANGLE_LIMIT+SERVO_ANGLE_ZERO;
   }
@@ -144,10 +147,22 @@ uint8_t saturateAngle(int8_t angle)
 
 void servoAngleCallBack(void)
 {
-  servoInit();
   servoSetAngle(saturateAngle(s_servo_angle));
 }
 
+static const DeckDriver servo_deck = {
+  .vid = 0x00,
+  .pid = 0x00,
+  .name = "bcServo",
+
+  .usedPeriph = 0,
+  .usedGpio = 0,
+  .init = servoInit,
+  .test = 0,
+};
+
+DECK_DRIVER(servo_deck);
+
 PARAM_GROUP_START(servo)
 
 PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, servoInitialized, &isInit)

From d536c30faac7c559dc2d4b579613543e707ec5b2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jos=C3=A9=20=C3=81ngel?= <gelanchez@gmail.com>
Date: Thu, 21 Dec 2023 17:00:11 +0000
Subject: [PATCH 07/17] PID avoid derivative kick

---
 src/utils/interface/pid.h |   2 +-
 src/utils/src/pid.c       | 118 ++++++++++++++++++--------------------
 2 files changed, 58 insertions(+), 62 deletions(-)

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)

From fc022b683eed71da4c5f4a67d559c64600aa4d54 Mon Sep 17 00:00:00 2001
From: LAMBERT des CILLEULS Charles <charles.lambert-des-cilleuls@loria.fr>
Date: Thu, 4 Jan 2024 13:36:21 +0100
Subject: [PATCH 08/17] moving some physical constants so they can be config
 dependants

---
 src/modules/src/controller/controller_brescianini.c |  1 +
 src/modules/src/controller/controller_mellinger.c   |  1 +
 src/modules/src/power_distribution_quadrotor.c      |  3 ++-
 src/platform/interface/platform_defaults.h          | 10 ++++++++++
 src/utils/interface/physicalConstants.h             |  4 ----
 5 files changed, 14 insertions(+), 5 deletions(-)

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/power_distribution_quadrotor.c b/src/modules/src/power_distribution_quadrotor.c
index c34de07d41..0f3a9d046e 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/platform/interface/platform_defaults.h b/src/platform/interface/platform_defaults.h
index af8d5d0f86..0ef68e2a2a 100644
--- a/src/platform/interface/platform_defaults.h
+++ b/src/platform/interface/platform_defaults.h
@@ -44,6 +44,16 @@
     #include "platform_defaults_flapper.h"
 #endif
 
+// Drone physical constants
+#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
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

From 4217d1640543859f47537cdc0a7d679f33330b73 Mon Sep 17 00:00:00 2001
From: LAMBERT des CILLEULS Charles <charles.lambert-des-cilleuls@loria.fr>
Date: Mon, 8 Jan 2024 11:56:48 +0100
Subject: [PATCH 09/17] providing specialised arm_length and weight for cf2
 platform

---
 src/platform/interface/platform_defaults.h     | 1 +
 src/platform/interface/platform_defaults_cf2.h | 6 ++++++
 2 files changed, 7 insertions(+)

diff --git a/src/platform/interface/platform_defaults.h b/src/platform/interface/platform_defaults.h
index 0ef68e2a2a..427bc39a47 100644
--- a/src/platform/interface/platform_defaults.h
+++ b/src/platform/interface/platform_defaults.h
@@ -45,6 +45,7 @@
 #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
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

From d448764d0c43951f0d9e5493352086760e5bf1ed Mon Sep 17 00:00:00 2001
From: matejkarasek <matejkarasek@gmail.com>
Date: Tue, 9 Jan 2024 13:32:29 +0100
Subject: [PATCH 10/17] Cleanup

---
 src/deck/drivers/src/bigquad.c | 8 +++-----
 src/drivers/src/motors_def.c   | 2 +-
 src/drivers/src/servo.c        | 2 +-
 src/modules/src/supervisor.c   | 6 ------
 4 files changed, 5 insertions(+), 13 deletions(-)

diff --git a/src/deck/drivers/src/bigquad.c b/src/deck/drivers/src/bigquad.c
index 82fa67aed3..00bcd20f33 100644
--- a/src/deck/drivers/src/bigquad.c
+++ b/src/deck/drivers/src/bigquad.c
@@ -42,7 +42,6 @@
 
 #include "FreeRTOS.h"
 #include "task.h"
-#include "servo.h"
 
 #define BIGQUAD_BAT_VOLT_PIN       DECK_GPIO_MISO
 #define BIGQUAD_BAT_VOLT_MULT      (CONFIG_DECK_BIGQUAD_BAT_VOLT_MULT_MV / 1000.0)
@@ -53,7 +52,6 @@
 
 //Hardware configuration
 static bool isInit;
-//static int8_t s_servo_angle = 0;
 
 #ifdef CONFIG_DECK_BIGQUAD_ENABLE_OSD
 static MspObject s_MspObject;
@@ -84,7 +82,7 @@ static void bigquadInit(DeckInfo *info)
 
   DEBUG_PRINT("Switching to brushless.\n");
   motorsInit(motorMapBigQuadDeck);
-//  extRxInit();
+  extRxInit();
 
   // Ignore charging/charged state to allow low-battery warning.
   pmIgnoreChargedState(true);
@@ -101,7 +99,6 @@ static void bigquadInit(DeckInfo *info)
               BQ_OSD_TASK_STACKSIZE, NULL, BQ_OSD_TASK_PRI, NULL);
 #endif
 
-  servoInit();
   isInit = true;
 }
 
@@ -124,7 +121,7 @@ static const DeckDriver bigquad_deck = {
 
   .usedPeriph = DECK_USING_TIMER3 | DECK_USING_TIMER14,
   .usedGpio = DECK_USING_PA2 | DECK_USING_PA3 | DECK_USING_IO_3 |
-              DECK_USING_IO_2 | DECK_USING_PA7 | DECK_USING_IO_4,
+              DECK_USING_IO_2 | DECK_USING_PA7,
   .init = bigquadInit,
   .test = bigquadTest,
 };
@@ -137,6 +134,7 @@ PARAM_GROUP_START(deck)
  * @brief Nonzero if [BigQuad deck](%https://www.bitcraze.io/products/bigquad-deck) is attached
  */
 PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcBigQuad, &isInit)
+
 PARAM_GROUP_STOP(deck)
 
 #endif // CONFIG_DECK_BIGQUAD
diff --git a/src/drivers/src/motors_def.c b/src/drivers/src/motors_def.c
index 05659a9902..4f84e7aa86 100644
--- a/src/drivers/src/motors_def.c
+++ b/src/drivers/src/motors_def.c
@@ -830,4 +830,4 @@ const MotorPerifDef* motorMapCF21Brushless[NBR_OF_MOTORS] =
 /**
  * Servo mapped to the Bigquad CPPM (MOSI) port
  */
-const MotorPerifDef* servoMapMOSI = &MOTORS_PA7_TIM14_CH1_SERVO;
\ No newline at end of file
+const MotorPerifDef* servoMapMOSI = &MOTORS_PA7_TIM14_CH1_SERVO;
diff --git a/src/drivers/src/servo.c b/src/drivers/src/servo.c
index e3bcb64169..c7fd1d38f8 100644
--- a/src/drivers/src/servo.c
+++ b/src/drivers/src/servo.c
@@ -168,4 +168,4 @@ PARAM_GROUP_START(servo)
 PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, servoInitialized, &isInit)
 PARAM_ADD_WITH_CALLBACK(PARAM_UINT8 , servoAngle, &s_servo_angle, &servoAngleCallBack)
 
-PARAM_GROUP_STOP(servo)
\ No newline at end of file
+PARAM_GROUP_STOP(servo)
diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c
index ec9572de07..2e209829ab 100644
--- a/src/modules/src/supervisor.c
+++ b/src/modules/src/supervisor.c
@@ -259,32 +259,26 @@ static supervisorConditionBits_t updateAndPopulateConditions(SupervisorMem_t* th
 
   const bool isTumbled = isTumbledCheck(this, sensors, currentTick);
   if (isTumbled) {
-      DEBUG_PRINT("Is Tumbled!\n");
     conditions |= SUPERVISOR_CB_IS_TUMBLED;
   }
 
   const uint32_t setpointAge = currentTick - setpoint->timestamp;
   if (setpointAge > COMMANDER_WDT_TIMEOUT_STABILIZE) {
-      DEBUG_PRINT("Setpoint is old!\n");
     conditions |= SUPERVISOR_CB_COMMANDER_WDT_WARNING;
   }
   if (setpointAge > COMMANDER_WDT_TIMEOUT_SHUTDOWN) {
-      DEBUG_PRINT("Setpoint is old! Timeout!\n");
     conditions |= SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT;
   }
 
   if (!checkEmergencyStopWatchdog(currentTick)) {
-          DEBUG_PRINT("Emergency Stop!\n");
     conditions |= SUPERVISOR_CB_EMERGENCY_STOP;
   }
 
   if (locSrvIsEmergencyStopRequested()) {
-          DEBUG_PRINT("Loc Srv Emergency stop!\n");
     conditions |= SUPERVISOR_CB_EMERGENCY_STOP;
   }
 
   if (this->paramEmergencyStop) {
-              DEBUG_PRINT("Param Emergency stop!\n");
     conditions |= SUPERVISOR_CB_EMERGENCY_STOP;
   }
 

From 34d1df2f7db8ec5885d73ba8796ecad1491b0688 Mon Sep 17 00:00:00 2001
From: matejkarasek <matejkarasek@gmail.com>
Date: Tue, 9 Jan 2024 17:26:54 +0100
Subject: [PATCH 11/17] Converted to deck, choice from multiple pins

---
 src/{ => deck}/drivers/interface/servo.h |  17 +-
 src/deck/drivers/src/Kbuild              |   1 +
 src/deck/drivers/src/Kconfig             |  31 +++
 src/deck/drivers/src/servo.c             | 233 +++++++++++++++++++++++
 src/deck/interface/deck_core.h           |   1 +
 src/drivers/src/Kbuild                   |   1 -
 src/drivers/src/motors_def.c             |  49 ++---
 src/drivers/src/servo.c                  | 171 -----------------
 8 files changed, 298 insertions(+), 206 deletions(-)
 rename src/{ => deck}/drivers/interface/servo.h (79%)
 create mode 100644 src/deck/drivers/src/servo.c
 delete mode 100644 src/drivers/src/servo.c

diff --git a/src/drivers/interface/servo.h b/src/deck/drivers/interface/servo.h
similarity index 79%
rename from src/drivers/interface/servo.h
rename to src/deck/drivers/interface/servo.h
index e17636a9e6..bc58675d09 100644
--- a/src/drivers/interface/servo.h
+++ b/src/deck/drivers/interface/servo.h
@@ -38,18 +38,12 @@
 #include "config.h"
 
 /******** Defines ********/
-#define SERVO_PWM_BITS           (0)
 #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)
-#define SERVO_BASE_FREQ          (0) // should be calculated
-#define SERVO_ANGLE_ZERO         130
-#define SERVO_ANGLE_LIMIT        30
-/*** Public interface ***/
 
 /**
- * Servo Initialization. Configures two output compare channels in PWM mode
- * with one of them inverted to increase power output.
+ * Servo Initialization
  */
 void servoInit();
 
@@ -58,7 +52,7 @@ bool servoTest(void);
 /**
  *
  * @brief Set servo angle.
- * @param: angle: desired servo angle
+ * @param: angle: desired servo angle in degrees
  */
 void servoSetAngle(uint8_t angle);
 
@@ -69,8 +63,9 @@ void servoSetAngle(uint8_t angle);
 void servoAngleCallBack(void);
 
 /**
- * Saturate servo angle. Limit is defined by macro: SERVO_ANGLE_LIMIT
+ * Saturate servo angle. Min is 0, max is defined by parameter 'servoRange'
  * @param angle: pointer to desired angle
  * */
-uint8_t saturateAngle(int8_t angle);
-#endif /* __MOTORS_H__ */
+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..8f04e2aaa1
--- /dev/null
+++ b/src/deck/drivers/src/servo.c
@@ -0,0 +1,233 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 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"
+
+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);
+  DEBUG_PRINT("Set Angle: %u deg, pulse width: %f us \n", angle, pulse_length_us);
+
+}
+
+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(servo)
+
+PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, servoInitialized, &isInit)
+PARAM_ADD(PARAM_UINT16 | PARAM_PERSISTENT, servoMINus, &servo_MIN_us)
+PARAM_ADD(PARAM_UINT16 | PARAM_PERSISTENT, servoMAXus, &servo_MAX_us)
+PARAM_ADD(PARAM_UINT8 | PARAM_PERSISTENT, servoRange, &servo_range)
+PARAM_ADD(PARAM_UINT8 | PARAM_PERSISTENT, servoIdle, &servo_idle)
+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/Kbuild b/src/drivers/src/Kbuild
index b496872bfb..18bc093636 100644
--- a/src/drivers/src/Kbuild
+++ b/src/drivers/src/Kbuild
@@ -17,7 +17,6 @@ obj-y += nvic.o
 obj-y += pca9685.o
 obj-y += piezo.o
 obj-y += pmw3901.o
-obj-y += servo.o
 obj-y += swd.o
 obj-y += uart1.o
 obj-y += uart2.o
diff --git a/src/drivers/src/motors_def.c b/src/drivers/src/motors_def.c
index 4f84e7aa86..93abbb1a99 100644
--- a/src/drivers/src/motors_def.c
+++ b/src/drivers/src/motors_def.c
@@ -710,28 +710,6 @@ static const MotorPerifDef MOTORS_PA7_TIM14_CH1_BRUSHLESS_OD =
     .preloadConfig = TIM_OC1PreloadConfig,
 };
 
-// Deck MOSI, PA7, TIM14_CH1, Servo
-static const MotorPerifDef MOTORS_PA7_TIM14_CH1_SERVO =
-{
-    .drvType       = BRUSHLESS,
-    .gpioPerif     = RCC_AHB1Periph_GPIOA,
-    .gpioPort      = GPIOA,
-    .gpioPin       = GPIO_Pin_7,
-    .gpioPinSource = GPIO_PinSource7,
-    .gpioOType     = GPIO_OType_PP,
-    .gpioAF        = GPIO_AF_TIM14,
-    .timPerif      = RCC_APB1Periph_TIM14,
-    .tim           = TIM14,
-    .timPolarity   = TIM_OCPolarity_High,
-    .timDbgStop    = DBGMCU_TIM14_STOP,
-    .timPeriod     = MOTORS_BL_PWM_PERIOD,
-    .timPrescaler  = MOTORS_BL_PWM_PRESCALE,
-    .setCompare    = TIM_SetCompare1,
-    .getCompare    = TIM_GetCapture1,
-    .ocInit        = TIM_OC1Init,
-    .preloadConfig = TIM_OC1PreloadConfig,
-};
-
 /**
  * Mapping for Tags that don't have motors.
  * Actually same mapping as for CF2 but the pins are not physically connected.
@@ -830,4 +808,29 @@ const MotorPerifDef* motorMapCF21Brushless[NBR_OF_MOTORS] =
 /**
  * Servo mapped to the Bigquad CPPM (MOSI) port
  */
-const MotorPerifDef* servoMapMOSI = &MOTORS_PA7_TIM14_CH1_SERVO;
+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_TIM2_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/drivers/src/servo.c b/src/drivers/src/servo.c
deleted file mode 100644
index c7fd1d38f8..0000000000
--- a/src/drivers/src/servo.c
+++ /dev/null
@@ -1,171 +0,0 @@
-/**
- *    ||          ____  _ __
- * +------+      / __ )(_) /_______________ _____  ___
- * | 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.c - Servo driver
- *
- * This code mainly interfacing the PWM peripheral lib of ST.
- * Author: 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"
-
-// TODO Verify PWM settings
-static const double SERVO_ZERO_PULSE_ms = 0.5;
-static const double SERVO_180_PULSE_ms  = 2.5;
-
-#include "servo.h"
-
-static bool isInit = false;
-// we use the "servoMapMOSI" struct to initialize PWM
-extern const MotorPerifDef* servoMapMOSI;
-
-/* Public functions */
-static int8_t s_servo_angle = 0;
-
-void servoInit()
-{
-  if (isInit){
-    return;
-  }
-
-  GPIO_InitTypeDef GPIO_InitStructure;
-  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
-  TIM_OCInitTypeDef  TIM_OCInitStructure;
-
-  //clock the servo pin and the timers
-  RCC_AHB1PeriphClockCmd(servoMapMOSI->gpioPerif, ENABLE);
-  RCC_APB1PeriphClockCmd(servoMapMOSI->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; // TODO: verify this
-  GPIO_InitStructure.GPIO_Pin = servoMapMOSI->gpioPin;
-  GPIO_Init(servoMapMOSI->gpioPort, &GPIO_InitStructure);
-
-  //map timer to alternate function
-  GPIO_PinAFConfig(servoMapMOSI->gpioPort, servoMapMOSI->gpioPinSource, servoMapMOSI->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(servoMapMOSI->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
-  servoMapMOSI->ocInit(servoMapMOSI->tim, &TIM_OCInitStructure);
-  servoMapMOSI->preloadConfig(servoMapMOSI->tim, TIM_OCPreload_Enable);
-
-
-  //Enable the timer PWM outputs
-  TIM_CtrlPWMOutputs(servoMapMOSI->tim, ENABLE);
-  servoMapMOSI->setCompare(servoMapMOSI->tim, 0x00);
-
-  //Enable the timer
-  TIM_Cmd(servoMapMOSI->tim, ENABLE);
-
-  DEBUG_PRINT("Init [OK]\n");
-  servoSetAngle(SERVO_ANGLE_ZERO);
-
-  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_ms = (double)(angle) / 180. * (SERVO_180_PULSE_ms - SERVO_ZERO_PULSE_ms) + SERVO_ZERO_PULSE_ms;
-  double pulse_length_s = pulse_length_ms / 1000;
-  const uint32_t ccr_val = (uint32_t)(pulse_length_s * SERVO_PWM_PERIOD * SERVO_PWM_FREQUENCY_HZ);
-  servoMapMOSI->setCompare(servoMapMOSI->tim, ccr_val);
-  DEBUG_PRINT("[SERVO]: Set Angle: %u, set ratio: %" PRId32 "\n", angle, ccr_val);
-
-}
-
-uint8_t saturateAngle(int8_t angle)
-{
-  if (angle > SERVO_ANGLE_LIMIT || angle < -SERVO_ANGLE_LIMIT) {
-//    DEBUG_PRINT("[SERVO]: Servo angle out of range! Capping...\n");
-    bool sign = angle > 0;
-    return sign ? SERVO_ANGLE_LIMIT + SERVO_ANGLE_ZERO : -SERVO_ANGLE_LIMIT+SERVO_ANGLE_ZERO;
-  }
-  else {
-    return angle + SERVO_ANGLE_ZERO;
-  }
-
-}
-
-void servoAngleCallBack(void)
-{
-  servoSetAngle(saturateAngle(s_servo_angle));
-}
-
-static const DeckDriver servo_deck = {
-  .vid = 0x00,
-  .pid = 0x00,
-  .name = "bcServo",
-
-  .usedPeriph = 0,
-  .usedGpio = 0,
-  .init = servoInit,
-  .test = 0,
-};
-
-DECK_DRIVER(servo_deck);
-
-PARAM_GROUP_START(servo)
-
-PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, servoInitialized, &isInit)
-PARAM_ADD_WITH_CALLBACK(PARAM_UINT8 , servoAngle, &s_servo_angle, &servoAngleCallBack)
-
-PARAM_GROUP_STOP(servo)

From 2d7ed59b6e790447de5e125aa143a784704f1863 Mon Sep 17 00:00:00 2001
From: matejkarasek <matejkarasek@gmail.com>
Date: Wed, 10 Jan 2024 12:19:10 +0100
Subject: [PATCH 12/17] Fixing typo in motors_def

---
 src/drivers/src/motors_def.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/drivers/src/motors_def.c b/src/drivers/src/motors_def.c
index 93abbb1a99..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
 };
@@ -823,7 +823,7 @@ const MotorPerifDef* servoMapIO2 = &MOTORS_PB5_TIM3_CH2_BRUSHLESS_OD;
 /**
  * Servo mapped to the Bigquad M2 / IO3 port
  */
-const MotorPerifDef* servoMapIO3 = &MOTORS_PB4_TIM2_CH1_BRUSHLESS_OD;
+const MotorPerifDef* servoMapIO3 = &MOTORS_PB4_TIM3_CH1_BRUSHLESS_OD;
 
 /**
  * Servo mapped to the Bigquad M4 / RX2 port

From 296658a76ae54a2a62d3e116abadaddb12c0c583 Mon Sep 17 00:00:00 2001
From: matejkarasek <matejkarasek@gmail.com>
Date: Wed, 17 Jan 2024 19:05:26 +0100
Subject: [PATCH 13/17] Adding debug flag for servo position messages

---
 src/deck/drivers/src/servo.c | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/deck/drivers/src/servo.c b/src/deck/drivers/src/servo.c
index 8f04e2aaa1..b7730c89a6 100644
--- a/src/deck/drivers/src/servo.c
+++ b/src/deck/drivers/src/servo.c
@@ -47,6 +47,8 @@ static uint16_t servo_MAX_us = 2000;
 
 #include "servo.h"
 
+// #define DEBUG_SERVO
+
 static bool isInit = false;
 
 const MotorPerifDef* servoMap;
@@ -164,8 +166,10 @@ void servoSetAngle(uint8_t angle)
   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);
-  DEBUG_PRINT("Set Angle: %u deg, pulse width: %f us \n", angle, pulse_length_us);
-
+  
+  #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)

From 126c952bc5b85e6ce199dc6f4c5f45fd61d042cd Mon Sep 17 00:00:00 2001
From: matejkarasek <matejkarasek@gmail.com>
Date: Wed, 31 Jan 2024 10:28:31 +0100
Subject: [PATCH 14/17] Adding parameter description, moving init parameter to
 the deck group

---
 src/deck/drivers/src/servo.c | 24 +++++++++++++++++++++++-
 1 file changed, 23 insertions(+), 1 deletion(-)

diff --git a/src/deck/drivers/src/servo.c b/src/deck/drivers/src/servo.c
index b7730c89a6..158557c9eb 100644
--- a/src/deck/drivers/src/servo.c
+++ b/src/deck/drivers/src/servo.c
@@ -225,13 +225,35 @@ static const DeckDriver servo_deck = {
 
 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)
 
-PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, servoInitialized, &isInit)
+/**
+ * @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)

From 971eeb201670ef0a9402bcd755a9db565c6f4209 Mon Sep 17 00:00:00 2001
From: Marcus <marcus.c.eliasson@gmail.com>
Date: Tue, 20 Feb 2024 13:18:51 +0100
Subject: [PATCH 15/17] Added crashed state and the ability to recover from it

---
 src/modules/interface/supervisor.h            | 19 +++++++++-
 .../interface/supervisor_state_machine.h      |  3 ++
 src/modules/src/platformservice.c             |  9 +++++
 src/modules/src/supervisor.c                  | 32 +++++++++++++++++
 src/modules/src/supervisor_state_machine.c    | 36 ++++++++++++++++++-
 5 files changed, 97 insertions(+), 2 deletions(-)

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..72c5a8bf7e 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,7 @@ typedef enum {
   supervisorConditionCommanderWdtWarning,
   supervisorConditionCommanderWdtTimeout,
   supervisorConditionEmergencyStop,
+  supervisorConditionIsCrashed,
   supervisorCondition_NrOfConditions,
 } supervisorConditions_t;
 
@@ -61,6 +63,7 @@ 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)
 
 
 // Enum that is used to describe how to combine the bits in the required field
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/supervisor.c b/src/modules/src/supervisor.c
index 2e209829ab..c274c0219e 100644
--- a/src/modules/src/supervisor.c
+++ b/src/modules/src/supervisor.c
@@ -66,6 +66,7 @@ typedef struct {
   bool isFlying;
   bool isTumbled;
   bool isArmingActivated;
+  bool isCrashed;
   uint16_t infoBitfield;
   uint8_t paramEmergencyStop;
 
@@ -115,6 +116,25 @@ bool supervisorIsLocked() {
   return supervisorStateLocked == supervisorMem.state;
 }
 
+bool supervisorIsCrashed() {
+  return supervisorMem.isCrashed;
+}
+
+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;
@@ -226,6 +246,11 @@ static void postTransitionActions(SupervisorMem_t* this, const supervisorState_t
     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) {
     DEBUG_PRINT("Can not fly\n");
@@ -282,6 +307,10 @@ static supervisorConditionBits_t updateAndPopulateConditions(SupervisorMem_t* th
     conditions |= SUPERVISOR_CB_EMERGENCY_STOP;
   }
 
+  if (supervisorIsCrashed()) {
+    conditions |= SUPERVISOR_CB_CRASHED;
+  }
+
   return conditions;
 }
 
@@ -312,6 +341,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) {
diff --git a/src/modules/src/supervisor_state_machine.c b/src/modules/src/supervisor_state_machine.c
index b3cd69ba75..f12c8cbf41 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,7 @@ static const char* const conditionNames[] = {
   "commanderWdtWarning",
   "commanderWdtTimeout",
   "emergencyStop",
+  "isCrashed",
 };
 static_assert(sizeof(conditionNames) / sizeof(conditionNames[0]) == supervisorCondition_NrOfConditions);
 
@@ -162,7 +164,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,
 
@@ -249,6 +260,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 +293,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);
 

From 367572953fcc87158d284b0daca27a0416fb2bb7 Mon Sep 17 00:00:00 2001
From: Rik Bouwmeester <rikjbouwmeester@gmail.com>
Date: Wed, 21 Feb 2024 10:56:59 +0100
Subject: [PATCH 16/17] Start timeout after landing before disarming

Previous behavior: The system would disarm (because of supervisor state reset) immediately upon landing.
New behavior: Upon landing, the system will remain in a landed state, maintaining its armed status and readiness for takeoff. A configurable timeout mechanism is introduced, triggering the supervisor state reset and disarming the system after the specified duration.

The timeout duration is a persistent parameter.
---
 .../interface/supervisor_state_machine.h      |  2 +
 src/modules/src/supervisor.c                  | 47 +++++++++++++++++--
 src/modules/src/supervisor_state_machine.c    | 14 +++++-
 src/platform/interface/platform_defaults.h    |  5 ++
 4 files changed, 62 insertions(+), 6 deletions(-)

diff --git a/src/modules/interface/supervisor_state_machine.h b/src/modules/interface/supervisor_state_machine.h
index 72c5a8bf7e..164284d572 100644
--- a/src/modules/interface/supervisor_state_machine.h
+++ b/src/modules/interface/supervisor_state_machine.h
@@ -50,6 +50,7 @@ typedef enum {
   supervisorConditionCommanderWdtTimeout,
   supervisorConditionEmergencyStop,
   supervisorConditionIsCrashed,
+  supervisorConditionLandingTimeout,
   supervisorCondition_NrOfConditions,
 } supervisorConditions_t;
 
@@ -64,6 +65,7 @@ typedef uint32_t supervisorConditionBits_t;
 #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/supervisor.c b/src/modules/src/supervisor.c
index c274c0219e..804c46457a 100644
--- a/src/modules/src/supervisor.c
+++ b/src/modules/src/supervisor.c
@@ -61,6 +61,8 @@
   #define AUTO_ARMING 0
 #endif
 
+static uint16_t landingTimeoutDuration = LANDING_TIMEOUT_MS;
+
 typedef struct {
   bool canFly;
   bool isFlying;
@@ -79,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
@@ -120,6 +125,19 @@ 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()) {
@@ -235,13 +253,18 @@ 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) {
+    DEBUG_PRINT("Landed, starting disarm timer\n");
+    supervisorSetLatestLandingTime(this, currentTick);
+  }
+
   if (newState == supervisorStateLocked) {
     DEBUG_PRINT("Locked, reboot required\n");
   }
@@ -252,13 +275,14 @@ static void postTransitionActions(SupervisorMem_t* this, const supervisorState_t
   }
 
   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);
   }
 
@@ -311,6 +335,10 @@ static supervisorConditionBits_t updateAndPopulateConditions(SupervisorMem_t* th
     conditions |= SUPERVISOR_CB_CRASHED;
   }
 
+  if (supervisorIsLandingTimeout(this, currentTick)) {
+    conditions |= SUPERVISOR_CB_LANDING_TIMEOUT;
+  }
+
   return conditions;
 }
 
@@ -359,7 +387,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;
@@ -375,6 +403,8 @@ void supervisorOverrideSetpoint(setpoint_t* setpoint) {
   switch(this->state){
     case supervisorStateReadyToFly:
       // Fall through
+    case supervisorStateLanded:
+      // Fall through
     case supervisorStateFlying:
       // Do nothing
       break;
@@ -402,7 +432,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) {
@@ -494,4 +525,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 f12c8cbf41..2ccb2c7e65 100644
--- a/src/modules/src/supervisor_state_machine.c
+++ b/src/modules/src/supervisor_state_machine.c
@@ -59,6 +59,7 @@ static const char* const conditionNames[] = {
   "commanderWdtTimeout",
   "emergencyStop",
   "isCrashed",
+  "landingTimeout",
 };
 static_assert(sizeof(conditionNames) / sizeof(conditionNames[0]) == supervisorCondition_NrOfConditions);
 
@@ -203,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[] = {
diff --git a/src/platform/interface/platform_defaults.h b/src/platform/interface/platform_defaults.h
index 427bc39a47..e85abfb7de 100644
--- a/src/platform/interface/platform_defaults.h
+++ b/src/platform/interface/platform_defaults.h
@@ -125,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

From 76df43b79a675a9716e607ddea80e93404486c3a Mon Sep 17 00:00:00 2001
From: Rik Bouwmeester <rikjbouwmeester@gmail.com>
Date: Wed, 21 Feb 2024 11:43:03 +0100
Subject: [PATCH 17/17] Notify user of landing timeout No longer notify for
 starting the timer.

---
 src/modules/src/supervisor.c | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c
index 804c46457a..66649ca353 100644
--- a/src/modules/src/supervisor.c
+++ b/src/modules/src/supervisor.c
@@ -261,9 +261,12 @@ static void postTransitionActions(SupervisorMem_t* this, const supervisorState_t
   }
 
   if (newState == supervisorStateLanded) {
-    DEBUG_PRINT("Landed, starting disarm timer\n");
     supervisorSetLatestLandingTime(this, currentTick);
   }
+  
+  if ((previousState == supervisorStateLanded) && (newState == supervisorStateReset)) {
+    DEBUG_PRINT("Landing timeout, disarming\n");
+  }
 
   if (newState == supervisorStateLocked) {
     DEBUG_PRINT("Locked, reboot required\n");