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 : // Automatic Identification System, https://gpsd.gitlab.io/gpsd/AIVDM.html
17 :
18 : // ToDo: enable receiving of the Mavlink AIS message, type bitmask?
19 :
20 : #include "AP_AIS.h"
21 :
22 : #if AP_AIS_ENABLED
23 :
24 : #include <AP_Vehicle/AP_Vehicle_Type.h>
25 :
26 : #define AP_AIS_DUMMY_METHODS_ENABLED ((AP_AIS_ENABLED == 2) && !APM_BUILD_TYPE(APM_BUILD_Rover))
27 :
28 : #if !AP_AIS_DUMMY_METHODS_ENABLED
29 :
30 : #include <AP_Logger/AP_Logger.h>
31 : #include <AP_SerialManager/AP_SerialManager.h>
32 : #include <GCS_MAVLink/GCS_MAVLink.h>
33 : #include <GCS_MAVLink/GCS.h>
34 : #include <AP_AHRS/AP_AHRS.h>
35 : #include <AP_Common/ExpandingString.h>
36 : #include <AC_Avoidance/AP_OADatabase.h>
37 :
38 : const AP_Param::GroupInfo AP_AIS::var_info[] = {
39 :
40 : // @Param: TYPE
41 : // @DisplayName: AIS receiver type
42 : // @Description: AIS receiver type
43 : // @Values: 0:None,1:NMEA AIVDM message
44 : // @User: Standard
45 : // @RebootRequired: True
46 : AP_GROUPINFO_FLAGS("TYPE", 1, AP_AIS, _type, 0, AP_PARAM_FLAG_ENABLE),
47 :
48 : // @Param: LIST_MAX
49 : // @DisplayName: AIS vessel list size
50 : // @Description: AIS list size of nearest vessels. Longer lists take longer to refresh with lower SRx_ADSB values.
51 : // @Range: 1 100
52 : // @User: Advanced
53 : AP_GROUPINFO("LIST_MAX", 2, AP_AIS, _max_list, 25),
54 :
55 : // @Param: TIME_OUT
56 : // @DisplayName: AIS vessel time out
57 : // @Description: if no updates are received in this time a vessel will be removed from the list
58 : // @Units: s
59 : // @Range: 1 2000
60 : // @User: Advanced
61 : AP_GROUPINFO("TIME_OUT", 3, AP_AIS, _time_out, 600),
62 :
63 : // @Param: LOGGING
64 : // @DisplayName: AIS logging options
65 : // @Description: Bitmask of AIS logging options
66 : // @Bitmask: 0:Log all AIVDM messages,1:Log only unsupported AIVDM messages,2:Log decoded messages
67 : // @User: Advanced
68 : AP_GROUPINFO("LOGGING", 4, AP_AIS, _log_options, AIS_OPTIONS_LOG_UNSUPPORTED_RAW | AIS_OPTIONS_LOG_DECODED),
69 :
70 : AP_GROUPEND
71 : };
72 :
73 : // constructor
74 285 : AP_AIS::AP_AIS()
75 : {
76 285 : if (_singleton != nullptr) {
77 0 : AP_HAL::panic("AIS must be singleton");
78 : }
79 285 : _singleton = this;
80 :
81 285 : AP_Param::setup_object_defaults(this, var_info);
82 285 : }
83 :
84 : // return true if AIS is enabled
85 124655 : bool AP_AIS::enabled() const
86 : {
87 124655 : return AISType(_type.get()) != AISType::NONE;
88 : }
89 :
90 : // Initialize the AIS object and prepare it for use
91 283 : void AP_AIS::init()
92 : {
93 283 : if (!enabled()) {
94 280 : return;
95 : }
96 :
97 3 : _uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_AIS, 0);
98 3 : if (_uart == nullptr) {
99 0 : return;
100 : }
101 :
102 3 : _uart->begin(AP::serialmanager().find_baudrate(AP_SerialManager::SerialProtocol_AIS, 0));
103 : }
104 :
105 : // update AIS, expected to be called at 20hz
106 62833 : void AP_AIS::update()
107 : {
108 62833 : if (!_uart || !enabled()) {
109 62641 : return;
110 : }
111 :
112 : // read any available lines
113 192 : uint32_t nbytes = MIN(_uart->available(),1024U);
114 3892 : while (nbytes-- > 0) {
115 3700 : const int16_t byte = _uart->read();
116 3700 : if (byte == -1) {
117 0 : break;
118 : }
119 3700 : const char c = byte;
120 3700 : if (decode(c)) {
121 71 : const bool log_all = (_log_options & AIS_OPTIONS_LOG_ALL_RAW) != 0;
122 71 : const bool log_unsupported = ((_log_options & AIS_OPTIONS_LOG_UNSUPPORTED_RAW) != 0) && !log_all; // only log unsupported if not logging all
123 :
124 71 : if (_incoming.total > AIVDM_BUFFER_SIZE) {
125 : // no point in trying to decode it wont fit
126 : #if HAL_LOGGING_ENABLED
127 0 : if (log_all || log_unsupported) {
128 0 : log_raw(&_incoming);
129 : }
130 : #endif
131 0 : continue;
132 : }
133 : #if HAL_LOGGING_ENABLED
134 71 : if (log_all) {
135 0 : log_raw(&_incoming);
136 : }
137 : #endif
138 :
139 71 : if (_incoming.num == 1 && _incoming.total == 1) {
140 : // single part message
141 45 : if (!payload_decode(_incoming.payload) && log_unsupported) {
142 : #if HAL_LOGGING_ENABLED
143 : // could not decode so log
144 0 : log_raw(&_incoming);
145 : #endif
146 : }
147 26 : } else if (_incoming.num == _incoming.total && _incoming.total > 1) {
148 : // last part of a multi part message
149 13 : uint8_t index = 0;
150 :
151 : // We have the last part, need to find preceding fragments
152 13 : const uint8_t parts = _incoming.total - 1;
153 :
154 13 : uint8_t msg_parts[parts];
155 13 : for (uint8_t i = 0; i < AIVDM_BUFFER_SIZE; i++) {
156 : // look for the rest of the message from the start of the buffer
157 : // we assume the message has be received in the correct order
158 13 : if (_AIVDM_buffer[i].num == (index + 1) && _AIVDM_buffer[i].total == _incoming.total && _AIVDM_buffer[i].ID == _incoming.ID) {
159 13 : msg_parts[index] = i;
160 13 : index++;
161 13 : if (index >= parts) {
162 13 : break;
163 : }
164 : }
165 : }
166 :
167 : // did we find the right number?
168 13 : if (parts != index) {
169 : // could not find all of the message, save messages
170 : #if HAL_LOGGING_ENABLED
171 0 : if (log_unsupported) {
172 0 : for (uint8_t i = 0; i < index; i++) {
173 0 : log_raw(&_AIVDM_buffer[msg_parts[i]]);
174 : }
175 0 : log_raw(&_incoming);
176 : }
177 : #endif
178 : // remove
179 0 : for (uint8_t i = 0; i < index; i++) {
180 0 : buffer_shift(msg_parts[i]);
181 : }
182 0 : continue;
183 0 : }
184 :
185 : // combine packets
186 13 : ExpandingString s;
187 13 : s.append(_AIVDM_buffer[msg_parts[0]].payload, strlen(_AIVDM_buffer[msg_parts[0]].payload));
188 13 : for (uint8_t i = 1; i < index; i++) {
189 0 : s.append(_AIVDM_buffer[msg_parts[i]].payload, strlen(_AIVDM_buffer[msg_parts[i]].payload));
190 : }
191 13 : s.append(_incoming.payload, strlen(_incoming.payload));
192 13 : const bool decoded = payload_decode(s.get_string());
193 : #if !HAL_LOGGING_ENABLED
194 : (void)decoded;
195 : #endif
196 26 : for (uint8_t i = 0; i < index; i++) {
197 : #if HAL_LOGGING_ENABLED
198 : // unsupported type, log and discard
199 13 : if (!decoded && log_unsupported) {
200 0 : log_raw(&_AIVDM_buffer[msg_parts[i]]);
201 : }
202 : #endif
203 13 : buffer_shift(msg_parts[i]);
204 : }
205 : #if HAL_LOGGING_ENABLED
206 13 : if (!decoded && log_unsupported) {
207 0 : log_raw(&_incoming);
208 : }
209 : #endif
210 26 : } else {
211 : // multi part message, store in buffer
212 13 : bool fits_in = false;
213 13 : for (uint8_t i = 0; i < AIVDM_BUFFER_SIZE; i++) {
214 : // find the first free spot
215 13 : if (_AIVDM_buffer[i].num == 0 && _AIVDM_buffer[i].total == 0 && _AIVDM_buffer[i].ID == 0) {
216 13 : _AIVDM_buffer[i] = _incoming;
217 13 : fits_in = true;
218 13 : break;
219 : }
220 : }
221 13 : if (!fits_in) {
222 : // remove the oldest message
223 : #if HAL_LOGGING_ENABLED
224 0 : if (log_unsupported) {
225 : // log the unused message before removing it
226 0 : log_raw(&_AIVDM_buffer[0]);
227 : }
228 : #endif
229 0 : buffer_shift(0);
230 0 : _AIVDM_buffer[AIVDM_BUFFER_SIZE - 1] = _incoming;
231 : }
232 : }
233 : }
234 : }
235 :
236 : // remove expired items from the list
237 192 : const uint32_t now = AP_HAL::millis();
238 192 : const uint32_t timeout = _time_out * 1000;
239 192 : if (now < timeout) {
240 192 : return;
241 : }
242 0 : const uint32_t deadline = now - timeout;
243 0 : for (uint16_t i = 0; i < _list.max_items(); i++) {
244 0 : if (_list[i].last_update_ms < deadline && _list[i].last_update_ms != 0) {
245 0 : _list[i].reset();
246 : }
247 : }
248 : }
249 :
250 : // Send a AIS mavlink message
251 124162 : void AP_AIS::send(mavlink_channel_t chan)
252 : {
253 124162 : if (!enabled()) {
254 123756 : return;
255 : }
256 :
257 406 : const uint16_t list_size = _list.max_items();
258 406 : const uint32_t now = AP_HAL::millis();
259 406 : uint16_t search_length = 0;
260 2835 : while (search_length < list_size) {
261 2475 : _send_index++;
262 2475 : search_length++;
263 2475 : if (_send_index == list_size) {
264 309 : _send_index = 0;
265 : }
266 3871 : if (_list[_send_index].last_update_ms != 0 &&
267 1396 : (_list[_send_index].last_send_ms < _list[_send_index].last_update_ms || now -_list[_send_index].last_send_ms > 30000)) {
268 : // only re-send if there has been a change or the resend time has expired
269 46 : _list[_send_index].last_send_ms = now;
270 46 : _list[_send_index].info.tslc = (now - _list[_send_index].last_update_ms) * 0.001;
271 46 : mavlink_msg_ais_vessel_send_struct(chan,&_list[_send_index].info);
272 46 : return;
273 : }
274 : }
275 : }
276 :
277 : #if AP_OADATABASE_ENABLED
278 : // Send a AIS vessel to the object avoidance database if its position is valid
279 45 : void AP_AIS::send_to_object_avoidance_database(const struct ais_vehicle_t &vessel)
280 : {
281 : // No point if database is not enabled
282 45 : AP_OADatabase *oaDb = AP::oadatabase();
283 45 : if (oaDb == nullptr || !oaDb->healthy()) {
284 45 : return;
285 : }
286 :
287 : // Need a location to avoid
288 0 : if (!check_location(vessel.info.lat, vessel.info.lon)) {
289 0 : return;
290 : }
291 :
292 : // Populate location object
293 0 : const Location loc { vessel.info.lat, vessel.info.lon, 0, Location::AltFrame::ABOVE_ORIGIN };
294 :
295 : // Get position relative to origin
296 0 : Vector3f pos;
297 0 : if (!loc.get_vector_from_origin_NEU_m(pos)) {
298 0 : return;
299 : }
300 :
301 : // Need current position to calculate distance
302 0 : Vector2f current_pos;
303 0 : if (!AP::ahrs().get_relative_position_NE_origin_float(current_pos)) {
304 0 : return;
305 : }
306 0 : float distance = (pos.xy() - current_pos).length();
307 :
308 0 : if ((vessel.info.flags & AIS_FLAGS_VALID_DIMENSIONS) == 0) {
309 : // No dimensions, let the database calculate from the configured beam width
310 0 : oaDb->queue_push(pos, vessel.last_update_ms, distance, AP_OADatabase::OA_DbItem::Source::AIS, vessel.info.MMSI);
311 0 : return;
312 : }
313 :
314 : // Strictly even if we do have a valid dimension the "LARGE_DIMENSION" flags could be set meaning
315 : // the value is larger than what can be sent over AIS. Depending on where the receiver is mounted
316 : // on the vessel this may happen on vessels longer than 511m or wider than 63m it will always happen
317 : // on vessels longer than 1022m or wider than 126m
318 : // There is currently no real vessel that would cause this.
319 :
320 0 : if (vessel.info.heading > 360 * 100) {
321 : // Heading invalid, use max dimension as radius
322 0 : float radius = vessel.info.dimension_bow;
323 0 : radius = MAX(radius, vessel.info.dimension_stern);
324 0 : radius = MAX(radius, vessel.info.dimension_port);
325 0 : radius = MAX(radius, vessel.info.dimension_starboard);
326 0 : oaDb->queue_push(pos, vessel.last_update_ms, distance, radius, AP_OADatabase::OA_DbItem::Source::AIS, vessel.info.MMSI);
327 0 : return;
328 : }
329 :
330 : // With heading we can offset the location to be in the center of the vessel
331 : Vector2f offset {
332 0 : (vessel.info.dimension_bow - vessel.info.dimension_stern) * 0.5,
333 0 : (vessel.info.dimension_starboard - vessel.info.dimension_port) * 0.5
334 0 : };
335 0 : offset.rotate(cd_to_rad(vessel.info.heading));
336 0 : pos.xy() += offset;
337 :
338 : // Update distance for new position
339 0 : distance = (pos.xy() - current_pos).length();
340 :
341 : // Radius is now the largest average dimension
342 0 : const float radius = MAX(
343 0 : (vessel.info.dimension_bow + vessel.info.dimension_stern) * 0.5,
344 0 : (vessel.info.dimension_starboard + vessel.info.dimension_port) * 0.5
345 0 : );
346 :
347 0 : oaDb->queue_push(pos, vessel.last_update_ms, distance, radius, AP_OADatabase::OA_DbItem::Source::AIS, vessel.info.MMSI);
348 : }
349 : #endif
350 :
351 : // Return true if location is valid
352 45 : bool AP_AIS::check_location(int32_t lat, int32_t lng) const
353 : {
354 : // Check for zero zero
355 45 : if (lat == 0 && lng == 0) {
356 0 : return false;
357 : }
358 :
359 :
360 : // Invalid lon is sent as 181 degrees and invalid lat as 91. Check both at in range
361 45 : return check_latlng(lat, lng);
362 : }
363 :
364 : // remove the given index from the AIVDM buffer and shift following elements up
365 13 : void AP_AIS::buffer_shift(uint8_t i)
366 : {
367 130 : for (uint8_t n = i; n < (AIVDM_BUFFER_SIZE - 1); n++) {
368 117 : _AIVDM_buffer[n].ID = _AIVDM_buffer[n+1].ID;
369 117 : _AIVDM_buffer[n].num = _AIVDM_buffer[n+1].num;
370 117 : _AIVDM_buffer[n].total = _AIVDM_buffer[n+1].total;
371 117 : strncpy(_AIVDM_buffer[n].payload,_AIVDM_buffer[n+1].payload,AIVDM_PAYLOAD_SIZE);
372 : }
373 13 : _AIVDM_buffer[AIVDM_BUFFER_SIZE - 1].ID = 0;
374 13 : _AIVDM_buffer[AIVDM_BUFFER_SIZE - 1].num = 0;
375 13 : _AIVDM_buffer[AIVDM_BUFFER_SIZE - 1].total = 0;
376 13 : _AIVDM_buffer[AIVDM_BUFFER_SIZE - 1].payload[0] = 0;
377 13 : }
378 :
379 : //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
380 : // Functions related to the vessel list
381 :
382 : // find vessel index in existing list, if not then return new index if possible, returns true if index is valid
383 58 : bool AP_AIS::get_vessel_index(uint32_t mmsi, uint16_t &index, int32_t lat, int32_t lon)
384 : {
385 58 : const uint16_t list_size = _list.max_items();
386 :
387 58 : uint16_t empty = 0;
388 58 : bool found_empty = false;
389 227 : for (uint16_t i = 0; i < list_size; i++) {
390 214 : if (_list[i].info.MMSI == mmsi) {
391 45 : index = i;
392 45 : return true;
393 : }
394 169 : if (_list[i].last_update_ms == 0 && !found_empty) {
395 10 : found_empty = true;
396 10 : empty = i;
397 : }
398 : }
399 :
400 : // got through the list without a match, return the first empty
401 13 : if (found_empty) {
402 10 : index = empty;
403 10 : _list[index].reset();
404 10 : _list[index].info.MMSI = mmsi;
405 10 : return true;
406 : }
407 :
408 : // no space in the list
409 3 : if (list_size < _max_list) {
410 : // if we can try and expand
411 3 : if (_list.expand(1)) {
412 3 : index = list_size;
413 3 : _list[index].reset();
414 3 : _list[index].info.MMSI = mmsi;
415 3 : return true;
416 : }
417 : }
418 :
419 : // could not expand list, either because of memory or max list param
420 : // if we have a valid incoming location we can bump a further item from the list
421 0 : if (!check_location(lat, lon)) {
422 0 : return false;
423 : }
424 :
425 0 : Location current_loc;
426 0 : if (!AP::ahrs().get_location(current_loc)) {
427 0 : return false;
428 : }
429 :
430 0 : Location loc;
431 : float dist;
432 0 : float max_dist = 0;
433 0 : for (uint16_t i = 0; i < list_size; i++) {
434 0 : if (!check_location(_list[i].info.lat, _list[i].info.lon)) {
435 : // Replace vessel with invalid location
436 0 : index = i;
437 0 : _list[index].reset();
438 0 : _list[index].info.MMSI = mmsi;
439 0 : return true;
440 : }
441 0 : loc.lat = _list[i].info.lat;
442 0 : loc.lng = _list[i].info.lon;
443 0 : dist = loc.get_distance(current_loc);
444 0 : if (dist > max_dist) {
445 0 : max_dist = dist;
446 0 : index = i;
447 : }
448 : }
449 :
450 : // find the current distance
451 0 : loc.lat = lat;
452 0 : loc.lng = lon;
453 0 : dist = loc.get_distance(current_loc);
454 :
455 0 : if (dist < max_dist) {
456 : // Replace the furthest vessel
457 0 : _list[index].reset();
458 0 : _list[index].info.MMSI = mmsi;
459 0 : return true;
460 : }
461 :
462 0 : return false;
463 : }
464 :
465 : //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
466 : // Functions for decoding AIVDM payload messages
467 :
468 58 : bool AP_AIS::payload_decode(const char *payload)
469 : {
470 : // the message type is defined by the first character
471 58 : const uint8_t type = payload_char_decode(payload[0]);
472 :
473 58 : switch (type) {
474 45 : case 1: // Position Report Class A
475 : case 2: // Position Report Class A (Assigned schedule)
476 : case 3: // Position Report Class A (Response to interrogation)
477 45 : return decode_position_report(payload, type);
478 0 : case 4: // Base Station Report
479 0 : return decode_base_station_report(payload);
480 13 : case 5: // Static and Voyage Related Data
481 13 : return decode_static_and_voyage_data(payload);
482 :
483 0 : case 18: // Standard Class B CS Position Report
484 : case 19: // Extended Class B CS Position Report
485 0 : return decode_class_B_position_report(payload, type);
486 :
487 0 : default:
488 0 : return false;
489 : }
490 : }
491 :
492 : // Apply scale to lat lon fields, avoiding integer overflow
493 90 : int32_t AP_AIS::scale_lat_lon(const int32_t val) const
494 : {
495 : // This is 16.66666...
496 90 : const double scale_factor = (1.0 / 600000.0) * 1e7;
497 :
498 : // Because the scale factor is larger than 1 if we do this straight into the integer its
499 : // possible that the value would overflow causing a floating point error
500 : // That should never happen with correctly formatted NMEA, but can happen with bad data.
501 90 : const double ret = val * scale_factor;
502 90 : if (ret > INT32_MAX || ret < INT32_MIN) {
503 : // This is outside the range of valid lat/lon
504 : // This means that both the lat and lon will be ignored
505 : // If we just set to 0 the lat/lon pair might look valid
506 0 : return INT32_MAX;
507 : }
508 90 : return ret;
509 : }
510 :
511 45 : bool AP_AIS::decode_position_report(const char *payload, uint8_t type)
512 : {
513 45 : if (strlen(payload) != 28) {
514 0 : return false;
515 : }
516 :
517 45 : uint8_t repeat = get_bits(payload, 6, 7);
518 45 : uint32_t mmsi = get_bits(payload, 8, 37);
519 45 : uint8_t nav = get_bits(payload, 38, 41);
520 45 : int8_t rot = get_bits_signed(payload, 42, 49);
521 45 : uint16_t sog = get_bits(payload, 50, 59);
522 45 : bool pos_acc = get_bits(payload, 60, 60);
523 45 : int32_t lon = scale_lat_lon(get_bits_signed(payload, 61, 88));
524 45 : int32_t lat = scale_lat_lon(get_bits_signed(payload, 89, 115));
525 45 : uint16_t cog = get_bits(payload, 116, 127) * 10; // convert from 0.1 deg to centi-deg
526 45 : uint16_t head = get_bits(payload, 128, 136) * 100; // convert from deg to centi-deg
527 45 : uint8_t sec_utc = get_bits(payload, 137, 142);
528 45 : uint8_t maneuver = get_bits(payload, 143, 144);
529 : // 145 - 147: spare
530 45 : bool raim = get_bits(payload, 148, 148);
531 45 : uint32_t radio = get_bits(payload, 149, 167);
532 :
533 : #if HAL_LOGGING_ENABLED
534 : // log the raw information
535 45 : if ((_log_options & AIS_OPTIONS_LOG_DECODED) != 0) {
536 45 : const struct log_AIS_msg1 pkt{
537 : LOG_PACKET_HEADER_INIT(LOG_AIS_MSG1),
538 45 : time_us : AP_HAL::micros64(),
539 : type : type,
540 : repeat : repeat,
541 : mmsi : mmsi,
542 : nav : nav,
543 : rot : rot,
544 : sog : sog,
545 : pos_acc : pos_acc,
546 : lon : lon,
547 : lat : lat,
548 : cog : cog,
549 : head : head,
550 : sec_utc : sec_utc,
551 : maneuver : maneuver,
552 45 : raim : raim,
553 : radio : radio
554 45 : };
555 45 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
556 : }
557 : #else
558 : (void)repeat;
559 : (void)sec_utc;
560 : (void)maneuver;
561 : (void)raim;
562 : (void)radio;
563 : #endif
564 :
565 : uint16_t index;
566 45 : if (!get_vessel_index(mmsi, index, lat, lon)) {
567 : // no room in the vessel list
568 0 : return true;
569 : }
570 :
571 : // Position accuracy
572 45 : _list[index].set_pos_acc(pos_acc);
573 :
574 : // Course over ground
575 45 : _list[index].set_cog(cog);
576 :
577 : // Speed over ground
578 45 : _list[index].set_sog(sog);
579 :
580 : // Rate of turn
581 45 : _list[index].set_rot(rot);
582 :
583 45 : if (check_location(lat, lon)) {
584 : // last_update_ms is only set if a valid position is received
585 45 : _list[index].info.lat = lat;
586 45 : _list[index].info.lon = lon;
587 45 : _list[index].last_update_ms = AP_HAL::millis();
588 : }
589 :
590 45 : _list[index].set_heading(head);
591 45 : _list[index].info.navigational_status = nav;
592 :
593 : #if AP_OADATABASE_ENABLED
594 45 : send_to_object_avoidance_database(_list[index]);
595 : #endif
596 :
597 45 : return true;
598 : }
599 :
600 0 : bool AP_AIS::decode_base_station_report(const char *payload)
601 : {
602 0 : if (strlen(payload) != 28) {
603 0 : return false;
604 : }
605 :
606 0 : uint8_t repeat = get_bits(payload, 6, 7);
607 0 : uint32_t mmsi = get_bits(payload, 8, 37);
608 0 : uint16_t year = get_bits(payload, 38, 51);
609 0 : uint8_t month = get_bits(payload, 52, 55);
610 0 : uint8_t day = get_bits(payload, 56, 60);
611 0 : uint8_t hour = get_bits(payload, 61, 65);
612 0 : uint8_t minute = get_bits(payload, 66, 71);
613 0 : uint8_t second = get_bits(payload, 72, 77);
614 0 : bool fix = get_bits(payload, 78, 78);
615 0 : int32_t lon = scale_lat_lon(get_bits_signed(payload, 79, 106));
616 0 : int32_t lat = scale_lat_lon(get_bits_signed(payload, 107, 133));
617 0 : uint8_t epfd = get_bits(payload, 134, 137);
618 : // 138 - 147: spare
619 0 : bool raim = get_bits(payload, 148, 148);
620 0 : uint32_t radio = get_bits(payload, 149, 167);
621 :
622 : #if HAL_LOGGING_ENABLED
623 : // log the raw information
624 0 : if ((_log_options & AIS_OPTIONS_LOG_DECODED) != 0) {
625 0 : struct log_AIS_msg4 pkt {
626 : LOG_PACKET_HEADER_INIT(LOG_AIS_MSG4),
627 0 : time_us : AP_HAL::micros64(),
628 : repeat : repeat,
629 : mmsi : mmsi,
630 : year : year,
631 : month : month,
632 : day : day,
633 : hour : hour,
634 : minute : minute,
635 : second : second,
636 0 : fix : fix,
637 : lon : lon,
638 : lat : lat,
639 : epfd : epfd,
640 0 : raim : raim,
641 : radio : radio
642 0 : };
643 0 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
644 : }
645 : #else
646 : (void)repeat;
647 : (void)year;
648 : (void)month;
649 : (void)day;
650 : (void)hour;
651 : (void)minute;
652 : (void)second;
653 : (void)fix;
654 : (void)epfd;
655 : (void)raim;
656 : (void)radio;
657 : #endif
658 :
659 : uint16_t index;
660 0 : if (!get_vessel_index(mmsi, index)) {
661 0 : return true;
662 : }
663 :
664 0 : if (check_location(lat, lon)) {
665 : // last_update_ms is only set if a valid position is received
666 0 : _list[index].info.lat = lat;
667 0 : _list[index].info.lon = lon;
668 0 : _list[index].last_update_ms = AP_HAL::millis();
669 : }
670 :
671 : #if AP_OADATABASE_ENABLED
672 0 : send_to_object_avoidance_database(_list[index]);
673 : #endif
674 :
675 0 : return true;
676 : }
677 :
678 13 : bool AP_AIS::decode_static_and_voyage_data(const char *payload)
679 : {
680 13 : if (strlen(payload) != 71) {
681 0 : return false;
682 : }
683 :
684 : // Strings to use in get_char calls
685 13 : char call_sign[8] = {};
686 13 : char name[21] = {};
687 13 : char dest[21] = {};
688 :
689 : // Make sure the strings are long enough for the bit numbers passed to `get_char`
690 : static_assert(sizeof(call_sign) > ((111 - 70) + 1)/6, "Callsign string length error");
691 : static_assert(sizeof(name) > ((231 - 112) + 1)/6, "Name string length error");
692 : static_assert(sizeof(dest) > ((421 - 302) + 1)/6, "Dest string length error");
693 :
694 : // Make sure the strings are long enough, `set_callsign` and `set_name` assume a fixed size
695 : static_assert(sizeof(call_sign) > sizeof(mavlink_ais_vessel_t::callsign), "Callsign string length error");
696 : static_assert(sizeof(name) > sizeof(mavlink_ais_vessel_t::name), "Name string length error");
697 :
698 13 : uint8_t repeat = get_bits(payload, 6, 7);
699 13 : uint32_t mmsi = get_bits(payload, 8, 37);
700 13 : uint8_t ver = get_bits(payload, 38, 39);
701 13 : uint32_t imo = get_bits(payload, 40, 69);
702 13 : get_char(payload, call_sign, 70, 111);
703 13 : get_char(payload, name, 112, 231);
704 13 : uint8_t vessel_type = get_bits(payload, 232, 239);
705 13 : uint16_t bow_dim = get_bits(payload, 240, 248);
706 13 : uint16_t stern_dim = get_bits(payload, 249, 257);
707 13 : uint8_t port_dim = get_bits(payload, 258, 263);
708 13 : uint8_t star_dim = get_bits(payload, 264, 269);
709 13 : uint8_t fix = get_bits(payload, 270, 273);
710 : //uint8_t month = get_bits(payload, 274, 277); // too much for a single log
711 : //uint8_t day = get_bits(payload, 278, 282);
712 : //uint8_t hour = get_bits(payload, 283, 287);
713 : //uint8_t minute = get_bits(payload, 288, 293);
714 13 : uint8_t draught = get_bits(payload, 294, 301);
715 13 : get_char(payload, dest, 302, 421);
716 13 : bool dte = get_bits(payload, 422, 422);
717 : // 423 - 426: spare
718 :
719 : #if HAL_LOGGING_ENABLED
720 : // log the raw information
721 13 : if ((_log_options & AIS_OPTIONS_LOG_DECODED) != 0) {
722 13 : struct log_AIS_msg5 pkt {
723 : LOG_PACKET_HEADER_INIT(LOG_AIS_MSG5),
724 13 : time_us : AP_HAL::micros64(),
725 : repeat : repeat,
726 : mmsi : mmsi,
727 : ver : ver,
728 : imo : imo,
729 : call_sign : {},
730 : name : {},
731 : vessel_type : vessel_type,
732 : bow_dim : bow_dim,
733 : stern_dim : stern_dim,
734 : port_dim : port_dim,
735 : star_dim : star_dim,
736 : fix : fix,
737 : draught : draught,
738 : dest : {},
739 13 : dte : dte
740 13 : };
741 13 : strncpy(pkt.call_sign, call_sign, sizeof(pkt.call_sign));
742 13 : strncpy(pkt.name, name, sizeof(pkt.name));
743 13 : strncpy(pkt.dest, dest, sizeof(pkt.dest));
744 13 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
745 : }
746 : #else
747 : (void)repeat;
748 : (void)ver;
749 : (void)imo;
750 : (void)fix;
751 : (void)draught;
752 : (void)dte;
753 : #endif
754 :
755 : uint16_t index;
756 13 : if (!get_vessel_index(mmsi, index)) {
757 0 : return true;
758 : }
759 :
760 : // Apply dimensions to vessel object
761 13 : _list[index].set_dimensions(bow_dim, stern_dim, port_dim, star_dim);
762 :
763 : // Set call sign and name
764 13 : _list[index].set_callsign(call_sign);
765 13 : _list[index].set_name(name);
766 :
767 13 : _list[index].info.type = vessel_type; // uint8_t Type of vessels
768 :
769 : // note that the last contact time is not updated, this message does not provide a location for a valid vessel a location must be received
770 13 : return true;
771 : }
772 :
773 : // Standard Class B CS Position Report
774 0 : bool AP_AIS::decode_class_B_position_report(const char *payload, uint8_t type)
775 : {
776 0 : const size_t len = strlen(payload);
777 :
778 0 : switch (type) {
779 0 : case 18: // Standard Class B CS Position Report
780 0 : if (len != 28) {
781 0 : return false;
782 : }
783 0 : break;
784 :
785 0 : case 19: // Extended Class B CS Position Report
786 0 : if (len != 52) {
787 0 : return false;
788 : }
789 0 : break;
790 :
791 0 : default: // Should never happen
792 0 : return false;
793 : }
794 :
795 : // Common fields between types 18 and 19
796 0 : uint8_t repeat = get_bits(payload, 6, 7);
797 0 : uint32_t mmsi = get_bits(payload, 8, 37);
798 : // 38 - 45: Regional Reserved
799 0 : uint16_t sog = get_bits(payload, 46, 55);
800 0 : bool pos_acc = get_bits(payload, 56, 56);
801 0 : int32_t lon = scale_lat_lon(get_bits_signed(payload, 57, 84));
802 0 : int32_t lat = scale_lat_lon(get_bits_signed(payload, 85, 111));
803 0 : uint16_t cog = get_bits(payload, 112, 123) * 10; // convert from 0.1 deg to centi-deg
804 0 : uint16_t head = get_bits(payload, 124, 132) * 100; // convert from deg to centi-deg
805 0 : uint8_t sec_utc = get_bits(payload, 133, 138);
806 :
807 : // From bit 139 onwards the standard and extended messages differ
808 0 : bool raim = false;
809 0 : uint32_t radio = 0;
810 :
811 0 : switch (type) {
812 0 : case 18: // Standard Class B CS Position Report
813 : // 139 - 140: Regional reserved
814 : // 141 - 141: CS Unit
815 : // 142 - 142: Display flag
816 : // 143 - 143: DSC flag
817 : // 144 - 144: Band flag
818 : // 145 - 145: Message 22 flag
819 : // 146 - 146: Assigned mode flag
820 0 : raim = get_bits(payload, 147, 147);
821 0 : radio = get_bits(payload, 148, 167);
822 0 : break;
823 :
824 0 : case 19: // Extended Class B CS Position Report
825 : // 139 - 142: Regional reserved
826 : // 143 - 262: Name
827 : // 263 - 270: vessel type
828 : // 271 - 279: bow dim
829 : // 280 - 288: stern dim
830 : // 289 - 294: port dim
831 : // 295 - 300: star dim
832 : // 301 - 304: fix
833 0 : raim = get_bits(payload, 305, 305);
834 : // 306 - 306: Assigned mode flag
835 : // 308 - 311: Spare
836 0 : break;
837 : }
838 :
839 : #if HAL_LOGGING_ENABLED
840 : // log the raw information
841 0 : if ((_log_options & AIS_OPTIONS_LOG_DECODED) != 0) {
842 0 : const struct log_AIS_msg1 pkt{
843 : LOG_PACKET_HEADER_INIT(LOG_AIS_MSG1),
844 0 : time_us : AP_HAL::micros64(),
845 : type : type,
846 : repeat : repeat,
847 : mmsi : mmsi,
848 : nav : 0,
849 : rot : 0,
850 : sog : sog,
851 : pos_acc : pos_acc,
852 : lon : lon,
853 : lat : lat,
854 : cog : cog,
855 : head : head,
856 : sec_utc : sec_utc,
857 : maneuver : 0,
858 0 : raim : raim,
859 : radio : radio
860 0 : };
861 0 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
862 : }
863 : #else
864 : (void)repeat;
865 : (void)sec_utc;
866 : (void)raim;
867 : (void)radio;
868 : #endif
869 :
870 : uint16_t index;
871 0 : if (!get_vessel_index(mmsi, index, lat, lon)) {
872 : // no room in the vessel list
873 0 : return true;
874 : }
875 :
876 : // Position accuracy
877 0 : _list[index].set_pos_acc(pos_acc);
878 :
879 : // Course over ground
880 0 : _list[index].set_cog(cog);
881 :
882 : // Speed over ground
883 0 : _list[index].set_sog(sog);
884 :
885 0 : if (type == 19) {
886 : // Fields only available in the extended message
887 0 : char name[21] = {};
888 : static_assert(sizeof(name) > sizeof(mavlink_ais_vessel_t::name), "Name string length error");
889 : static_assert(sizeof(name) > ((262 - 143) + 1)/6, "Name string length error");
890 0 : get_char(payload, name, 143, 262);
891 0 : _list[index].set_name(name);
892 :
893 0 : _list[index].info.type = get_bits(payload, 263, 270); // uint8_t Type of vessels
894 :
895 : // Dimensions
896 0 : _list[index].set_dimensions(get_bits(payload, 271, 279),
897 0 : get_bits(payload, 280, 288),
898 0 : get_bits(payload, 289, 294),
899 0 : get_bits(payload, 295, 300));
900 : }
901 :
902 0 : if (check_location(lat, lon)) {
903 : // last_update_ms is only set if a valid position is received
904 0 : _list[index].info.lat = lat;
905 0 : _list[index].info.lon = lon;
906 0 : _list[index].last_update_ms = AP_HAL::millis();
907 : }
908 :
909 0 : _list[index].set_heading(head);
910 :
911 : #if AP_OADATABASE_ENABLED
912 0 : send_to_object_avoidance_database(_list[index]);
913 : #endif
914 :
915 0 : return true;
916 : }
917 :
918 :
919 : //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
920 : // Functions for decoding AIVDM payload bits
921 :
922 : // decode bits to a char array
923 39 : void AP_AIS::get_char(const char *payload, char *array, uint16_t low, uint16_t high)
924 : {
925 39 : bool found_char = false;
926 39 : uint8_t length = ((high - low) + 1)/6;
927 650 : for (uint8_t i = length; i > 0; i--) {
928 611 : uint8_t ascii = get_bits(payload, low + (i-1)*6, (low + (i*6)) - 1);
929 611 : if (ascii < 32) {
930 611 : ascii += 64;
931 : }
932 611 : if (ascii == 64 || (ascii == 32 && !found_char)) { // '@' marks end of string, remove trailing spaces
933 481 : array[i-1] = 0;
934 : } else {
935 130 : found_char = true;
936 130 : array[i-1] = ascii;
937 : }
938 : }
939 39 : array[length] = 0; // always null terminate
940 39 : }
941 :
942 : // read the specified bits from the char array each char giving 6 bits
943 1532 : uint32_t AP_AIS::get_bits(const char *payload, uint16_t low, uint16_t high)
944 : {
945 1532 : uint8_t char_low = low / 6;
946 1532 : uint8_t bit_low = low % 6;
947 :
948 1532 : uint8_t char_high = high / 6;
949 1532 : uint8_t bit_high = (high % 6) + 1;
950 :
951 1532 : uint32_t val = 0;
952 4937 : for (uint8_t index = 0; index <= char_high - char_low; index++) {
953 3405 : uint8_t value = payload_char_decode(payload[char_low + index]);
954 3405 : uint8_t mask = 0b111111;
955 3405 : if (index == 0) {
956 1532 : mask = mask >> bit_low;
957 : }
958 3405 : value &= mask;
959 3405 : if (index == char_high - char_low) {
960 1532 : value = value >> (6 - bit_high);
961 1532 : val = val << bit_high;
962 : } else {
963 1873 : val = val << 6;
964 : }
965 :
966 3405 : val |= value;
967 : }
968 :
969 1532 : return val;
970 : }
971 :
972 : // read the specified bits from the char array each char giving 6 bits
973 : // As the values are a arbitrary length the sign bit is in the wrong place for standard length variables
974 135 : int32_t AP_AIS::get_bits_signed(const char *payload, uint16_t low, uint16_t high)
975 : {
976 135 : uint32_t value = get_bits(payload, low, high);
977 135 : if (get_bits(payload, low, low)) { // check sign bit
978 : // negative number
979 54 : return value | (UINT32_MAX << (high - low));
980 : }
981 81 : return value;
982 : }
983 :
984 : // Convert payload chars to bits
985 3463 : uint8_t AP_AIS::payload_char_decode(const char c)
986 : {
987 3463 : uint8_t value = c;
988 3463 : value -= 48;
989 3463 : if (value > 40) {
990 758 : value -= 8;
991 : }
992 3463 : return value;
993 : }
994 :
995 : //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
996 : // Functions for decoding and logging AIVDM NMEA sentence
997 :
998 : #if HAL_LOGGING_ENABLED
999 : // log a raw AIVDM a message
1000 0 : void AP_AIS::log_raw(const AIVDM *msg)
1001 : {
1002 0 : struct log_AIS_raw pkt{
1003 : LOG_PACKET_HEADER_INIT(LOG_AIS_RAW_MSG),
1004 0 : time_us : AP_HAL::micros64(),
1005 0 : num : msg->num,
1006 0 : total : msg->total,
1007 0 : ID : msg->ID,
1008 : payload : {}
1009 0 : };
1010 0 : memcpy(pkt.payload, msg->payload, sizeof(pkt.payload));
1011 0 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
1012 0 : }
1013 : #endif
1014 :
1015 : // add a single character to the buffer and attempt to decode
1016 : // returns true if a complete sentence was successfully decoded
1017 3700 : bool AP_AIS::decode(char c)
1018 : {
1019 3700 : switch (c) {
1020 426 : case ',':
1021 : // end of a term, add to checksum
1022 426 : _checksum ^= c;
1023 : FALLTHROUGH;
1024 639 : case '\r':
1025 : case '\n':
1026 : case '*':
1027 : {
1028 639 : if (_sentence_done) {
1029 71 : return false;
1030 : }
1031 :
1032 : // null terminate and decode latest term
1033 568 : _term[_term_offset] = 0;
1034 568 : bool valid_sentence = decode_latest_term();
1035 :
1036 : // move onto next term
1037 568 : _term_number++;
1038 568 : _term_offset = 0;
1039 568 : _term_is_checksum = (c == '*');
1040 568 : return valid_sentence;
1041 : }
1042 :
1043 71 : case '!': // sentence begin
1044 71 : _sentence_valid = false;
1045 71 : _term_number = 0;
1046 71 : _term_offset = 0;
1047 71 : _checksum = 0;
1048 71 : _term_is_checksum = false;
1049 71 : _sentence_done = false;
1050 71 : return false;
1051 : }
1052 :
1053 : // ordinary characters are added to term
1054 2990 : if (_term_offset < sizeof(_term) - 1) {
1055 2990 : _term[_term_offset++] = c;
1056 : }
1057 2990 : if (!_term_is_checksum) {
1058 2848 : _checksum ^= c;
1059 : }
1060 :
1061 2990 : return false;
1062 : }
1063 :
1064 : // decode the most recently consumed term
1065 : // returns true if new sentence has just passed checksum test and is validated
1066 568 : bool AP_AIS::decode_latest_term()
1067 : {
1068 : // handle the last term in a message
1069 568 : if (_term_is_checksum) {
1070 71 : _sentence_done = true;
1071 : uint8_t checksum;
1072 71 : if (!hex_twochars_to_uint8(_term, checksum)) {
1073 0 : return false;
1074 : }
1075 71 : return ((checksum == _checksum) && _sentence_valid);
1076 : }
1077 :
1078 : // the first term determines the sentence type
1079 497 : if (_term_number == 0) {
1080 71 : if (strcmp(_term, "AIVDM") == 0) {
1081 : // we found the sentence type for AIS
1082 71 : _sentence_valid = true;
1083 : }
1084 71 : return false;
1085 : }
1086 :
1087 : // if this is not the sentence we want then wait for another
1088 426 : if (!_sentence_valid) {
1089 0 : return false;
1090 : }
1091 :
1092 426 : switch (_term_number) {
1093 71 : case 1:
1094 71 : _incoming.total = strtol(_term, NULL, 10);
1095 71 : if (_incoming.total == 0) {
1096 : // Can't have a zero part message
1097 0 : _sentence_valid = false;
1098 : }
1099 71 : break;
1100 :
1101 71 : case 2:
1102 71 : _incoming.num = strtol(_term, NULL, 10);
1103 71 : if (_incoming.num > _incoming.total || _incoming.num == 0) {
1104 : // Fragment cannot be zero, fragment cannot be larger than the total number of fragments
1105 0 : _sentence_valid = false;
1106 : }
1107 71 : break;
1108 :
1109 71 : case 3:
1110 71 : _incoming.ID = 0;
1111 71 : if (strlen(_term) > 0) {
1112 26 : _incoming.ID = strtol(_term, NULL, 10);
1113 45 : } else if (_incoming.num != 1 || _incoming.total != 1) {
1114 : // only allow no ID if this is a single part message
1115 0 : _sentence_valid = false;
1116 : }
1117 71 : break;
1118 :
1119 : // case 4, chanel, either A or B, discarded
1120 :
1121 71 : case 5:
1122 71 : if (strlen(_term) == 0) {
1123 0 : _sentence_valid = false;
1124 : } else {
1125 71 : strcpy(_incoming.payload,_term);
1126 : }
1127 71 : break;
1128 :
1129 : //case 6, number of fill bits, discarded
1130 : }
1131 426 : return false;
1132 : }
1133 :
1134 : // get singleton instance
1135 124162 : AP_AIS *AP_AIS::get_singleton() {
1136 124162 : return _singleton;
1137 : }
1138 :
1139 : #else
1140 : // Dummy methods are required to allow functionality to be enabled for Rover.
1141 : // It is not possible to compile in or out the full code based on vehicle type due to limitations
1142 : // of the handling of `APM_BUILD_TYPE` define.
1143 : // These dummy methods minimise flash cost in that case.
1144 :
1145 : const AP_Param::GroupInfo AP_AIS::var_info[] = { AP_GROUPEND };
1146 1813 : AP_AIS::AP_AIS() {};
1147 :
1148 0 : bool AP_AIS::enabled() const { return false; }
1149 :
1150 1803 : void AP_AIS::init() {};
1151 688808 : void AP_AIS::update() {};
1152 0 : void AP_AIS::send(mavlink_channel_t chan) {};
1153 :
1154 990377 : AP_AIS *AP_AIS::get_singleton() { return nullptr; }
1155 :
1156 : #endif // AP_AIS_DUMMY_METHODS_ENABLED
1157 :
1158 : AP_AIS *AP_AIS::_singleton;
1159 :
1160 : #endif // AP_AIS_ENABLED
|
