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
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
|
#include "SerialPort.hpp"
#include "Logger.hpp"
#include "ExitException.hpp"
#include "ExitCodes.hpp"
#include <cstring>
#include <sstream>
using std::ostringstream;
#define READ_DEFAULT_TIMEOUT 3000 // ms
void
CSerialPort::writeWord(uint16_t w)
{
uint8_t b;
b = (w & 0xff);
this->write(&b, 1, 1);
b = (w & 0xff00) >> 8;
this->write(&b, 1, 1);
}
uint16_t
CSerialPort::readWord()
{
uint8_t b;
uint16_t w;
w = 0;
this->read(&b, 1);
w |= ((uint16_t) b);
this->read(&b, 1);
w |= ((uint16_t) b) << 8;
return w;
}
uint32_t
CSerialPort::readDoubleWord()
{
uint8_t b;
uint32_t dw;
dw = 0;
this->read(&b, 1);
dw |= ((uint32_t) b);
this->read(&b, 1);
dw |= ((uint32_t) b) << 8;
this->read(&b, 1);
dw |= ((uint32_t) b) << 16;
this->read(&b, 1);
dw |= ((uint32_t) b) << 24;
return dw;
}
void
CSerialPort::setReadTimeout(int timetout)
{
mReadTimeoutMs = timetout;
}
void
CSerialPort::setDefaultTimeout()
{
mReadTimeoutMs = READ_DEFAULT_TIMEOUT;
}
void
CSerialPort::write(uint8_t *data, int data_length, int padd_to)
{
// Write data
for (int i = 0; i < data_length; )
i += writeSingle(data, data_length);
// Write pad
// TODO: odkial brat hodnotu pad byte??
uint8_t p[512];
memset(p, 0xFF, 512);
for (padd_to -= data_length; padd_to > 0; ) {
int w = 512;
if (padd_to < 512)
w = padd_to;
padd_to -= writeSingle(p, w);
}
}
void
CSerialPort::read(uint8_t *data, int data_length)
{
for (int i = 0; i < data_length; ) {
int old_i = i;
i += readSingle(data + i, data_length - i);
// TODO: pre Win32, pretoze tam nedetekujeme timeout a moze vracat 0 bajtov precitanych
if (i == old_i) {
CLogger::error("Timeout occured while reading data from serial port", EXIT_SERIAL_PORT);
}
}
}
void
CSerialPort::sendSafeByte(uint8_t b)
{
uint8_t r;
this->write(&b, 1, 1);
this->read(&r, 1);
if (r != b) {
ostringstream os;
os << "Bad echo when sending byte safely, expected " << CLogger::decToHex(b);
os << " received " << CLogger::decToHex(r);
CLogger::error(os.str(), EXIT_SERIAL_PORT);
}
this->write(&b, 1, 1);
this->read(&r, 1);
if (r != 0x00) {
ostringstream os;
os << "Cannot send byte safely, expected 0x00 received " << CLogger::decToHex(r);
CLogger::error(os.str(), EXIT_SERIAL_PORT);
}
}
void
CSerialPort::sendSafeWord(uint16_t w)
{
this->writeWord(w);
uint16_t r = this->readWord();
if (r != w) {
ostringstream os;
os << "Bad echo when sending word safely, expected " << CLogger::decToHex(w);
os << " received " << CLogger::decToHex(r);
CLogger::error(os.str(), EXIT_SERIAL_PORT);
}
this->writeWord(w);
r = this->readWord();
if (r != 0x00) {
ostringstream os;
os << "Cannot send word safely, expected 0x0000 received " << CLogger::decToHex(r);
CLogger::error(os.str(), EXIT_SERIAL_PORT);
}
}
void
CSerialPort::sendSafeDoubleWord(uint32_t w)
{
// Send low word
sendSafeWord(w & 0x0000FFFF);
// Send high word
sendSafeWord((w & 0xFFFF0000) >> 16);
}
|
