File content as of revision 0:4801795a9073:

#include "goto_serial.hpp"

namespace ARRC{

gotoSerial::gotoSerial(PinName tx,PinName rx,int baudrate):ser(tx,rx,baudrate),sub_vars_size(0),sub_vars(0),buf(256){
    ser.attach(callback(this,&gotoSerial::interrupt_read),Serial::RxIrq);
}
bool gotoSerial::publish(unsigned id,int num,unsigned type){
    Pack byte;
    byte.integer = num;
    int8_t _1_byte = (byte.integer >> 24) & 0xFF;
    uint8_t _2_byte = (byte.integer >> 16) & 0xFF;
    uint8_t _3_byte = (byte.integer >> 8) & 0xFF;
    uint8_t _4_byte = (byte.integer >> 0) & 0xFF;
    int8_t sum = id+type+_1_byte+_2_byte+_3_byte+_4_byte;
    int32_t numsum = (_1_byte << 24) + (_2_byte<<16) + (_3_byte << 8) + _4_byte;
    ser.putc(STARTDATA);
    ser.putc(id);
    ser.putc(type);
    ser.putc(_1_byte);
    ser.putc(_2_byte);
    ser.putc(_3_byte);
    ser.putc(_4_byte);
    ser.putc(sum);
    ser.putc(ENDDATA);
    while(!ser.writeable());
    return true;
}
bool gotoSerial::publish(unsigned id,float num,unsigned type){
    Pack byte;
    byte.decimal = num;
    uint8_t _1_byte = (byte.integer >> 24) & 0xFF;
    uint8_t _2_byte = (byte.integer >> 16) & 0xFF;
    uint8_t _3_byte = (byte.integer >> 8) & 0xFF;
    uint8_t _4_byte = (byte.integer >> 0) & 0xFF;
    uint8_t sum = id+type+_1_byte+_2_byte+_3_byte+_4_byte;
    int32_t numsum = (_1_byte << 24) + (_2_byte<<16) + (_3_byte << 8) + _4_byte;
    ser.putc(STARTDATA);
    ser.putc(id);
    ser.putc(type);
    ser.putc(_1_byte);
    ser.putc(_2_byte);
    ser.putc(_3_byte);
    ser.putc(_4_byte);
    ser.putc(sum);
    ser.putc(ENDDATA);
    while(!ser.writeable());
    return true;
}
bool gotoSerial::subscribe(unsigned id,int* var){
    if(sub_vars_size < id) sub_vars_size = id;
    sub_vars.resize(sub_vars_size+1);
    sub_vars.at(id).integer = var;
    return true;
}
bool gotoSerial::subscribe(unsigned id,float* var){
    if(sub_vars_size < id) sub_vars_size = id;
    sub_vars.resize(sub_vars_size+1);
    sub_vars.at(id).decimal = var;
    return true;
}
bool gotoSerial::subscribe(unsigned id,Func func){
    if(sub_vars_size < id) sub_vars_size = id;
    sub_funcs.resize(sub_vars_size+1);
    sub_funcs.at(id) = func;
    return true;
}
void gotoSerial::interrupt_read(){
    int8_t byte;
    while(ser.readable()){
        byte = ser.getc();
        buf.write(byte);
        if(byte == ENDDATA) while(buf.readable()) update();
    }
}
bool gotoSerial::update(){
    if(buf.readable()){
        int id = 0;
        int type = 0;
        if(buf.read() == STARTDATA){
            id = buf.read();
            type = buf.read();
            if(0 <= id and id <= sub_vars_size){
                uint8_t _1_byte = buf.read();
                uint8_t _2_byte = buf.read();
                uint8_t _3_byte = buf.read();
                uint8_t _4_byte = buf.read();
                uint8_t sum = id+type+_1_byte+_2_byte+_3_byte+_4_byte;
                if(buf.read() == sum) {
                    Pack byte;
                    byte.integer = ((int32_t)_1_byte << 24) + ((int32_t)_2_byte<<16) + ((int32_t)_3_byte << 8) + (int32_t)_4_byte;
                    if(type == INT) *sub_vars.at(id).integer = byte.integer;
                    else if(type == FLOAT) *sub_vars.at(id).decimal = byte.decimal;
                    else if(type == FUNC) (*(sub_funcs.at(id)))(byte);
                }
            }
            else for(int i = 0;i < 5;i++) buf.read();
        }
    }
    return true;
}

}