SerialBridgeMbed/SerialBridge.cpp at main · TNCT-Mechatech/SerialBridgeMbed · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
#include "SerialBridge.h"

SerialBridge::SerialBridge(BufferSerial *dev)
{
    _dev = dev;
    _max_data_size = 0;
}

void SerialBridge::wait_host(char* device_name)
{
    _wait_host(device_name);
}

void SerialBridge::add_frame(SerialBridge::frame_id id, sb::_Message *str)
{
    if (str != NULL && _str[id] == NULL && id <= STRUCT_MAX_NUM)
    {
        _str[id] = str;
        if (_max_data_size < str->size())
            _max_data_size = str->size();
    }
}

int SerialBridge::read()
{
    int size = FRAME_LEN + _max_data_size;
    if (_dev->available() >= size){
        uint8_t check_sum = 0;
        int data_sum = 0;

        if (_read_once(&data_sum) != HEADER)
            return -2;

        frame_id id = _read_once(&data_sum);

        if (_str[id] == NULL)
            return -3;

        if (_read_once(&data_sum) != _str[id]->msg_id())
            return -4;

        for (int i = 0; i < _str[id]->size(); i++){
            //uint8_t c;
            _str[id]->ptr()[i] = _read_once(&data_sum);
        }

        check_sum = _dev->read();

        if (_read_once(&data_sum) != END)
            return -5;

        if ((data_sum & 0xFF) == check_sum)
            return 0;
        else
            return -6;
    }
    return -1;
}

void SerialBridge::write(SerialBridge::frame_id id, sb::_Message *str)
{
    int data_sum = 0;
    _write_once(HEADER, &data_sum);
    _write_once(id, &data_sum);
    _write_once(str->msg_id(), &data_sum);
    for (int i = 0; i < str->size(); i++)
        _write_once(str->ptr()[i], &data_sum);

    data_sum += END;
    _dev->putc((uint8_t)(data_sum & 0xFF)); //下位ビット8bitを送信
    _dev->putc(END);
    //_dev->flush();
}

void SerialBridge::_wait_host(char* device_name)
{
    char res[3] = {};
    while(strcmp(res, "OK")){
        _dev->printf("%s\n",device_name);
        _dev->readBytesUntil('\n', res, 3);
        //delay(250);
    }
}

inline void SerialBridge::_write_once(uint8_t c, int *data_sum){
    _dev->putc(c);
    *data_sum += c;
}

inline uint8_t SerialBridge::_read_once(int *data_sum){
    uint8_t c = _dev->read();
    *data_sum += c;
    return c;
}