@@ -34,9 +34,8 @@ MavLinkConnectionImpl::~MavLinkConnectionImpl() {
34
34
close ();
35
35
}
36
36
37
- template <typename PortType>
38
37
std::shared_ptr<MavLinkConnection> MavLinkConnectionImpl::createConnection (const std::string &nodeName,
39
- std::shared_ptr<PortType > port) {
38
+ std::shared_ptr<Port > port) {
40
39
// std::shared_ptr<MavLinkCom> owner, const std::string& nodeName
41
40
std::shared_ptr<MavLinkConnection> con = std::make_shared<MavLinkConnection>();
42
41
con->startListening (nodeName, port);
@@ -47,7 +46,7 @@ std::shared_ptr<MavLinkConnection> MavLinkConnectionImpl::connectLocalUdp(const
47
46
const std::string &localAddr, int localPort) {
48
47
std::shared_ptr<UdpClientPort> socket = std::make_shared<UdpClientPort>();
49
48
socket->connect (localAddr, localPort, " " , 0 );
50
- return createConnection<UdpClientPort> (nodeName, socket);
49
+ return createConnection (nodeName, socket);
51
50
}
52
51
53
52
std::shared_ptr<MavLinkConnection> MavLinkConnectionImpl::connectRemoteUdp (const std::string &nodeName,
@@ -61,7 +60,7 @@ std::shared_ptr<MavLinkConnection> MavLinkConnectionImpl::connectRemoteUdp(const
61
60
}
62
61
std::shared_ptr<UdpClientPort> socket = std::make_shared<UdpClientPort>();
63
62
socket->connect (local, 0 , remoteAddr, remotePort);
64
- return createConnection<UdpClientPort> (nodeName, socket);
63
+ return createConnection (nodeName, socket);
65
64
}
66
65
67
66
std::shared_ptr<MavLinkConnection> MavLinkConnectionImpl::connectTcp (const std::string &nodeName,
@@ -74,7 +73,7 @@ std::shared_ptr<MavLinkConnection> MavLinkConnectionImpl::connectTcp(const std::
74
73
}
75
74
std::shared_ptr<TcpClientPort> socket = std::make_shared<TcpClientPort>();
76
75
socket->connect (local, 0 , remoteIpAddr, remotePort);
77
- return createConnection<TcpClientPort> (nodeName, socket);
76
+ return createConnection (nodeName, socket);
78
77
}
79
78
80
79
std::string MavLinkConnectionImpl::acceptTcp (std::shared_ptr<MavLinkConnection> parent, const std::string &nodeName,
@@ -109,7 +108,7 @@ std::shared_ptr<MavLinkConnection> MavLinkConnectionImpl::connectSerial(const st
109
108
serial->write (reinterpret_cast <const uint8_t *>(initString.c_str ()), static_cast <int >(initString.size ()));
110
109
}
111
110
112
- return createConnection<SerialPort> (nodeName, serial);
111
+ return createConnection (nodeName, serial);
113
112
}
114
113
115
114
void MavLinkConnectionImpl::startListening (std::shared_ptr<MavLinkConnection> parent, const std::string &nodeName,
@@ -144,7 +143,6 @@ void MavLinkConnectionImpl::close() {
144
143
port->close ();
145
144
port = nullptr ;
146
145
}
147
-
148
146
if (read_thread.joinable ()) {
149
147
read_thread.join ();
150
148
}
@@ -159,6 +157,7 @@ void MavLinkConnectionImpl::close() {
159
157
bool MavLinkConnectionImpl::isOpen () { return !closed; }
160
158
161
159
int MavLinkConnectionImpl::getTargetComponentId () { return this ->other_component_id ; }
160
+
162
161
int MavLinkConnectionImpl::getTargetSystemId () { return this ->other_system_id ; }
163
162
164
163
uint8_t MavLinkConnectionImpl::getNextSequence () {
@@ -169,8 +168,9 @@ uint8_t MavLinkConnectionImpl::getNextSequence() {
169
168
void MavLinkConnectionImpl::ignoreMessage (uint8_t message_id) { ignored_messageids.insert (message_id); }
170
169
171
170
void MavLinkConnectionImpl::sendMessage (const MavLinkMessage &m) {
172
- if (ignored_messageids.find (m.msgid ) != ignored_messageids.end ())
171
+ if (ignored_messageids.find (m.msgid ) != ignored_messageids.end ()) {
173
172
return ;
173
+ }
174
174
175
175
if (closed) {
176
176
return ;
0 commit comments