diff --git a/crazyflie_groundstation/src/ccrazyflie/CCrazyflie.cpp b/crazyflie_groundstation/src/ccrazyflie/CCrazyflie.cpp
index 2b32bfc1ce2e390b725b266f1277063e35acfdfb..49d8cbae5d1ab5c6d739fe607ac7b6f6398b82de 100644
--- a/crazyflie_groundstation/src/ccrazyflie/CCrazyflie.cpp
+++ b/crazyflie_groundstation/src/ccrazyflie/CCrazyflie.cpp
@@ -425,6 +425,7 @@ enum State CCrazyflie::getCurrentState() {
 
 void* CCrazyflie::startTestStand(void* args) {
 	CCrazyflie *quad = (CCrazyflie*) args;
+	quad->testStand.setupTestStand(0);
 	quad->testStandLoop();
 
 	return(NULL);
diff --git a/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_loggingFuncs.cpp b/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_loggingFuncs.cpp
index ce8b7309bdfb870283868f0db99b26aa68824987..faeab6ce038bd424dceae868b1427170420f24e4 100644
--- a/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_loggingFuncs.cpp
+++ b/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_loggingFuncs.cpp
@@ -254,9 +254,8 @@ double CCrazyflie::readTestStand() {
 	// this->test_stand->readline();
 	// result = this->test_stand->readline();
 	// this->test_stand->flushInput();
-	this->test_stand->write(&reqSer, 1);
-	result = this->test_stand->readline();
-	
+	result = this->testStand.getCurrentValue();
+
 	double value;
 	try {
 		value = std::stod(result);