@@ -48,7 +48,6 @@ std::shared_ptr<AdHocConnection> AdHocConnectionImpl::connectRemoteUdp(const std
48
48
if (remoteAddr == " 127.0.0.1" ) {
49
49
local = " 127.0.0.1" ;
50
50
}
51
-
52
51
std::shared_ptr<UdpClientPort> socket = std::make_shared<UdpClientPort>();
53
52
socket->connect (local, 0 , remoteAddr, remotePort);
54
53
return createConnection (nodeName, std::static_pointer_cast<Port>(socket));
@@ -61,7 +60,6 @@ std::shared_ptr<AdHocConnection> AdHocConnectionImpl::connectTcp(const std::stri
61
60
if (remoteIpAddr == " 127.0.0.1" ) {
62
61
local = " 127.0.0.1" ;
63
62
}
64
-
65
63
std::shared_ptr<TcpClientPort> socket = std::make_shared<TcpClientPort>();
66
64
socket->connect (local, 0 , remoteIpAddr, remotePort);
67
65
return createConnection (nodeName, std::static_pointer_cast<Port>(socket));
@@ -71,10 +69,9 @@ std::shared_ptr<AdHocConnection> AdHocConnectionImpl::connectSerial(const std::s
71
69
int baudRate, const std::string initString) {
72
70
std::shared_ptr<SerialPort> serial = std::make_shared<SerialPort>();
73
71
int hr = serial->connect (name.c_str (), baudRate);
74
-
75
- if (hr != 0 )
72
+ if (hr != 0 ) {
76
73
throw std::runtime_error (Utils::stringf (" Could not open the serial port %s, error=%d" , name.c_str (), hr));
77
-
74
+ }
78
75
// send this right away just in case serial link is not already configured
79
76
if (initString.size () > 0 ) {
80
77
serial->write (reinterpret_cast <const uint8_t *>(initString.c_str ()), static_cast <int >(initString.size ()));
@@ -102,7 +99,6 @@ void AdHocConnectionImpl::close() {
102
99
port->close ();
103
100
port = nullptr ;
104
101
}
105
-
106
102
if (read_thread.joinable ()) {
107
103
read_thread.join ();
108
104
}
@@ -122,7 +118,6 @@ void AdHocConnectionImpl::sendMessage(const std::vector<uint8_t> &msg) {
122
118
if (closed) {
123
119
return ;
124
120
}
125
-
126
121
try {
127
122
port->write (msg.data (), static_cast <int >(msg.size ()));
128
123
} catch (std::exception &e) {
@@ -166,40 +161,31 @@ void AdHocConnectionImpl::readPackets() {
166
161
std::this_thread::sleep_for (std::chrono::milliseconds (10 ));
167
162
continue ;
168
163
}
169
-
170
164
int count = safePort->read (buffer, MAXBUFFER);
171
165
if (count <= 0 ) {
172
166
// error? well let's try again, but we should be careful not to spin too fast and kill the CPU
173
167
std::this_thread::sleep_for (std::chrono::milliseconds (1 ));
174
168
continue ;
175
169
}
176
-
177
170
if (count >= MAXBUFFER) {
178
-
179
171
std::cerr << " GAH KM911 message size (" << std::to_string (count)
180
172
<< " ) is bigger than max buffer size! Time to support frame breaks, Moffitt" << std::endl;
181
-
182
173
// error? well let's try again, but we should be careful not to spin too fast and kill the CPU
183
174
std::this_thread::sleep_for (std::chrono::milliseconds (1 ));
184
175
continue ;
185
176
}
186
-
187
177
// queue event for publishing.
188
178
{
189
179
std::lock_guard<std::mutex> guard (msg_queue_mutex_);
190
180
std::vector<uint8_t > message (count);
191
181
memcpy (message.data (), buffer, count);
192
182
msg_queue_.push (message);
193
183
}
194
-
195
184
if (waiting_for_msg_) {
196
185
msg_available_.post ();
197
186
}
198
-
199
187
} // while
200
-
201
188
delete[] buffer;
202
-
203
189
} // readPackets
204
190
205
191
void AdHocConnectionImpl::drainQueue () {
@@ -231,7 +217,6 @@ void AdHocConnectionImpl::drainQueue() {
231
217
snapshot_stale = false ;
232
218
}
233
219
auto end = snapshot.end ();
234
-
235
220
auto startTime = std::chrono::system_clock::now ();
236
221
std::shared_ptr<AdHocConnection> sharedPtr = std::shared_ptr<AdHocConnection>(this ->con_ );
237
222
for (auto ptr = snapshot.begin (); ptr != end; ptr++) {
@@ -250,9 +235,7 @@ void AdHocConnectionImpl::publishPackets() {
250
235
// CurrentThread::setMaximumPriority();
251
236
CurrentThread::setThreadName (" MavLinkThread" );
252
237
while (!closed) {
253
-
254
238
drainQueue ();
255
-
256
239
waiting_for_msg_ = true ;
257
240
msg_available_.wait ();
258
241
waiting_for_msg_ = false ;
0 commit comments