Updates reading/startup commands for TestStand

See Issue #145

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);

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);