goto_serial.cpp

00001 #include "goto_serial.hpp"
00002 
00003 namespace ARRC{
00004 
00005 gotoSerial::gotoSerial(PinName tx,PinName rx,int baudrate):ser(tx,rx,baudrate),sub_vars_size(0),sub_vars(0),buf(256){
00006     ser.attach(callback(this,&gotoSerial::interrupt_read),Serial::RxIrq);
00007 }
00008 bool gotoSerial::publish(unsigned id,int num,unsigned type){
00009     Pack byte;
00010     byte.integer = num;
00011     int8_t _1_byte = (byte.integer >> 24) & 0xFF;
00012     uint8_t _2_byte = (byte.integer >> 16) & 0xFF;
00013     uint8_t _3_byte = (byte.integer >> 8) & 0xFF;
00014     uint8_t _4_byte = (byte.integer >> 0) & 0xFF;
00015     int8_t sum = id+type+_1_byte+_2_byte+_3_byte+_4_byte;
00016     int32_t numsum = (_1_byte << 24) + (_2_byte<<16) + (_3_byte << 8) + _4_byte;
00017     ser.putc(STARTDATA);
00018     ser.putc(id);
00019     ser.putc(type);
00020     ser.putc(_1_byte);
00021     ser.putc(_2_byte);
00022     ser.putc(_3_byte);
00023     ser.putc(_4_byte);
00024     ser.putc(sum);
00025     ser.putc(ENDDATA);
00026     while(!ser.writeable());
00027     return true;
00028 }
00029 bool gotoSerial::publish(unsigned id,float num,unsigned type){
00030     Pack byte;
00031     byte.decimal = num;
00032     uint8_t _1_byte = (byte.integer >> 24) & 0xFF;
00033     uint8_t _2_byte = (byte.integer >> 16) & 0xFF;
00034     uint8_t _3_byte = (byte.integer >> 8) & 0xFF;
00035     uint8_t _4_byte = (byte.integer >> 0) & 0xFF;
00036     uint8_t sum = id+type+_1_byte+_2_byte+_3_byte+_4_byte;
00037     int32_t numsum = (_1_byte << 24) + (_2_byte<<16) + (_3_byte << 8) + _4_byte;
00038     ser.putc(STARTDATA);
00039     ser.putc(id);
00040     ser.putc(type);
00041     ser.putc(_1_byte);
00042     ser.putc(_2_byte);
00043     ser.putc(_3_byte);
00044     ser.putc(_4_byte);
00045     ser.putc(sum);
00046     ser.putc(ENDDATA);
00047     while(!ser.writeable());
00048     return true;
00049 }
00050 bool gotoSerial::subscribe(unsigned id,int* var){
00051     if(sub_vars_size < id) sub_vars_size = id;
00052     sub_vars.resize(sub_vars_size+1);
00053     sub_vars.at(id).integer = var;
00054     return true;
00055 }
00056 bool gotoSerial::subscribe(unsigned id,float* var){
00057     if(sub_vars_size < id) sub_vars_size = id;
00058     sub_vars.resize(sub_vars_size+1);
00059     sub_vars.at(id).decimal = var;
00060     return true;
00061 }
00062 bool gotoSerial::subscribe(unsigned id,Func func){
00063     if(sub_vars_size < id) sub_vars_size = id;
00064     sub_funcs.resize(sub_vars_size+1);
00065     sub_funcs.at(id) = func;
00066     return true;
00067 }
00068 void gotoSerial::interrupt_read(){
00069     int8_t byte;
00070     while(ser.readable()){
00071         byte = ser.getc();
00072         buf.write(byte);
00073         if(byte == ENDDATA) while(buf.readable()) update();
00074     }
00075 }
00076 bool gotoSerial::update(){
00077     if(buf.readable()){
00078         int id = 0;
00079         int type = 0;
00080         if(buf.read() == STARTDATA){
00081             id = buf.read();
00082             type = buf.read();
00083             if(0 <= id and id <= sub_vars_size){
00084                 uint8_t _1_byte = buf.read();
00085                 uint8_t _2_byte = buf.read();
00086                 uint8_t _3_byte = buf.read();
00087                 uint8_t _4_byte = buf.read();
00088                 uint8_t sum = id+type+_1_byte+_2_byte+_3_byte+_4_byte;
00089                 if(buf.read() == sum) {
00090                     Pack byte;
00091                     byte.integer = ((int32_t)_1_byte << 24) + ((int32_t)_2_byte<<16) + ((int32_t)_3_byte << 8) + (int32_t)_4_byte;
00092                     if(type == INT) *sub_vars.at(id).integer = byte.integer;
00093                     else if(type == FLOAT) *sub_vars.at(id).decimal = byte.decimal;
00094                     else if(type == FUNC) (*(sub_funcs.at(id)))(byte);
00095                 }
00096             }
00097             else for(int i = 0;i < 5;i++) buf.read();
00098         }
00099     }
00100     return true;
00101 }
00102 
00103 }
00104