File content as of revision 5:957580f33e52:

/*
 * 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);
    }
}