diff --git a/src/deck/drivers/src/Kbuild b/src/deck/drivers/src/Kbuild
index 282db6cd71..e6c3f9ad98 100644
--- a/src/deck/drivers/src/Kbuild
+++ b/src/deck/drivers/src/Kbuild
@@ -20,4 +20,5 @@ obj-$(CONFIG_DECK_ZRANGER)       += zranger.o
 obj-$(CONFIG_DECK_ZRANGER2)      += zranger2.o
 # obj-y                            += loadcell_nau7802.o
 # obj-y                            += rpm.o
-# obj-y                            += acs37800.o
\ No newline at end of file
+# obj-y                            += acs37800.o
+obj-y                            += deck_servo.o
\ No newline at end of file
diff --git a/src/deck/drivers/src/deck_electromagnet.c b/src/deck/drivers/src/deck_electromagnet.c
new file mode 100644
index 0000000000..c29ff58271
--- /dev/null
+++ b/src/deck/drivers/src/deck_electromagnet.c
@@ -0,0 +1,78 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2023 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/>.
+ *
+ * deck_electromagnet.c - Deck driver for the electromagnet deck
+ */
+
+#define DEBUG_MODULE "DeckElectromagnet"
+#include "debug.h"
+
+#include "deck.h"
+#include "param.h"
+
+#define GATE_PIN    DECK_GPIO_IO1
+uint8_t state = 0;
+
+static void activateCallback(void);
+
+static void initializeDeckElectromagnet()
+{
+	DEBUG_PRINT("Initializing electromagnet deck!\n");
+
+	// Configure pin IO1 as an output with pull-down resistor
+	pinMode(GATE_PIN, OUTPUT);
+
+	// initialize with given default state
+	activateCallback();
+
+	// Test: Activate electromagnet
+	// digitalWrite(GATE_PIN, HIGH);
+
+	DEBUG_PRINT("Electromagnet deck initialized successfully!\n");
+}
+
+static void activateCallback(void)
+{
+	if (state) {
+		DEBUG_PRINT("Activating electromagnet deck!\n");
+		digitalWrite(GATE_PIN, HIGH);
+	} else {
+		DEBUG_PRINT("Deactivating electromagnet deck!\n");
+		digitalWrite(GATE_PIN, LOW);
+	}
+}
+
+static const DeckDriver driverElectromagnet = {
+	.vid = 0,
+  	.pid = 0,
+	.name = "deckElectromagnet",
+	.usedGpio = DECK_USING_IO_1,
+	.init = initializeDeckElectromagnet,
+};
+
+DECK_DRIVER(driverElectromagnet);
+
+// add a parameter to the deck driver
+PARAM_GROUP_START(electromagnet)
+PARAM_ADD_WITH_CALLBACK(PARAM_UINT8, activate, &state, &activateCallback)
+PARAM_GROUP_STOP(electromagnet)
diff --git a/src/deck/drivers/src/deck_servo.c b/src/deck/drivers/src/deck_servo.c
new file mode 100644
index 0000000000..badf2f506e
--- /dev/null
+++ b/src/deck/drivers/src/deck_servo.c
@@ -0,0 +1,158 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2023 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/>.
+ *
+ * deck_servo.c - Deck driver for the servo deck
+ */
+
+#define DEBUG_MODULE "DeckServo"
+#include "debug.h"
+
+#include "deck.h"
+#include "param.h"
+
+#include "servo.h"
+
+// linear servo ratios 
+// right servo definitions: shaft looking up -> arm at top == 100% length, arm at bottom == 0% length)
+// the following comments for the ratios apply for the right servo only
+#define ratio_500us 	6 	// 0.5 ms pulse at 50 Hz 	-> 100% length (GETS HOT, CRITICAL) -> (CABLES) BOTTOM |-----...----x| TOP (SHAFT)
+#define ratio_700us 	9 	// 0.7 ms pulse at 50 Hz 	-> 95% length (OK, COOL)			-> (CABLES) BOTTOM |-----...---x-| TOP (SHAFT)
+#define ratio_750us 	10 	// 0.75 ms pulse at 50 Hz 	-> 90% length						-> (CABLES) BOTTOM |-----...--x--| TOP (SHAFT)
+#define ratio_850us 	11 	// 0.85 ms pulse at 50 Hz 	-> ...								-> (CABLES) BOTTOM |-----...-----| TOP (SHAFT)
+#define ratio_950us 	12 	// 0.95 ms pulse at 50 Hz 	-> ...								-> (CABLES) BOTTOM |-----...-----| TOP (SHAFT)	
+#define ratio_1000us 	13 	// 1.0 ms pulse at 50 Hz 	-> ...								-> (CABLES) BOTTOM |-----...-----| TOP (SHAFT)
+#define ratio_1500us 	19 	// 1.5 ms pulse at 50 Hz 	-> ...								-> (CABLES) BOTTOM |-----...-----| TOP (SHAFT)
+#define ratio_2000us 	26 	// 2.0 ms pulse at 50 Hz 	-> 20% length						-> (CABLES) BOTTOM |----x...-----| TOP (SHAFT)
+#define ratio_2300us 	29 	// 2.3 ms pulse at 50 Hz 	-> 5% length						-> (CABLES) BOTTOM |-x---...-----| TOP (SHAFT)
+#define ratio_2500us 	32 	// 2.5 ms pulse at 50 Hz 	-> 0% length (OK, COOL)				-> (CABLES) BOTTOM |x----...-----| TOP (SHAFT)
+
+// ratio limits of the linear actuators (top == shaft end)
+#define ratioRightTop 		ratio_700us
+#define ratioRightBottom 	ratio_2300us
+#define ratioLeftTop 		ratio_2300us	// mirrored
+#define ratioLeftBottom 	ratio_850us		// mirrored, but with ratio_700us, the left servo gets crititcally hot
+
+// normal servo info (currently not in use)
+//  500 us -> 190 deg
+// 1000 us -> 135 deg
+// 1500 us -> 100 deg
+// 2000 us ->  60 deg
+// 2500 us ->  10 deg
+
+uint8_t ratio = 0;
+
+uint16_t frequency = 50;
+uint16_t frequencyDefault = 50;
+
+uint8_t state = 0;
+
+static void testServo(void);
+static void activateServo(void);
+static void deactivateServo(void);
+static void setRatioCallback(void);
+static void setFrequencyCallback(void);
+
+static void initializeDeckServo()
+{
+	DEBUG_PRINT("Initializing servo deck!\n");
+
+	// initialize the PWM pin TX1
+    servoInit();
+
+    DEBUG_PRINT("Servo deck initialized successfully!\n");
+
+	// test the servo deck
+	testServo();
+}
+
+static void testServo(void)
+{
+	DEBUG_PRINT("Starting servo deck test!\n");
+
+	servoSetFreq(frequencyDefault);
+	activateServo();  					// CLOSE GRIPPER with right servo
+	deactivateServo(); 					// OPEN GRIPPER with right servo
+
+	DEBUG_PRINT("Ending servo deck test!\n");
+}
+
+static void activateCallback(void)
+{
+	if (state) {
+		activateServo(); 				// CLOSE GRIPPER with right servo
+	} else {
+		deactivateServo(); 				// OPEN GRIPPER with right servo
+	}
+}
+
+static void activateServo()
+{	
+	DEBUG_PRINT("Activating servo!\n");
+
+	servoSetFreq(frequencyDefault);
+	servoSetRatio(ratioRightBottom); 	// CLOSE GRIPPER with right servo
+}
+
+static void deactivateServo()
+{
+	DEBUG_PRINT("Deactivating servo!\n");
+
+	servoSetFreq(frequencyDefault);
+	servoSetRatio(ratioRightTop); 		// OPEN GRIPPER with right servo
+}
+
+static void setRatioCallback(void)
+{	
+	DEBUG_PRINT("Setting servo ratio to %d!\n", ratio);
+
+	servoSetRatio(ratio);
+}
+
+static void setFrequencyCallback(void)
+{
+	DEBUG_PRINT("Setting servo frequency to %d!\n", frequency);
+
+	servoSetFreq(frequency);
+}
+
+static const DeckDriver driverServo = {
+	.vid = 0,
+  	.pid = 0,
+	.name = "deckServo",
+
+	.usedPeriph = DECK_USING_PA2 | DECK_USING_TIMER5,
+	.usedGpio = 0,
+	
+	.init = initializeDeckServo,
+};
+
+DECK_DRIVER(driverServo);
+
+// servo deck parameters
+PARAM_GROUP_START(servo)
+
+PARAM_ADD_WITH_CALLBACK(PARAM_UINT8, activate, &state, &activateCallback)
+PARAM_ADD_WITH_CALLBACK(PARAM_UINT8, setRatio, &ratio, &setRatioCallback)
+PARAM_ADD_WITH_CALLBACK(PARAM_UINT16, setFrequency, &frequency, &setFrequencyCallback)
+
+PARAM_GROUP_STOP(servo)
\ No newline at end of file
diff --git a/src/drivers/interface/servo.h b/src/drivers/interface/servo.h
new file mode 100644
index 0000000000..4f30325028
--- /dev/null
+++ b/src/drivers/interface/servo.h
@@ -0,0 +1,59 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 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 motor driver header file (based on motors.h & piezo.h)
+ *
+ * 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 TX1 as a PWM pin.
+ */
+void servoInit();
+
+/**
+ * Set servo PWM ratio/power.
+ */
+void servoSetRatio(uint8_t ratio);
+
+/**
+ * Set servo PWM frequency in hertz.
+ */
+void servoSetFreq(uint16_t freq);
+
+#endif /* __SERVO_H__ */
diff --git a/src/drivers/src/Kbuild b/src/drivers/src/Kbuild
index 18bc093636..789a928c1e 100644
--- a/src/drivers/src/Kbuild
+++ b/src/drivers/src/Kbuild
@@ -25,3 +25,4 @@ obj-y += vl53l0x.o
 obj-y += vl53l1x.o
 obj-y += watchdog.o
 obj-y += ws2812_cf2.o
+obj-y += servo.o
diff --git a/src/drivers/src/servo.c b/src/drivers/src/servo.c
new file mode 100644
index 0000000000..849fff2252
--- /dev/null
+++ b/src/drivers/src/servo.c
@@ -0,0 +1,127 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 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 motor driver
+ *
+ * This code mainly interfacing the PWM peripheral lib of ST.
+ */
+
+#include <stdbool.h>
+
+/* ST includes */
+#include "stm32fxxx.h"
+
+#include "servo.h"
+
+//FreeRTOS includes
+#include "FreeRTOS.h"
+#include "task.h"
+
+// HW defines
+#define SERVO_TIM_PERIF       RCC_APB1Periph_TIM5
+#define SERVO_TIM             TIM5
+#define SERVO_TIM_DBG         DBGMCU_TIM5_STOP
+#define SERVO_TIM_SETCOMPARE  TIM_SetCompare2
+#define SERVO_TIM_GETCAPTURE  TIM_GetCapture2
+
+#define SERVO_GPIO_POS_PERIF         RCC_AHB1Periph_GPIOA
+#define SERVO_GPIO_POS_PORT          GPIOA
+#define SERVO_GPIO_POS_PIN           GPIO_Pin_2 // TIM5_CH3 OR TX1
+#define SERVO_GPIO_AF_POS_PIN        GPIO_PinSource2
+#define SERVO_GPIO_AF_POS            GPIO_AF_TIM5
+
+#define SERVO_PWM_BITS      (8)
+#define SERVO_PWM_PERIOD    ((1<<SERVO_PWM_BITS) - 1)
+#define SERVO_PWM_PRESCALE  (0)
+
+/* This should be calculated.. */
+#define SERVO_BASE_FREQ (329500)
+
+static bool isInit = false;
+
+/* Public functions */
+
+void servoInit()
+{
+  if (isInit)
+    return;
+
+  //Init structures
+  GPIO_InitTypeDef GPIO_InitStructure;
+  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
+  TIM_OCInitTypeDef  TIM_OCInitStructure;
+
+  //Clock the gpio and the timers
+  RCC_AHB1PeriphClockCmd(SERVO_GPIO_POS_PERIF, ENABLE);
+  RCC_APB1PeriphClockCmd(SERVO_TIM_PERIF, ENABLE);
+
+  // Configure the GPIO for the timer output
+  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;
+  GPIO_InitStructure.GPIO_Pin = SERVO_GPIO_POS_PIN;
+  GPIO_Init(SERVO_GPIO_POS_PORT, &GPIO_InitStructure);
+
+  //Map timers to alternate functions
+  GPIO_PinAFConfig(SERVO_GPIO_POS_PORT, SERVO_GPIO_AF_POS_PIN, SERVO_GPIO_AF_POS);
+
+  //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(SERVO_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
+  TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
+  TIM_OC3Init(SERVO_TIM, &TIM_OCInitStructure);
+  TIM_OC3PreloadConfig(SERVO_TIM, TIM_OCPreload_Enable);
+
+  //Enable the timer PWM outputs
+  TIM_CtrlPWMOutputs(SERVO_TIM, ENABLE);
+  TIM_SetCompare3(SERVO_TIM, 0x00);
+
+  //Enable the timer
+  TIM_Cmd(SERVO_TIM, ENABLE);
+
+  isInit = true;
+}
+
+void servoSetRatio(uint8_t ratio)
+{
+  TIM_SetCompare3(SERVO_TIM, ratio);
+}
+
+void servoSetFreq(uint16_t freq)
+{
+  TIM_PrescalerConfig(SERVO_TIM, (SERVO_BASE_FREQ/freq), TIM_PSCReloadMode_Update);
+}