InertialNavigationSystem/SerialProtocol.cpp at master · JTiefnig/InertialNavigationSystem · 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
#include "SerialProtocol.h"


void SerialProtocol::SendData(const SerialMessage& msg)
{

    putc('>');
    putc(msg.mtype);
    putc(msg.dlc+1);

    putc(msg.id);


    //printf(">%c%c%c", msg.mtype, msg.dlc+1, msg.id);

    write(msg.data, msg.dlc);

}


void SerialProtocol::SendInt(const char* id, int value)
{
    // int siz =sizeof(int);
    // SerialMessage msg;
    // msg.id = id;
    // msg.typeRep = 'i';
    // msg.datalenght = siz;
    // msg.data = (char*)(&value); // littlebit crazy type punning

    // SendData(msg);
}


void SerialProtocol::SendFloat(unsigned char id, float value)
{
    int siz =sizeof(float);
    SerialMessage msg;

    msg.mtype = MessageType::SEND_DATA;

    msg.dlc = siz;
    msg.id = id;
    msg.data = (char*)(&value); // littlebit crazy type punning

    SendData(msg);
}


void SerialProtocol::SendDouble(unsigned char id, double value)
{
    int siz =sizeof(double);
    SerialMessage msg;


    msg.mtype = MessageType::SEND_DATA;

    msg.dlc = siz;
    msg.id = id;
    msg.data = (char*)(&value); // littlebit crazy type punning

    SendData(msg);
}


void SerialProtocol::SendShort(const char* id, int16_t value)
{
    // int siz =sizeof(int16_t);
    // SerialMessage msg;
    // msg.id = id;
    // msg.typeRep = 's';
    // msg.datalenght = siz;
    // msg.data = (char*)(&value); // littlebit crazy type punning

    // SendData(msg);
}