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/11] 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/11] 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/11] 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/11] 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/11] 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/11] 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 d448764d0c43951f0d9e5493352086760e5bf1ed Mon Sep 17 00:00:00 2001
From: matejkarasek <matejkarasek@gmail.com>
Date: Tue, 9 Jan 2024 13:32:29 +0100
Subject: [PATCH 07/11] 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 08/11] 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 09/11] 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 10/11] 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 11/11] 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)