Diff: goto_serial.cpp

Revision:

0:4801795a9073

diff -r 000000000000 -r 4801795a9073 goto_serial.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/goto_serial.cpp	Sat Dec 11 02:37:36 2021 +0000
@@ -0,0 +1,104 @@
+#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;
+}
+
+}
+