Diff: SerialServer.cpp

Revision:

0:351a2fb21235

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SerialServer.cpp	Fri May 18 12:05:32 2018 +0000
@@ -0,0 +1,142 @@
+/*
+ * SerialServer.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include <vector>
+#include "SerialServer.h"
+
+using namespace std;
+
+inline string float2string(float f) {
+
+    char buffer[32];
+    sprintf(buffer, "%.3f", f);
+    
+    return string(buffer);
+}
+
+inline string int2string(int i) {
+    
+    char buffer[32];
+    sprintf(buffer, "%d", i);
+    
+    return string(buffer);
+}
+
+const float SerialServer::PERIOD = 0.001f; // period of transmit task, given in [s]
+const char SerialServer::INT_TO_CHAR[] = {0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46};
+
+/**
+ * Creates a serial server object.
+ */
+SerialServer::SerialServer(RawSerial& serial, LIDAR& lidar, Controller& controller) : serial(serial), lidar(lidar), controller(controller) {
+    
+    input.clear();
+    output.clear();
+    
+    serial.attach(callback(this, &SerialServer::receive), Serial::RxIrq);
+    //serial.attach(callback(this, &SerialServer::transmit), Serial::TxIrq);
+    ticker.attach(callback(this, &SerialServer::transmit), PERIOD);
+}
+
+/**
+ * Deletes the serial server object.
+ */
+SerialServer::~SerialServer() {}
+
+/**
+ * Callback method of serial interface.
+ */
+void SerialServer::receive() {
+    
+    // read received characters while input buffer is full
+    
+    while (serial.readable()) {
+        int c = serial.getc();
+        if (input.size() < BUFFER_SIZE) input += (char)c;
+    }
+    
+    // check if input is complete (terminated with CR & LF)
+    
+    if (input.find("\r\n") != string::npos) {
+
+        // parse request
+        
+        string request = input.substr(0, input.find("\r\n"));
+        string name;
+        vector<string> values;
+        
+        if (request.find(' ') != string::npos) {
+
+            name = request.substr(0, request.find(' '));
+            request = request.substr(request.find(' ')+1);
+
+            while (request.find(' ') != string::npos) {
+                values.push_back(request.substr(0, request.find(' ')));
+                request = request.substr(request.find(' ')+1);
+            }
+            values.push_back(request);
+
+        } else {
+            
+            name = request;
+        }
+        
+        input.clear();
+        
+        // process request
+        
+        if (name.compare("getDistance") == 0) {
+            short angle = atoi(values[0].c_str());
+            short distance = lidar.getDistance(angle);
+            output = "distance ";
+            for (int i = 0; i < 4; i++) output += INT_TO_CHAR[(distance >> (4*(3-i))) & 0x0F];
+            output += "\r\n";
+        } else if (name.compare("getBeacon") == 0) {
+            short angle = lidar.getAngleOfBeacon();
+            short distance = lidar.getDistanceOfBeacon();
+            output = "beacon ";
+            for (int i = 0; i < 4; i++) output += INT_TO_CHAR[(angle >> (4*(3-i))) & 0x0F];
+            output += " ";
+            for (int i = 0; i < 4; i++) output += INT_TO_CHAR[(distance >> (4*(3-i))) & 0x0F];
+            output += "\r\n";
+        } else if (name.compare("getRobotPose") == 0) {
+            float x = controller.getX();
+            float y = controller.getY();
+            float alpha = controller.getAlpha();
+            output = "pose "+float2string(x)+" "+float2string(y)+" "+float2string(alpha)+"\r\n";
+        } else if (name.compare("getOrientation") == 0) {
+            float alpha = controller.getAlpha();
+            output = "orientation "+float2string(alpha)+"\r\n";
+        } else {
+            output = "request unknown\r\n";
+        }
+        
+        // transmit first byte of output buffer
+        
+        if (serial.writeable() && (output.size() > 0)) {
+            serial.putc(output[0]);
+            output.erase(0, 1);
+        }
+        
+    } else if (input.size() >= BUFFER_SIZE) {
+        
+        input.clear();
+    }
+}
+
+/**
+ * Callback method of serial interface.
+ */
+void SerialServer::transmit() {
+    
+    // transmit output
+    
+    while (serial.writeable() && (output.size() > 0)) {
+        serial.putc(output[0]);
+        output.erase(0, 1);
+    }
+}
+