SerialServer.cpp

00001 /*
00002  * SerialServer.cpp
00003  * Copyright (c) 2018, ZHAW
00004  * All rights reserved.
00005  */
00006 
00007 #include <vector>
00008 #include "SerialServer.h"
00009 
00010 using namespace std;
00011 
00012 inline string float2string(float f) {
00013 
00014     char buffer[32];
00015     sprintf(buffer, "%.3f", f);
00016     
00017     return string(buffer);
00018 }
00019 
00020 inline string int2string(int i) {
00021     
00022     char buffer[32];
00023     sprintf(buffer, "%d", i);
00024     
00025     return string(buffer);
00026 }
00027 
00028 const float SerialServer::PERIOD = 0.001f; // period of transmit task, given in [s]
00029 const char SerialServer::INT_TO_CHAR[] = {0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46};
00030 
00031 /**
00032  * Creates a serial server object.
00033  */
00034 SerialServer::SerialServer(RawSerial& serial, LIDAR& lidar, Controller& controller) : serial(serial), lidar(lidar), controller(controller) {
00035     
00036     input.clear();
00037     output.clear();
00038     
00039     serial.attach(callback(this, &SerialServer::receive), Serial::RxIrq);
00040     //serial.attach(callback(this, &SerialServer::transmit), Serial::TxIrq);
00041     ticker.attach(callback(this, &SerialServer::transmit), PERIOD);
00042 }
00043 
00044 /**
00045  * Deletes the serial server object.
00046  */
00047 SerialServer::~SerialServer() {}
00048 
00049 /**
00050  * Callback method of serial interface.
00051  */
00052 void SerialServer::receive() {
00053     
00054     // read received characters while input buffer is full
00055     
00056     while (serial.readable()) {
00057         int c = serial.getc();
00058         if (input.size() < BUFFER_SIZE) input += (char)c;
00059     }
00060     
00061     // check if input is complete (terminated with CR & LF)
00062     
00063     if (input.find("\r\n") != string::npos) {
00064 
00065         // parse request
00066         
00067         string request = input.substr(0, input.find("\r\n"));
00068         string name;
00069         vector<string> values;
00070         
00071         if (request.find(' ') != string::npos) {
00072 
00073             name = request.substr(0, request.find(' '));
00074             request = request.substr(request.find(' ')+1);
00075 
00076             while (request.find(' ') != string::npos) {
00077                 values.push_back(request.substr(0, request.find(' ')));
00078                 request = request.substr(request.find(' ')+1);
00079             }
00080             values.push_back(request);
00081 
00082         } else {
00083             
00084             name = request;
00085         }
00086         
00087         input.clear();
00088         
00089         // process request
00090         
00091         if (name.compare("getDistance") == 0) {
00092             short angle = atoi(values[0].c_str());
00093             short distance = lidar.getDistance(angle);
00094             output = "distance ";
00095             for (int i = 0; i < 4; i++) output += INT_TO_CHAR[(distance >> (4*(3-i))) & 0x0F];
00096             output += "\r\n";
00097         } else if (name.compare("getBeacon") == 0) {
00098             short angle = lidar.getAngleOfBeacon();
00099             short distance = lidar.getDistanceOfBeacon();
00100             output = "beacon ";
00101             for (int i = 0; i < 4; i++) output += INT_TO_CHAR[(angle >> (4*(3-i))) & 0x0F];
00102             output += " ";
00103             for (int i = 0; i < 4; i++) output += INT_TO_CHAR[(distance >> (4*(3-i))) & 0x0F];
00104             output += "\r\n";
00105         } else if (name.compare("getRobotPose") == 0) {
00106             float x = controller.getX();
00107             float y = controller.getY();
00108             float alpha = controller.getAlpha();
00109             output = "pose "+float2string(x)+" "+float2string(y)+" "+float2string(alpha)+"\r\n";
00110         } else if (name.compare("getOrientation") == 0) {
00111             float alpha = controller.getAlpha();
00112             output = "orientation "+float2string(alpha)+"\r\n";
00113         } else {
00114             output = "request unknown\r\n";
00115         }
00116         
00117         // transmit first byte of output buffer
00118         
00119         if (serial.writeable() && (output.size() > 0)) {
00120             serial.putc(output[0]);
00121             output.erase(0, 1);
00122         }
00123         
00124     } else if (input.size() >= BUFFER_SIZE) {
00125         
00126         input.clear();
00127     }
00128 }
00129 
00130 /**
00131  * Callback method of serial interface.
00132  */
00133 void SerialServer::transmit() {
00134     
00135     // transmit output
00136     
00137     while (serial.writeable() && (output.size() > 0)) {
00138         serial.putc(output[0]);
00139         output.erase(0, 1);
00140     }
00141 }
00142