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 
00030 /**
00031  * Creates a serial server object.
00032  */
00033 SerialServer::SerialServer(Serial& serial, IMU& imu, Controller& controller) : serial(serial), imu(imu), controller(controller) {
00034     
00035     input.clear();
00036     output.clear();
00037     
00038     serial.attach(callback(this, &SerialServer::receive), Serial::RxIrq);
00039     //serial.attach(callback(this, &SerialServer::transmit), Serial::TxIrq);
00040     ticker.attach(callback(this, &SerialServer::transmit), PERIOD);
00041 }
00042 
00043 /**
00044  * Deletes the serial server object.
00045  */
00046 SerialServer::~SerialServer() {}
00047 
00048 /**
00049  * Callback method of serial interface.
00050  */
00051 void SerialServer::receive() {
00052     
00053     // read received characters while input buffer is full
00054     
00055     while (serial.readable()) {
00056         int c = serial.getc();
00057         if (input.size() < BUFFER_SIZE) input += (char)c;
00058     }
00059     
00060     // check if input is complete (terminated with CR & LF)
00061     
00062     if (input.find("\r\n") != string::npos) {
00063 
00064         // parse request
00065         
00066         string request = input.substr(0, input.find("\r\n"));
00067         string name;
00068         vector<string> values;
00069         
00070         if (request.find(' ') != string::npos) {
00071 
00072             name = request.substr(0, request.find(' '));
00073             request = request.substr(request.find(' ')+1);
00074 
00075             while (request.find(' ') != string::npos) {
00076                 values.push_back(request.substr(0, request.find(' ')));
00077                 request = request.substr(request.find(' ')+1);
00078             }
00079             values.push_back(request);
00080 
00081         } else {
00082             
00083             name = request;
00084         }
00085         
00086         input.clear();
00087         
00088         // process request
00089         
00090         if (name.compare("getRobotPose") == 0) {
00091             float x = controller.getX();
00092             float y = controller.getY();
00093             float alpha = controller.getAlpha();
00094             output = "pose "+float2string(x)+" "+float2string(y)+" "+float2string(alpha)+"\r\n";
00095         } else if (name.compare("getOrientation") == 0) {
00096             float heading = imu.readHeading();
00097             float alpha = controller.getAlpha();
00098             output = "orientation "+float2string(heading)+" "+float2string(alpha)+"\r\n";
00099         } else {
00100             output = "request unknown\r\n";
00101         }
00102         
00103         // transmit first byte of output buffer
00104         
00105         if (serial.writeable() && (output.size() > 0)) {
00106             serial.putc(output[0]);
00107             output.erase(0, 1);
00108         }
00109         
00110     } else if (input.size() >= BUFFER_SIZE) {
00111         
00112         input.clear();
00113     }
00114 }
00115 
00116 /**
00117  * Callback method of serial interface.
00118  */
00119 void SerialServer::transmit() {
00120     
00121     // transmit output
00122     
00123     while (serial.writeable() && (output.size() > 0)) {
00124         serial.putc(output[0]);
00125         output.erase(0, 1);
00126     }
00127 }
00128