diff --git a/crazyflie_groundstation/inc/testStand.h b/crazyflie_groundstation/inc/testStand.h
index 625703cb6fb82147e6ba33fbbf3dead2bc2c49b5..8cef16215609c5272e9413b2d0d2d607e8b18df2 100644
--- a/crazyflie_groundstation/inc/testStand.h
+++ b/crazyflie_groundstation/inc/testStand.h
@@ -17,6 +17,7 @@ class TestStand {
         void toggleMode();
         void setModePosition();
         void setModeRate();
+        void setupTestStand(int deviceMode);
 
         std::string getCurrentValue();
         std::string getCurrentPosition();
diff --git a/crazyflie_groundstation/src/testStand.cpp b/crazyflie_groundstation/src/testStand.cpp
index 9ae0ef63b7631c54e7aca12070084f2d4c2ef677..63ab8c1039d61e981ed65e24a3050aebbc970461 100644
--- a/crazyflie_groundstation/src/testStand.cpp
+++ b/crazyflie_groundstation/src/testStand.cpp
@@ -114,4 +114,18 @@ std::string TestStand::getCurrentRate(){
 std::string TestStand::getCurrentADCValue(){
     this->serialInterface->write(&GET_ADC, 1);
     return this->serialInterface->readline();
-}
\ No newline at end of file
+}
+
+/**
+ * @brief Sets up Test Stand for opertation mode
+ *
+ * @param deviceMode 0 for Rate Mode | 1 for Position Mode (default)
+ */
+void TestStand::setupTestStand(int deviceMode){
+    if(deviceMode == 0){
+        setModeRate();
+    }else{
+        zeroHeading();
+        setModePosition();
+    }
+}