Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : /// @file MAVLink_routing.h
17 : /// @brief handle routing of MAVLink packets by sysid/componentid
18 :
19 : #include "GCS_config.h"
20 :
21 : #if HAL_GCS_ENABLED
22 :
23 : #include <stdio.h>
24 : #include <AP_HAL/AP_HAL.h>
25 : #include <AP_Common/AP_Common.h>
26 : #include "GCS.h"
27 : #include "MAVLink_routing.h"
28 :
29 : #include <AP_ADSB/AP_ADSB.h>
30 :
31 : extern const AP_HAL::HAL& hal;
32 :
33 : #define ROUTING_DEBUG 0
34 :
35 : // constructor
36 2193 : MAVLink_routing::MAVLink_routing(void) : num_routes(0) {}
37 :
38 : /*
39 : forward a MAVLink message to the right port. This also
40 : automatically learns the route for the sender if it is not
41 : already known.
42 :
43 : This returns true if the message should be processed locally
44 :
45 : Theory of MAVLink routing:
46 :
47 : When a flight controller receives a message it should process it
48 : locally if any of these conditions hold:
49 :
50 : 1a) the message has no target_system field
51 :
52 : 1b) the message has a target_system of zero
53 :
54 : 1c) the message has the flight controllers target system and has no
55 : target_component field
56 :
57 : 1d) the message has the flight controllers target system and has
58 : the flight controllers target_component
59 :
60 : 1e) the message has the flight controllers target system and the
61 : flight controller has not seen any messages on any of its links
62 : from a system that has the messages
63 : target_system/target_component combination
64 :
65 : When a flight controller receives a message it should forward it
66 : onto another different link if any of these conditions hold for that
67 : link:
68 :
69 : 2a) the message has no target_system field
70 :
71 : 2b) the message has a target_system of zero
72 :
73 : 2c) the message does not have the flight controllers target_system
74 : and the flight controller has seen a message from the messages
75 : target_system on the link
76 :
77 : 2d) the message has the flight controllers target_system and has a
78 : target_component field and the flight controllers has seen a
79 : message from the target_system/target_component combination on
80 : the link
81 :
82 : Note: This proposal assumes that ground stations will not send command
83 : packets to a non-broadcast destination (sysid/compid combination)
84 : until they have received at least one package from that destination
85 : over the link. This is essential to prevent a flight controller from
86 : acting on a message that is not meant for it. For example, a PARAM_SET
87 : cannot be sent to a specific sysid/compid combination until the GCS
88 : has seen a packet from that sysid/compid combination on the link.
89 :
90 : The GCS must also reset what sysid/compid combinations it has seen on
91 : a link when it sees a SYSTEM_TIME message with a decrease in
92 : time_boot_ms from a particular sysid/compid. That is essential to
93 : detect a reset of the flight controller, which implies a reset of its
94 : routing table.
95 :
96 : */
97 2784593 : bool MAVLink_routing::check_and_forward(uint8_t framing_status,
98 : GCS_MAVLINK &in_link,
99 : const mavlink_message_t &msg)
100 : {
101 : // handle the case of loopback of our own messages, due to
102 : // incorrect serial configuration.
103 2784593 : if (msg.sysid == mavlink_system.sysid &&
104 21901 : msg.compid == mavlink_system.compid) {
105 103 : return false; // do not process locally
106 : }
107 :
108 2784490 : switch (framing_status) {
109 2784490 : case MAVLINK_FRAMING_OK:
110 2784490 : break;
111 0 : case MAVLINK_FRAMING_BAD_CRC:
112 0 : if (in_link.option_enabled(GCS_MAVLINK::Option::FORWARD_BAD_CRC) &&
113 0 : msg.msgid != MAVLINK_MSG_ID_RADIO &&
114 0 : msg.msgid != MAVLINK_MSG_ID_RADIO_STATUS &&
115 0 : msg.msgid != MAVLINK_MSG_ID_ADSB_VEHICLE) {
116 0 : forward(in_link, msg);
117 : }
118 0 : return false; // do not process locally
119 0 : case MAVLINK_FRAMING_BAD_SIGNATURE:
120 : default:
121 0 : return false; // do not process locally
122 : }
123 :
124 : #if HAL_SOLO_GIMBAL_ENABLED
125 : // check if a Gopro is connected. If yes, we allow the routing
126 : // of mavlink messages to a private channel (Solo Gimbal case)
127 2784490 : if (!gopro_status_check && (msg.msgid == MAVLINK_MSG_ID_GOPRO_HEARTBEAT)) {
128 0 : gopro_status_check = true;
129 0 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "GoPro in Solo gimbal detected");
130 : }
131 : #endif // HAL_SOLO_GIMBAL_ENABLED
132 :
133 : // learn new routes including private channels
134 : // so that find_mav_type works for all channels
135 2784490 : learn_route(in_link, msg);
136 :
137 2784490 : if (msg.msgid == MAVLINK_MSG_ID_RADIO ||
138 2784490 : msg.msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
139 : // don't forward RADIO packets
140 0 : return true;
141 : }
142 :
143 2784490 : const bool from_private_channel = in_link.is_private();
144 :
145 2784490 : if (msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
146 : // heartbeat needs special handling
147 1064198 : if (!from_private_channel) {
148 1063976 : handle_heartbeat(in_link, msg);
149 : }
150 1064198 : return true;
151 : }
152 :
153 : /*
154 : * Note that if you are looking at handling routing especially for
155 : * certain packets here then you may need to add your message to
156 : * the list of IDs we will not forward *even if* the option to
157 : * send bad-CRC packets has been set (this is in a switch
158 : * statement, above).
159 : */
160 :
161 : #if HAL_ADSB_ENABLED
162 1720292 : if (msg.msgid == MAVLINK_MSG_ID_ADSB_VEHICLE) {
163 : // if enabled ADSB packets are not forwarded, they have their own stream rate
164 15 : const AP_ADSB *adsb = AP::ADSB();
165 15 : if ((adsb != nullptr) && (adsb->enabled())) {
166 9 : return true;
167 : }
168 : }
169 : #endif
170 :
171 1720283 : return forward(in_link, msg);
172 : }
173 :
174 1720283 : bool MAVLink_routing::forward(GCS_MAVLINK &in_link,
175 : const mavlink_message_t &msg)
176 : {
177 : // extract the targets for this packet
178 1720283 : int16_t target_system = -1;
179 1720283 : int16_t target_component = -1;
180 1720283 : get_targets(msg, target_system, target_component);
181 :
182 1720283 : bool broadcast_system = (target_system == 0 || target_system == -1);
183 1720283 : bool broadcast_component = (target_component == 0 || target_component == -1);
184 1720283 : bool match_system = broadcast_system || (target_system == mavlink_system.sysid);
185 2368450 : bool match_component = match_system && (broadcast_component ||
186 648167 : (target_component == mavlink_system.compid));
187 1720283 : bool process_locally = match_system && match_component;
188 :
189 : // don't ever forward data from a private channel
190 : // unless a Gopro camera is connected to a Solo gimbal
191 1720283 : const bool from_private_channel = in_link.is_private();
192 1720283 : bool should_process_locally = from_private_channel;
193 : #if HAL_SOLO_GIMBAL_ENABLED
194 1720283 : if (gopro_status_check) {
195 0 : should_process_locally = false;
196 : }
197 : #endif
198 1720283 : if (should_process_locally) {
199 18221 : return process_locally;
200 : }
201 :
202 1702062 : if (process_locally && !broadcast_system && !broadcast_component) {
203 : // nothing more to do - it can only be for us
204 625158 : return true;
205 : }
206 :
207 : // forward on any channels matching the targets
208 1076904 : bool forwarded = false;
209 : bool sent_to_chan[MAVLINK_COMM_NUM_BUFFERS];
210 1076904 : memset(sent_to_chan, 0, sizeof(sent_to_chan));
211 2324635 : for (uint8_t i=0; i<num_routes; i++) {
212 :
213 : // Skip if channel is private and the target system or component IDs do not match
214 1247731 : GCS_MAVLINK *out_link = gcs().chan(routes[i].channel);
215 1247731 : if (out_link == nullptr) {
216 : // this is bad
217 0 : continue;
218 : }
219 1249240 : if (out_link->is_private() &&
220 1509 : (target_system != routes[i].sysid ||
221 0 : target_component != routes[i].compid)) {
222 1509 : continue;
223 : }
224 :
225 1246222 : if (broadcast_system || (target_system == routes[i].sysid &&
226 0 : (broadcast_component ||
227 0 : target_component == routes[i].compid ||
228 0 : !match_system))) {
229 :
230 1236402 : if (&in_link != out_link && !sent_to_chan[routes[i].channel]) {
231 158355 : if (out_link->check_payload_size(msg.len)) {
232 : #if ROUTING_DEBUG
233 : ::printf("fwd msg %u from chan %u on chan %u sysid=%d compid=%d\n",
234 : msg.msgid,
235 : (unsigned)in_link.get_chan(),
236 : (unsigned)routes[i].channel,
237 : (int)target_system,
238 : (int)target_component);
239 : #endif
240 130909 : _mavlink_resend_uart(routes[i].channel, &msg);
241 : }
242 158355 : sent_to_chan[routes[i].channel] = true;
243 158355 : forwarded = true;
244 : }
245 : }
246 : }
247 :
248 1076904 : if ((!forwarded && match_system) ||
249 : broadcast_system) {
250 1076850 : process_locally = true;
251 : }
252 :
253 1076904 : return process_locally;
254 : }
255 :
256 : /*
257 : send a MAVLink message to all components with this vehicle's system id
258 :
259 : This is a no-op if no routes to components have been learned
260 : */
261 21 : void MAVLink_routing::send_to_components(uint32_t msgid, const char *pkt, uint8_t pkt_len)
262 : {
263 21 : const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(msgid);
264 21 : if (entry == nullptr) {
265 0 : return;
266 : }
267 21 : send_to_components(pkt, entry, pkt_len);
268 : }
269 :
270 21 : void MAVLink_routing::send_to_components(const char *pkt, const mavlink_msg_entry_t *entry, const uint8_t pkt_len)
271 : {
272 21 : bool sent_to_chan[MAVLINK_COMM_NUM_BUFFERS] {};
273 :
274 : // check learned routes
275 53 : for (uint8_t i=0; i<num_routes; i++) {
276 32 : if (routes[i].sysid != mavlink_system.sysid) {
277 : // our system ID hasn't been seen on this link
278 27 : continue;
279 : }
280 5 : if (sent_to_chan[routes[i].channel]) {
281 : // we've already send it on this link
282 0 : continue;
283 : }
284 5 : if (comm_get_txspace(routes[i].channel) <
285 5 : ((uint16_t)entry->max_msg_len) + GCS_MAVLINK::packet_overhead_chan(routes[i].channel)) {
286 : // it doesn't fit on this channel
287 0 : continue;
288 : }
289 : #if ROUTING_DEBUG
290 : ::printf("send msg %u on chan %u sysid=%u compid=%u\n",
291 : entry->msgid,
292 : (unsigned)routes[i].channel,
293 : (unsigned)routes[i].sysid,
294 : (unsigned)routes[i].compid);
295 : #endif
296 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
297 5 : if (entry->max_msg_len > pkt_len) {
298 0 : AP_HAL::panic("Passed packet message length (%u > %u)",
299 0 : entry->max_msg_len, pkt_len);
300 : }
301 : #endif
302 10 : _mav_finalize_message_chan_send(routes[i].channel,
303 5 : entry->msgid,
304 : pkt,
305 5 : entry->min_msg_len,
306 5 : MIN(entry->max_msg_len, pkt_len),
307 5 : entry->crc_extra);
308 5 : sent_to_chan[routes[i].channel] = true;
309 : }
310 21 : }
311 :
312 : /*
313 : search for the first vehicle or component in the routing table with given mav_type and retrieve it's sysid, compid and channel
314 : returns true if a match is found
315 : */
316 10 : bool MAVLink_routing::find_by_mavtype(uint8_t mavtype, uint8_t &sysid, uint8_t &compid, mavlink_channel_t &channel)
317 : {
318 : // check learned routes
319 20 : for (uint8_t i=0; i<num_routes; i++) {
320 10 : if (routes[i].mavtype == mavtype) {
321 0 : sysid = routes[i].sysid;
322 0 : compid = routes[i].compid;
323 0 : channel = routes[i].channel;
324 0 : return true;
325 : }
326 : }
327 :
328 : // if we've reached we have not found the component
329 10 : return false;
330 : }
331 :
332 : /*
333 : search for the first vehicle or component in the routing table with given mav_type and component id and retrieve its sysid and channel
334 : returns true if a match is found
335 : */
336 8 : bool MAVLink_routing::find_by_mavtype_and_compid(uint8_t mavtype, uint8_t compid, uint8_t &sysid, mavlink_channel_t &channel) const
337 : {
338 28 : for (uint8_t i=0; i<num_routes; i++) {
339 28 : if ((routes[i].mavtype == mavtype) && (routes[i].compid == compid)) {
340 8 : sysid = routes[i].sysid;
341 8 : channel = routes[i].channel;
342 8 : return true;
343 : }
344 : }
345 0 : return false;
346 : }
347 :
348 : /*
349 : see if the message is for a new route and learn it
350 : */
351 2784490 : void MAVLink_routing::learn_route(GCS_MAVLINK &in_link, const mavlink_message_t &msg)
352 : {
353 : uint8_t i;
354 2784490 : if (msg.sysid == 0) {
355 : // don't learn routes to the broadcast system
356 0 : return;
357 : }
358 2784490 : if (msg.sysid == mavlink_system.sysid &&
359 21798 : msg.compid == mavlink_system.compid) {
360 : // don't learn routes to ourself. We know where we are.
361 0 : return;
362 : }
363 2784490 : if (msg.sysid == mavlink_system.sysid &&
364 21798 : msg.compid == MAV_COMP_ID_ALL) {
365 : // don't learn routes to the broadcast component ID for our
366 : // own system id. We should still broadcast these, but we
367 : // should also process them locally.
368 0 : return;
369 : }
370 2784490 : const mavlink_channel_t in_channel = in_link.get_chan();
371 2865757 : for (i=0; i<num_routes; i++) {
372 2863561 : if (routes[i].sysid == msg.sysid &&
373 2790397 : routes[i].compid == msg.compid &&
374 2782807 : routes[i].channel == in_channel) {
375 2782294 : if (routes[i].mavtype == 0 && msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
376 22 : routes[i].mavtype = mavlink_msg_heartbeat_get_type(&msg);
377 : }
378 2782294 : break;
379 : }
380 : }
381 2784490 : if (i == num_routes && i<MAVLINK_MAX_ROUTES) {
382 2196 : routes[i].sysid = msg.sysid;
383 2196 : routes[i].compid = msg.compid;
384 2196 : routes[i].channel = in_channel;
385 2196 : if (msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
386 2167 : routes[i].mavtype = mavlink_msg_heartbeat_get_type(&msg);
387 : }
388 2196 : num_routes++;
389 : #if ROUTING_DEBUG
390 : ::printf("learned route %u %u via %u\n",
391 : (unsigned)msg.sysid,
392 : (unsigned)msg.compid,
393 : (unsigned)in_channel);
394 : #endif
395 : }
396 : }
397 :
398 :
399 : /*
400 : special handling for heartbeat messages. To ensure routing
401 : propagation heartbeat messages need to be forwarded on all channels
402 : except channels where the sysid/compid of the heartbeat could come from
403 : */
404 1063976 : void MAVLink_routing::handle_heartbeat(GCS_MAVLINK &link, const mavlink_message_t &msg)
405 : {
406 1063976 : uint16_t mask = GCS_MAVLINK::active_channel_mask() & ~GCS_MAVLINK::private_channel_mask();
407 :
408 1063976 : const mavlink_channel_t in_channel = link.get_chan();
409 :
410 : // don't send on the incoming channel. This should only matter if
411 : // the routing table is full
412 1063976 : mask &= ~(1U<<(in_channel-MAVLINK_COMM_0));
413 :
414 : // mask out channels that do not want the heartbeat to be forwarded
415 1063976 : mask &= ~no_route_mask;
416 :
417 : // mask out channels that are known sources for this sysid/compid
418 2227235 : for (uint8_t i=0; i<num_routes; i++) {
419 1163259 : if (routes[i].sysid == msg.sysid && routes[i].compid == msg.compid) {
420 1064760 : mask &= ~(1U<<((unsigned)(routes[i].channel-MAVLINK_COMM_0)));
421 : }
422 : }
423 :
424 1063976 : if (mask == 0) {
425 : // nothing to send to
426 977289 : return;
427 : }
428 :
429 : // send on the remaining channels
430 1473679 : for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
431 1386992 : if (mask & (1U<<i)) {
432 88247 : mavlink_channel_t channel = (mavlink_channel_t)(MAVLINK_COMM_0 + i);
433 88247 : if (comm_get_txspace(channel) >= ((uint16_t)msg.len) +
434 88247 : GCS_MAVLINK::packet_overhead_chan(channel)) {
435 : #if ROUTING_DEBUG
436 : ::printf("fwd HB from chan %u on chan %u from sysid=%u compid=%u\n",
437 : (unsigned)in_channel,
438 : (unsigned)channel,
439 : (unsigned)msg.sysid,
440 : (unsigned)msg.compid);
441 : #endif
442 56402 : _mavlink_resend_uart(channel, &msg);
443 : }
444 : }
445 : }
446 : }
447 :
448 :
449 : /*
450 : extract target sysid and compid from a message. int16_t is used so
451 : that the caller can set them to -1 and know when a sysid or compid
452 : target is found in the message
453 : */
454 1720283 : void MAVLink_routing::get_targets(const mavlink_message_t &msg, int16_t &sysid, int16_t &compid)
455 : {
456 1720283 : const mavlink_msg_entry_t *msg_entry = mavlink_get_msg_entry(msg.msgid);
457 1720283 : if (msg_entry == nullptr) {
458 0 : return;
459 : }
460 1720283 : if (msg_entry->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM) {
461 648334 : sysid = _MAV_RETURN_uint8_t(&msg, msg_entry->target_system_ofs);
462 : }
463 1720283 : if (msg_entry->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT) {
464 648257 : compid = _MAV_RETURN_uint8_t(&msg, msg_entry->target_component_ofs);
465 : }
466 : }
467 :
468 : #endif // HAL_GCS_ENABLED
|
