diff --git a/src/deck/drivers/interface/servo.h b/src/deck/drivers/interface/servo.h
new file mode 100644
index 0000000000..bc58675d09
--- /dev/null
+++ b/src/deck/drivers/interface/servo.h
@@ -0,0 +1,71 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2012 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ *
+ * 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
+#include
+#include "config.h"
+
+/******** Defines ********/
+#define SERVO_PWM_PERIOD 1000 // ARR register content
+#define SERVO_PWM_FREQUENCY_HZ 50 // target servo pwm frequency
+#define SERVO_PWM_PRESCALE (uint16_t) (1680) // 84mhz / (50hz * ARR)
+
+/**
+ * Servo Initialization
+ */
+void servoInit();
+
+bool servoTest(void);
+
+/**
+ *
+ * @brief Set servo angle.
+ * @param: angle: desired servo angle in degrees
+ */
+void servoSetAngle(uint8_t angle);
+
+/**
+ * Servo angle parameter callback. When servo angle is changed, call
+ * function to change servo angle automatically
+ * */
+void servoAngleCallBack(void);
+
+/**
+ * Saturate servo angle. Min is 0, max is defined by parameter 'servoRange'
+ * @param angle: pointer to desired angle
+ * */
+uint8_t saturateAngle(uint8_t angle);
+
+#endif /* __SERVO_H__ */
diff --git a/src/deck/drivers/src/Kbuild b/src/deck/drivers/src/Kbuild
index 79ff5d316b..52765dfab6 100644
--- a/src/deck/drivers/src/Kbuild
+++ b/src/deck/drivers/src/Kbuild
@@ -15,6 +15,7 @@ obj-$(CONFIG_DECK_LOCO) += lpsTdoa3Tag.o
obj-$(CONFIG_DECK_LOCO) += lpsTwrTag.o
obj-$(CONFIG_DECK_MULTIRANGER) += multiranger.o
obj-$(CONFIG_DECK_OA) += oa.o
+obj-$(CONFIG_DECK_SERVO) += servo.o
obj-$(CONFIG_DECK_USD) += usddeck.o
obj-$(CONFIG_DECK_ZRANGER) += zranger.o
obj-$(CONFIG_DECK_ZRANGER2) += zranger2.o
diff --git a/src/deck/drivers/src/Kconfig b/src/deck/drivers/src/Kconfig
index 1db2aa01b4..1c4ee4ff58 100644
--- a/src/deck/drivers/src/Kconfig
+++ b/src/deck/drivers/src/Kconfig
@@ -390,6 +390,37 @@ config DECK_OA
help
Codename: Obstacle Avoidance driver, obsolete deck.
+config DECK_SERVO
+ bool "Support the servo deck (prototype/via BigQuad deck)"
+ default n
+ help
+ Servo PWM driver.
+
+choice
+ prompt "Servo pin"
+ depends on DECK_SERVO
+ default DECK_SERVO_USE_TX2
+
+ config DECK_SERVO_USE_TX2
+ bool "PA2/TX2"
+
+ config DECK_SERVO_USE_RX2
+ bool "PA3/RX2"
+
+ config DECK_SERVO_USE_IO1
+ bool "PB8/IO_1"
+
+ config DECK_SERVO_USE_IO2
+ bool "PB5/IO_2"
+
+ config DECK_SERVO_USE_IO3
+ bool "PB4/IO_3"
+
+ config DECK_SERVO_USE_MOSI
+ bool "PA7/MOSI"
+
+endchoice
+
config DECK_USD
bool "Support the Micro SD card deck"
default y
diff --git a/src/deck/drivers/src/servo.c b/src/deck/drivers/src/servo.c
new file mode 100644
index 0000000000..158557c9eb
--- /dev/null
+++ b/src/deck/drivers/src/servo.c
@@ -0,0 +1,259 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2024 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ *
+ * servo.c - Servo driver
+ *
+ * Using contributions from: Eric Ewing, Will Wu
+ */
+#define DEBUG_MODULE "SERVO"
+
+#include
+
+/* ST includes */
+#include "stm32fxxx.h"
+
+//FreeRTOS includes
+#include "FreeRTOS.h"
+#include "task.h"
+#include "debug.h"
+#include
+#include
+#include "motors.h"
+#include "param.h"
+#include "deck.h"
+
+static uint16_t servo_MIN_us = 1000;
+static uint16_t servo_MAX_us = 2000;
+
+#include "servo.h"
+
+// #define DEBUG_SERVO
+
+static bool isInit = false;
+
+const MotorPerifDef* servoMap;
+extern const MotorPerifDef* servoMapIO1;
+extern const MotorPerifDef* servoMapIO2;
+extern const MotorPerifDef* servoMapIO3;
+extern const MotorPerifDef* servoMapRX2;
+extern const MotorPerifDef* servoMapTX2;
+extern const MotorPerifDef* servoMapMOSI;
+
+/* Public functions */
+static uint8_t servo_idle = 90;
+static uint8_t s_servo_angle;
+static uint8_t servo_range = 180; // in degrees
+
+void servoMapInit(const MotorPerifDef* servoMapSelect)
+{
+ servoMap = servoMapSelect;
+
+ GPIO_InitTypeDef GPIO_InitStructure;
+ TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+ TIM_OCInitTypeDef TIM_OCInitStructure;
+
+ //clock the servo pin and the timers
+ RCC_AHB1PeriphClockCmd(servoMap->gpioPerif, ENABLE);
+ RCC_APB1PeriphClockCmd(servoMap->timPerif, ENABLE);
+
+ //configure gpio for timer out
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+ GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+ GPIO_InitStructure.GPIO_Pin = servoMap->gpioPin;
+ GPIO_Init(servoMap->gpioPort, &GPIO_InitStructure);
+
+ //map timer to alternate function
+ GPIO_PinAFConfig(servoMap->gpioPort, servoMap->gpioPinSource, servoMap->gpioAF);
+
+ //Timer configuration
+ TIM_TimeBaseStructure.TIM_Period = SERVO_PWM_PERIOD;
+ TIM_TimeBaseStructure.TIM_Prescaler = SERVO_PWM_PRESCALE;
+ TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+ TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+ TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
+ TIM_TimeBaseInit(servoMap->tim, &TIM_TimeBaseStructure);
+
+ // PWM channels configuration
+ TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
+ TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+ TIM_OCInitStructure.TIM_Pulse = 0;
+ TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
+ TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
+
+ // Configure OC1
+ servoMap->ocInit(servoMap->tim, &TIM_OCInitStructure);
+ servoMap->preloadConfig(servoMap->tim, TIM_OCPreload_Enable);
+
+
+ //Enable the timer PWM outputs
+ TIM_CtrlPWMOutputs(servoMap->tim, ENABLE);
+ servoMap->setCompare(servoMap->tim, 0x00);
+
+ //Enable the timer
+ TIM_Cmd(servoMap->tim, ENABLE);
+}
+
+void servoInit()
+{
+ if (isInit){
+ return;
+ }
+
+ #ifdef CONFIG_DECK_SERVO_USE_IO1
+ servoMapInit(servoMapIO1);
+ DEBUG_PRINT("Init on IO1 [OK]\n");
+ #elif CONFIG_DECK_SERVO_USE_IO2
+ servoMapInit(servoMapIO2);
+ DEBUG_PRINT("Init on IO2 [OK]\n");
+ #elif CONFIG_DECK_SERVO_USE_IO3
+ servoMapInit(servoMapIO3);
+ DEBUG_PRINT("Init on IO3 [OK]\n");
+ #elif CONFIG_DECK_SERVO_USE_RX2
+ servoMapInit(servoMapRX2);
+ DEBUG_PRINT("Init on RX2 [OK]\n"); // not working on Bolt 1.1...
+ #elif CONFIG_DECK_SERVO_USE_MOSI
+ servoMapInit(servoMapMOSI);
+ DEBUG_PRINT("Init on MOSI [OK]\n");
+ #elif CONFIG_DECK_SERVO_USE_TX2
+ servoMapInit(servoMapTX2);
+ DEBUG_PRINT("Init on TX2 [OK]\n"); // not working on Bolt 1.1...
+ #else
+ isInit = false
+ DEBUG_PRINT("Failed to configure servo pin!\n");
+ return;
+ #endif
+
+ servoSetAngle(saturateAngle(servo_idle));
+
+ s_servo_angle = servo_idle;
+
+ isInit = true;
+}
+
+bool servoTest(void)
+{
+ return isInit;
+}
+
+void servoSetAngle(uint8_t angle)
+{
+ // set CCR register
+ // Duty% = CCR/ARR*100, so CCR = Duty%/100 * ARR
+
+ double pulse_length_us = (double)(angle) / servo_range * (servo_MAX_us - servo_MIN_us) + servo_MIN_us;
+ double pulse_length_s = pulse_length_us / 1000000;
+ const uint32_t ccr_val = (uint32_t)(pulse_length_s * SERVO_PWM_PERIOD * SERVO_PWM_FREQUENCY_HZ);
+ servoMap->setCompare(servoMap->tim, ccr_val);
+
+ #ifdef DEBUG_SERVO
+ DEBUG_PRINT("Set Angle: %u deg, pulse width: %f us \n", angle, pulse_length_us);
+ #endif
+}
+
+uint8_t saturateAngle(uint8_t angle)
+{
+ if (angle > servo_range) {
+ return servo_range;
+ }
+ else if (angle < 0) {
+ return 0;
+ }
+ else {
+ return angle;
+ }
+
+}
+
+void servoAngleCallBack(void)
+{
+ servoSetAngle(saturateAngle(s_servo_angle));
+}
+
+static const DeckDriver servo_deck = {
+ .vid = 0x00,
+ .pid = 0x00,
+ .name = "bcServo",
+
+ #ifdef CONFIG_DECK_SERVO_USE_IO1
+ .usedPeriph = DECK_USING_TIMER4,
+ .usedGpio = DECK_USING_IO_1,
+ #elif CONFIG_DECK_SERVO_USE_IO2
+ .usedPeriph = DECK_USING_TIMER3,
+ .usedGpio = DECK_USING_IO_2,
+ #elif CONFIG_DECK_SERVO_USE_IO3
+ .usedPeriph = DECK_USING_TIMER3,
+ .usedGpio = DECK_USING_IO_3,
+ #elif CONFIG_DECK_SERVO_USE_RX2
+ .usedPeriph = DECK_USING_TIMER5,
+ .usedGpio = DECK_USING_PA3,
+ #elif CONFIG_DECK_SERVO_USE_MOSI
+ .usedPeriph = DECK_USING_TIMER14,
+ .usedGpio = DECK_USING_PA7,
+ #elif CONFIG_DECK_SERVO_USE_TX2
+ .usedPeriph = DECK_USING_TIMER5,
+ .usedGpio = DECK_USING_PA2,
+ #else
+ .usedPeriph = 0,
+ .usedGpio = 0,
+ #endif
+
+ .init = servoInit,
+ .test = servoTest,
+};
+
+DECK_DRIVER(servo_deck);
+
+PARAM_GROUP_START(deck)
+
+PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcServo, &isInit)
+PARAM_GROUP_STOP(deck)
+
+/**
+ * "Servo" deck parameters
+ */
+PARAM_GROUP_START(servo)
+
+/**
+ * @brief PWM pulse width for minimal servo position (in microseconds)
+ */
+PARAM_ADD(PARAM_UINT16 | PARAM_PERSISTENT, servoMINus, &servo_MIN_us)
+/**
+ * @brief PWM pulse width for maximal servo position (in microseconds)
+ */
+PARAM_ADD(PARAM_UINT16 | PARAM_PERSISTENT, servoMAXus, &servo_MAX_us)
+/**
+ * @brief Servo range, i.e. angle between the min and max positions (in degrees)
+ */
+PARAM_ADD(PARAM_UINT8 | PARAM_PERSISTENT, servoRange, &servo_range)
+/**
+ * @brief Servo idle (startup) angular position (in degrees, min = 0, max = servoRange)
+ */
+PARAM_ADD(PARAM_UINT8 | PARAM_PERSISTENT, servoIdle, &servo_idle)
+/**
+ * @brief Servo angular position (in degrees, min = 0, max = servoRange)
+ */
+PARAM_ADD_WITH_CALLBACK(PARAM_UINT8 , servoAngle, &s_servo_angle, &servoAngleCallBack)
+
+PARAM_GROUP_STOP(servo)
diff --git a/src/deck/interface/deck_core.h b/src/deck/interface/deck_core.h
index c13deaccd5..47dc23d867 100644
--- a/src/deck/interface/deck_core.h
+++ b/src/deck/interface/deck_core.h
@@ -69,6 +69,7 @@ bool deckTest(void);
#define DECK_USING_TIMER10 (1 << 16)
#define DECK_USING_TIMER14 (1 << 15)
#define DECK_USING_TIMER9 (1 << 17)
+#define DECK_USING_TIMER4 (1 << 18)
struct deckInfo_s;
struct deckFwUpdate_s;
diff --git a/src/drivers/src/motors_def.c b/src/drivers/src/motors_def.c
index 0e334efdcd..d9ef9658c1 100644
--- a/src/drivers/src/motors_def.c
+++ b/src/drivers/src/motors_def.c
@@ -623,7 +623,7 @@ static const MotorPerifDef MOTORS_PB5_TIM3_CH2_BRUSHLESS_OD =
};
// Deck IO3, PB4, TIM3_CH1
-static const MotorPerifDef MOTORS_PB4_TIM2_CH1_BRUSHLESS_OD =
+static const MotorPerifDef MOTORS_PB4_TIM3_CH1_BRUSHLESS_OD =
{
.drvType = BRUSHLESS,
.gpioPerif = RCC_AHB1Periph_GPIOB,
@@ -743,7 +743,7 @@ const MotorPerifDef* motorMapDefaultBrushed[NBR_OF_MOTORS] =
const MotorPerifDef* motorMapBigQuadDeck[NBR_OF_MOTORS] =
{
&MOTORS_PA2_TIM2_CH3_BRUSHLESS_OD,
- &MOTORS_PB4_TIM2_CH1_BRUSHLESS_OD,
+ &MOTORS_PB4_TIM3_CH1_BRUSHLESS_OD,
&MOTORS_PB5_TIM3_CH2_BRUSHLESS_OD,
&MOTORS_PA3_TIM2_CH4_BRUSHLESS_OD
};
@@ -805,3 +805,32 @@ const MotorPerifDef* motorMapCF21Brushless[NBR_OF_MOTORS] =
&MOTORS_PB10_TIM2_CH3_BRUSHLESS_OD
};
+/**
+ * Servo mapped to the Bigquad CPPM (MOSI) port
+ */
+const MotorPerifDef* servoMapMOSI = &MOTORS_PA7_TIM14_CH1_BRUSHLESS_OD;
+
+/**
+ * Servo mapped to the Bigquad M1 / TX2 port
+ */
+const MotorPerifDef* servoMapTX2 = &MOTORS_PA2_TIM5_CH3_BRUSHLESS_OD;
+
+/**
+ * Servo mapped to the Bigquad M3 / IO2 port
+ */
+const MotorPerifDef* servoMapIO2 = &MOTORS_PB5_TIM3_CH2_BRUSHLESS_OD;
+
+/**
+ * Servo mapped to the Bigquad M2 / IO3 port
+ */
+const MotorPerifDef* servoMapIO3 = &MOTORS_PB4_TIM3_CH1_BRUSHLESS_OD;
+
+/**
+ * Servo mapped to the Bigquad M4 / RX2 port
+ */
+const MotorPerifDef* servoMapRX2 = &MOTORS_PA3_TIM5_CH4_BRUSHLESS_OD;
+
+/**
+ * Servo mapped to IO1 port
+ */
+const MotorPerifDef* servoMapIO1 = &MOTORS_PB8_TIM4_CH3_BRUSHLESS_OD;