diff --git a/crazyflie_groundstation/inc/CCrazyflie.h b/crazyflie_groundstation/inc/CCrazyflie.h
index 8530f364d6bc3a04fd300b184a036cacf757e0bb..f8098107fcc88cb280897f679d15cfa26091e21d 100644
--- a/crazyflie_groundstation/inc/CCrazyflie.h
+++ b/crazyflie_groundstation/inc/CCrazyflie.h
@@ -52,6 +52,7 @@
#include "serial/serial.h"
#include "vrpn_Connection.h"
#include "vrpn_Tracker.h"
+#include "testStand.h"
// Ports defined for the CRTP protocol
@@ -298,7 +299,7 @@ class CCrazyflie {
CTOC *m_tocParameters;
CTOC *m_tocLogs;
int testStandError = 0;
- serial::Serial * test_stand;
+ TestStand testStand;
bool allowLogging = false;
CCrazyflie(CCrazyRadio *crRadio, int radioChannel, XferRate dataRate, int quadNum, double startTime);
diff --git a/crazyflie_groundstation/inc/testStand.h b/crazyflie_groundstation/inc/testStand.h
new file mode 100644
index 0000000000000000000000000000000000000000..8cef16215609c5272e9413b2d0d2d607e8b18df2
--- /dev/null
+++ b/crazyflie_groundstation/inc/testStand.h
@@ -0,0 +1,43 @@
+#ifndef TESTSTAND_H_
+#define TESTSTAND_H_
+
+#include "serial/serial.h"
+
+
+class TestStand {
+
+ public:
+ TestStand(serial::Serial * ptr);
+
+ void setSerialInterface(serial::Serial * ptr);
+ void zeroHeading();
+ void startStream();
+ void stopStream();
+ void setCycleDelay(int period);
+ void toggleMode();
+ void setModePosition();
+ void setModeRate();
+ void setupTestStand(int deviceMode);
+
+ std::string getCurrentValue();
+ std::string getCurrentPosition();
+ std::string getCurrentRate();
+ std::string getCurrentADCValue();
+
+ private:
+ serial::Serial *serialInterface;
+
+ const uint8_t ZERO_HEADING = '@h';
+ const uint8_t MODE_TOGGLE = '@t';
+ const uint8_t GET_VALUE = '@v';
+ const uint8_t START_STREAM = '@s';
+ const uint8_t STOP_STREAM = '@d';
+ const uint8_t MODE_POSITION = '@p';
+ const uint8_t MODE_RATE = '@r';
+ const uint8_t GET_POSITION = '@P';
+ const uint8_t GET_RATE = '@R';
+ const uint8_t GET_ADC = '@x';
+
+};
+
+#endif
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);
diff --git a/crazyflie_groundstation/src/crazyflieGroundStation.cpp b/crazyflie_groundstation/src/crazyflieGroundStation.cpp
index f0754b35ab28874da6902b47e496201ef52ac67a..c83fdcfb1618e02bcf5d309f5942c5fc378bb8c7 100644
--- a/crazyflie_groundstation/src/crazyflieGroundStation.cpp
+++ b/crazyflie_groundstation/src/crazyflieGroundStation.cpp
@@ -203,10 +203,10 @@ int main(int argc, char** argv) {
if(selectedPort.compare("n/a")) {
serial::Serial * test_stand_ptr = new serial::Serial(selectedPort, 9600);
test_stand_ptr->setTimeout(0, 200, 0, 200, 0);
- crazyflie_info[i].cflieCopter->setTestStand(test_stand_ptr);
+ crazyflie_info[i].cflieCopter->testStand.setSerialInterface(test_stand_ptr);
}
else {
- crazyflie_info[i].cflieCopter->setTestStand(NULL);
+ crazyflie_info[i].cflieCopter->testStand.setSerialInterface(NULL);
}
// Check the controller type
diff --git a/crazyflie_groundstation/src/testStand.cpp b/crazyflie_groundstation/src/testStand.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..63ab8c1039d61e981ed65e24a3050aebbc970461
--- /dev/null
+++ b/crazyflie_groundstation/src/testStand.cpp
@@ -0,0 +1,131 @@
+#include "testStand.h"
+
+/**
+ * @brief Construct a new Test Stand:: Test Stand object
+ *
+ * @param ptr pointer to serial object for writing and reading data
+ */
+TestStand::TestStand(serial::Serial * ptr){
+ setSerialInterface(ptr);
+}
+
+/**
+ * @brief Helper function to set member variable for serial interface ptr
+ *
+ * @param ptr pointer to serial object for writing and reading data
+ */
+void TestStand::setSerialInterface(serial::Serial * ptr){
+ this->serialInterface = ptr;
+}
+
+/**
+ * @brief Zero the heading on the test stand
+ *
+ */
+void TestStand::zeroHeading(){
+ this->serialInterface->write(&ZERO_HEADING, 1);
+}
+
+/**
+ * @brief Starts the streaming mode where the test stand write data nonstop
+ *
+ */
+void TestStand::startStream(){
+ this->serialInterface->write(&START_STREAM, 1);
+}
+
+/**
+ * @brief Stops the streaming mode
+ *
+ */
+void TestStand::stopStream(){
+ this->serialInterface->write(&STOP_STREAM, 1);
+}
+
+/**
+ * @brief Sets the time between samples for streaming mode
+ *
+ * @param period time between samples in microseconds
+ */
+void TestStand::setCycleDelay(int period){
+ uint8_t delayString = ('@w', std::to_string(period), '!');
+ this->serialInterface->write(&delayString, 6);
+}
+
+/**
+ * @brief Toggles between Position Mode and Rate Mode
+ *
+ */
+void TestStand::toggleMode(){
+ this->serialInterface->write(&MODE_TOGGLE, 1);
+}
+
+/**
+ * @brief Sets the Test Stand to Position Mode
+ *
+ */
+void TestStand::setModePosition(){
+ this->serialInterface->write(&MODE_POSITION, 1);
+}
+
+/**
+ * @brief Sets the Test Stand to Rate Mode
+ *
+ */
+void TestStand::setModeRate(){
+ this->serialInterface->write(&MODE_RATE, 1);
+}
+
+/**
+ * @brief Gets the current value from the Test Stand
+ *
+ * @return std::string string containing sample from Test Stand
+ */
+std::string TestStand::getCurrentValue(){
+ this->serialInterface->write(&GET_VALUE, 1);
+ return this->serialInterface->readline();
+}
+
+/**
+ * @brief Gets the current value from the Test Stand as a Position
+ *
+ * @return std::string string containing sample from Test Stand as Position
+ */
+std::string TestStand::getCurrentPosition(){
+ this->serialInterface->write(&GET_POSITION, 1);
+ return this->serialInterface->readline();
+}
+
+/**
+ * @brief Gets the current value from the Test Stand as a Rate
+ *
+ * @return std::string string containing sample from Test Stand as Rate
+ */
+std::string TestStand::getCurrentRate(){
+ this->serialInterface->write(&GET_RATE, 1);
+ return this->serialInterface->readline();
+}
+
+/**
+ * @brief Gets the current value from the ADC on the Test Stand
+ *
+ * @return std::string string containing raw sample from ADC
+ */
+std::string TestStand::getCurrentADCValue(){
+ this->serialInterface->write(&GET_ADC, 1);
+ return this->serialInterface->readline();
+}
+
+/**
+ * @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();
+ }
+}