-
Notifications
You must be signed in to change notification settings - Fork 7
/
CanRxTxThreads.cpp
188 lines (138 loc) · 5.38 KB
/
CanRxTxThreads.cpp
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
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
#include "CanBusControlboard.hpp"
#include <cstring>
#include <yarp/os/Time.h>
#include <ColorDebug.h>
#include "YarpCanSenderDelegate.hpp"
using namespace roboticslab;
// -----------------------------------------------------------------------------
void CanReaderWriterThread::beforeStart()
{
CD_INFO("Initializing CanBusControlboard %s thread %s.\n", type.c_str(), id.c_str());
}
// -----------------------------------------------------------------------------
void CanReaderWriterThread::afterStart(bool success)
{
CD_INFO("Configuring CanBusControlboard %s thread %s... %s\n", type.c_str(), id.c_str(), success ? "success" : "failure");
}
// -----------------------------------------------------------------------------
void CanReaderWriterThread::onStop()
{
CD_INFO("Stopping CanBusControlboard %s thread %s.\n", type.c_str(), id.c_str());
}
// -----------------------------------------------------------------------------
void CanReaderWriterThread::setDelay(double delay)
{
this->delay = delay <= 0.0 ? std::numeric_limits<double>::min() : delay;
}
// -----------------------------------------------------------------------------
CanReaderThread::CanReaderThread(const std::string & id)
: CanReaderWriterThread("read", id)
{}
// -----------------------------------------------------------------------------
void CanReaderThread::registerHandle(ICanBusSharer * p)
{
canIdToHandle[p->getId()] = p;
for (auto id : p->getAdditionalIds())
{
canIdToHandle[id] = p;
}
}
// -----------------------------------------------------------------------------
void CanReaderThread::run()
{
unsigned int read;
bool ok;
while (!isStopping())
{
//-- Lend CPU time to write threads.
// https://github.com/roboticslab-uc3m/yarp-devices/issues/191
yarp::os::Time::delay(delay);
//-- Return immediately if there is nothing to be read (non-blocking call), return false on errors.
ok = iCanBus->canRead(canBuffer, bufferSize, &read);
//-- All debugging messages should be contained in canRead, so just loop again.
if (!ok || read == 0) continue;
for (int i = 0; i < read; i++)
{
const yarp::dev::CanMessage & msg = canBuffer[i];
const int canId = msg.getId() & 0x7F;
auto it = canIdToHandle.find(canId);
if (it == canIdToHandle.end()) //-- Can ID not found
{
//-- Intercept 700h 0 msg that just indicates presence.
if (msg.getId() - canId == 0x700)
{
CD_SUCCESS("Device %d indicating presence.\n", canId);
}
continue;
}
it->second->interpretMessage(msg);
}
}
}
// -----------------------------------------------------------------------------
CanWriterThread::CanWriterThread(const std::string & id)
: CanReaderWriterThread("write", id),
sender(nullptr),
preparedMessages(0)
{}
// -----------------------------------------------------------------------------
CanWriterThread::~CanWriterThread()
{
delete sender;
}
// -----------------------------------------------------------------------------
void CanWriterThread::flush()
{
unsigned int sent;
std::lock_guard<std::mutex> lock(bufferMutex);
//-- Nothing to write, exit.
if (preparedMessages == 0) return;
//-- Write as many bytes as it can, return false on errors.
bool ok = iCanBus->canWrite(canBuffer, preparedMessages, &sent);
//-- Something bad happened, try again on the next call.
if (!ok) return;
//-- Some messages could not be sent, preserve them for later.
if (sent != preparedMessages)
{
CD_WARNING("Partial write! Prepared: %d, sent: %d.\n", preparedMessages, sent);
handlePartialWrite(sent);
}
preparedMessages -= sent;
}
// -----------------------------------------------------------------------------
void CanWriterThread::run()
{
while (!isStopping())
{
//-- Lend CPU time to read threads.
// https://github.com/roboticslab-uc3m/yarp-devices/issues/191
yarp::os::Time::delay(delay);
//-- Send everything and reset the queue.
flush();
}
}
// -----------------------------------------------------------------------------
void CanWriterThread::handlePartialWrite(unsigned int sent)
{
for (int i = sent, j = 0; i < preparedMessages; i++, j++)
{
yarp::dev::CanMessage & msg = canBuffer[j];
const yarp::dev::CanMessage & pendingMsg = canBuffer[i];
msg.setId(pendingMsg.getId());
msg.setLen(pendingMsg.getLen());
std::memcpy(msg.getData(), pendingMsg.getData(), pendingMsg.getLen());
}
}
// -----------------------------------------------------------------------------
void CanWriterThread::setCanHandles(yarp::dev::ICanBus * iCanBus, yarp::dev::ICanBufferFactory * iCanBufferFactory, unsigned int bufferSize)
{
CanReaderWriterThread::setCanHandles(iCanBus, iCanBufferFactory, bufferSize);
sender = new YarpCanSenderDelegate(canBuffer, bufferMutex, preparedMessages, bufferSize);
}
// -----------------------------------------------------------------------------
CanSenderDelegate * CanWriterThread::getDelegate()
{
return sender;
}
// -----------------------------------------------------------------------------