File content as of revision 6:67263dc2c2cf:

/*
 * 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]

/**
 * Creates a serial server object.
 */
SerialServer::SerialServer(Serial& serial, IMU& imu, Controller& controller) : serial(serial), imu(imu), 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("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 heading = imu.readHeading();
            float alpha = controller.getAlpha();
            output = "orientation "+float2string(heading)+" "+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);
    }
}