From d7c22b4a6661600dabeb6a9bab845319c8f26ff4 Mon Sep 17 00:00:00 2001
From: Rahul Menon <rmeno12@utexas.edu>
Date: Wed, 11 Oct 2023 10:48:31 -0500
Subject: [PATCH] basic serial setup

---
 CMakeLists.txt                       |  3 ++-
 src/radio_driver/radio_interface.cpp | 37 ++++++++++++++--------------
 src/radio_driver/radio_interface.h   |  8 ++++--
 3 files changed, 27 insertions(+), 21 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index b101102..560f571 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -91,7 +91,8 @@ IF(${CMAKE_BUILD_MODE} MATCHES "Omega")
   ROSBUILD_ADD_EXECUTABLE(radio_driver
     src/radio_driver/radio_driver_node.cpp
     src/radio_driver/radio_driver.cpp
-    src/radio_driver/radio_interface.cpp)
+    src/radio_driver/radio_interface.cpp
+    src/vesc_driver/serial.cc)
   TARGET_LINK_LIBRARIES(radio_driver ${libs})
 ENDIF()
 
diff --git a/src/radio_driver/radio_interface.cpp b/src/radio_driver/radio_interface.cpp
index 325d309..0567e81 100644
--- a/src/radio_driver/radio_interface.cpp
+++ b/src/radio_driver/radio_interface.cpp
@@ -4,17 +4,11 @@
 
 namespace radio_driver {
 
-RadioInterface::RadioInterface() : connected_(false) {
+RadioInterface::RadioInterface() : buffer_size_(0), connected_(false) {
     buffer_ = new char[12];  // TODO: check if 12 is enough
 }
 
-RadioInterface::~RadioInterface() {
-    if (!disconnect()) {
-        LOG(ERROR) << "Failed to cleanly disconnect from radio on shutdown";
-    }
-
-    delete[] buffer_;
-}
+RadioInterface::~RadioInterface() { delete[] buffer_; }
 
 bool RadioInterface::isConnected() const { return connected_; }
 
@@ -25,20 +19,25 @@ bool RadioInterface::sendControl(float throttle, float steering) {
     }
 
     commandToBuffer(throttle, steering);
-    // TODO: send buf to serial device
-    return true;
+    if (buffer_size_) {
+        serial_.write(buffer_, buffer_size_);
+        return true;
+    } else {
+        return false;
+    }
 }
 
-bool RadioInterface::connect(const std::string& port) {
-    // TODO: actually connect to serial device
-    connected_ = true;
-    return true;
+bool RadioInterface::connect(const std::string& port, int baud) {
+    connected_ = serial_.open(port.c_str(), baud);
+    if (!connected_) {
+        LOG(ERROR) << "Failed to connect to radio on serial port " << port;
+    }
+    return connected_;
 }
 
-bool RadioInterface::disconnect() {
-    // TODO: actually disconnect from serial device
+void RadioInterface::disconnect() {
+    serial_.close();
     connected_ = false;
-    return true;
 }
 
 void RadioInterface::commandToBuffer(float throttle, float steering) {
@@ -56,7 +55,9 @@ void RadioInterface::commandToBuffer(float throttle, float steering) {
     } else {
         std::ostringstream ss;
         ss << throttle_pwm << "," << steering_pwm << "\n";
-        ss.str().copy(buffer_, ss.str().size());
+        std::string data = ss.str();
+        data.copy(buffer_, data.size());
+        buffer_size_ = data.size();
     }
 }
 
diff --git a/src/radio_driver/radio_interface.h b/src/radio_driver/radio_interface.h
index 4a7ac99..b8b1286 100644
--- a/src/radio_driver/radio_interface.h
+++ b/src/radio_driver/radio_interface.h
@@ -1,5 +1,7 @@
 #include <string>
 
+#include "vesc_driver/serial.h"
+
 namespace radio_driver {
 
 class RadioInterface {
@@ -15,14 +17,16 @@ class RadioInterface {
     bool sendControl(float throttle, float steering);
 
    private:
-    bool connect(const std::string& port);
-    bool disconnect();
+    bool connect(const std::string& port, int baud);
+    void disconnect();
 
     // convert throttle and steering commands to ascii bytes in the format arduino expects
     // throttle and steering are in the range [-1, 1]
     void commandToBuffer(float throttle, float steering);
 
+    Serial serial_;
     char* buffer_;
+    size_t buffer_size_;
     bool connected_;
 };