diff --git a/libraries/AP_AIS/AP_AIS.cpp b/libraries/AP_AIS/AP_AIS.cpp
new file mode 100644
index 0000000000..02d1ec4e91
--- /dev/null
+++ b/libraries/AP_AIS/AP_AIS.cpp
@@ -0,0 +1,759 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+// Automatic Identification System, https://gpsd.gitlab.io/gpsd/AIVDM.html
+
+// ToDo: enable receiving of the Mavlink AIS message, type bitmask?
+
+#include "AP_AIS.h"
+
+#if HAL_AIS_ENABLED
+
+#include
+#include
+#include
+
+const AP_Param::GroupInfo AP_AIS::var_info[] = {
+
+ // @Param: TYPE
+ // @DisplayName: AIS receiver type
+ // @Description: AIS receiver type
+ // @Values: 0:None,1:NMEA AIVDM message
+ // @User: Standard
+ // @RebootRequired: True
+ AP_GROUPINFO_FLAGS("TYPE", 1, AP_AIS, _type, 0, AP_PARAM_FLAG_ENABLE),
+
+ // @Param: LIST_MAX
+ // @DisplayName: AIS vessel list size
+ // @Description: AIS list size of nearest vessels. Longer lists take longer to refresh with lower SRx_ADSB values.
+ // @Range: 1 100
+ // @User: Advanced
+ AP_GROUPINFO("LIST_MAX", 2, AP_AIS, _max_list, 25),
+
+ // @Param: TIME_OUT
+ // @DisplayName: AIS vessel time out
+ // @Description: if no updates are received in this time a vessel will be removed from the list
+ // @Units: s
+ // @Range: 1 2000
+ // @User: Advanced
+ AP_GROUPINFO("TIME_OUT", 3, AP_AIS, _time_out, 600),
+
+ // @Param: LOGGING
+ // @DisplayName: AIS logging options
+ // @Description: Bitmask of AIS logging options
+ // @Bitmask: 0:Log all AIVDM messages,1:Log only unsupported AIVDM messages,2:Log decoded messages
+ // @User: Advanced
+ AP_GROUPINFO("LOGGING", 4, AP_AIS, _log_options, AIS_OPTIONS_LOG_UNSUPPORTED_RAW | AIS_OPTIONS_LOG_DECODED),
+
+ AP_GROUPEND
+};
+
+// constructor
+AP_AIS::AP_AIS()
+{
+ AP_Param::setup_object_defaults(this, var_info);
+}
+
+// Initialize the AIS object and prepare it for use
+void AP_AIS::init()
+{
+ if (!enabled()) {
+ return;
+ }
+
+ _uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_AIS, 0);
+ if (_uart == nullptr) {
+ return;
+ }
+
+ _uart->begin(AP::serialmanager().find_baudrate(AP_SerialManager::SerialProtocol_AIS, 0));
+}
+
+// update AIS, expected to be called at 20hz
+void AP_AIS::update()
+{
+ if (!_uart || !enabled()) {
+ return;
+ }
+
+ // read any available lines
+ uint32_t nbytes = MAX(_uart->available(),1024U);
+ while (nbytes-- > 0) {
+ const int16_t byte = _uart->read();
+ if (byte == -1) {
+ break;
+ }
+ const char c = byte;
+ if (decode(c)) {
+ const bool log_all = (_log_options & AIS_OPTIONS_LOG_ALL_RAW) != 0;
+ const bool log_unsupported = ((_log_options & AIS_OPTIONS_LOG_UNSUPPORTED_RAW) != 0) && !log_all; // only log unsupported if not logging all
+
+ if (_incoming.total > AIVDM_BUFFER_SIZE) {
+ // no point in trying to decode it wont fit
+ if (log_all || log_unsupported) {
+ log_raw(&_incoming);
+ }
+ break;
+ }
+ if (log_all) {
+ log_raw(&_incoming);
+ }
+
+ if (_incoming.num == 1 && _incoming.total == 1) {
+ // single part message
+ if (!payload_decode(_incoming.payload) && log_unsupported) {
+ // could not decode so log
+ log_raw(&_incoming);
+ }
+ } else if (_incoming.num == _incoming.total) {
+ // last part of a multi part message
+ uint8_t index = 0;
+ uint8_t msg_parts[_incoming.num - 1];
+ for (uint8_t i = 0; i < AIVDM_BUFFER_SIZE; i++) {
+ // look for the rest of the message from the start of the buffer
+ // we assume the mesage has be received in the correct order
+ if (_AIVDM_buffer[i].num == (index + 1) && _AIVDM_buffer[i].total == _incoming.total && _AIVDM_buffer[i].ID == _incoming.ID) {
+ msg_parts[index] = i;
+ index++;
+ if (index >= _incoming.num) {
+ break;
+ }
+ }
+ }
+
+ // did we find the right number?
+ if (_incoming.num != index) {
+ // could not find all of the message, save messages
+ if (log_unsupported) {
+ for (uint8_t i = 0; i < index; i++) {
+ log_raw(&_AIVDM_buffer[msg_parts[i]]);
+ }
+ log_raw(&_incoming);
+ }
+ // remove
+ for (uint8_t i = 0; i < index; i++) {
+ buffer_shift(msg_parts[i]);
+ }
+ break;
+ }
+
+ // combine packets
+ char multi[AIVDM_PAYLOAD_SIZE*_incoming.total];
+ strncpy(multi,_AIVDM_buffer[msg_parts[0]].payload,AIVDM_PAYLOAD_SIZE);
+ for (uint8_t i = 1; i < _incoming.total - 1; i++) {
+ strncat(multi,_AIVDM_buffer[msg_parts[i]].payload,sizeof(multi));
+ }
+ strncat(multi,_incoming.payload,sizeof(multi));
+ const bool decoded = payload_decode(multi);
+ for (uint8_t i = 0; i < _incoming.total; i++) {
+ // unsupported type, log and discard
+ if (!decoded && log_unsupported) {
+ log_raw(&_AIVDM_buffer[msg_parts[i]]);
+ }
+ buffer_shift(msg_parts[i]);
+ }
+ if (!decoded && log_unsupported) {
+ log_raw(&_incoming);
+ }
+ } else {
+ // multi part message, store in buffer
+ bool fits_in = false;
+ for (uint8_t i = 0; i < AIVDM_BUFFER_SIZE; i++) {
+ // find the first free spot
+ if (_AIVDM_buffer[i].num == 0 && _AIVDM_buffer[i].total == 0 && _AIVDM_buffer[i].ID == 0) {
+ _AIVDM_buffer[i] = _incoming;
+ fits_in = true;
+ break;
+ }
+ }
+ if (!fits_in) {
+ // remove the oldest message
+ if (log_unsupported) {
+ // log the unused message before removing it
+ log_raw(&_AIVDM_buffer[0]);
+ }
+ buffer_shift(0);
+ _AIVDM_buffer[AIVDM_BUFFER_SIZE - 1] = _incoming;
+ }
+ }
+ }
+ }
+
+ // remove expired items from the list
+ const uint32_t now = AP_HAL::millis();
+ const uint32_t timeout = _time_out * 1000;
+ if (now < timeout) {
+ return;
+ }
+ const uint32_t deadline = now - timeout;
+ for (uint16_t i = 0; i < _list.max_items(); i++) {
+ if (_list[i].last_update_ms < deadline && _list[i].last_update_ms != 0) {
+ clear_list_item(i);
+ }
+ }
+}
+
+// Send a AIS mavlink message
+void AP_AIS::send(mavlink_channel_t chan)
+{
+ if (!enabled()) {
+ return;
+ }
+
+ const uint16_t list_size = _list.max_items();
+ const uint32_t now = AP_HAL::millis();
+ uint16_t search_length = 0;
+ while (search_length < list_size) {
+ _send_index++;
+ search_length++;
+ if (_send_index == list_size) {
+ _send_index = 0;
+ }
+ if (_list[_send_index].last_update_ms != 0 &&
+ (_list[_send_index].last_send_ms < _list[_send_index].last_update_ms || now -_list[_send_index].last_send_ms > 30000)) {
+ // only re-send if there has been a change or the resend time has expired
+ _list[_send_index].last_send_ms = now;
+ _list[_send_index].info.tslc = (now - _list[_send_index].last_update_ms) * 0.001;
+ mavlink_msg_ais_vessel_send_struct(chan,&_list[_send_index].info);
+ return;
+ }
+ }
+}
+
+// remove the given index from the AIVDM buffer and shift following elements up
+void AP_AIS::buffer_shift(uint8_t i)
+{
+ for (uint8_t n = i; n < (AIVDM_BUFFER_SIZE - 1); n++) {
+ _AIVDM_buffer[n].ID = _AIVDM_buffer[n+1].ID;
+ _AIVDM_buffer[n].num = _AIVDM_buffer[n+1].num;
+ _AIVDM_buffer[n].total = _AIVDM_buffer[n+1].total;
+ strncpy(_AIVDM_buffer[n].payload,_AIVDM_buffer[n+1].payload,AIVDM_PAYLOAD_SIZE);
+ }
+ _AIVDM_buffer[AIVDM_BUFFER_SIZE - 1].ID = 0;
+ _AIVDM_buffer[AIVDM_BUFFER_SIZE - 1].num = 0;
+ _AIVDM_buffer[AIVDM_BUFFER_SIZE - 1].total = 0;
+ _AIVDM_buffer[AIVDM_BUFFER_SIZE - 1].payload[0] = 0;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Functions related to the vessel list
+
+// find vessel index in existing list, if not then return new index if possible
+bool AP_AIS::get_vessel_index(uint32_t mmsi, uint16_t &index, uint32_t lat, uint32_t lon)
+{
+ const uint16_t list_size = _list.max_items();
+
+ uint16_t empty = 0;
+ bool found_empty = false;
+ for (uint16_t i = 0; i < list_size; i++) {
+ if (_list[i].info.MMSI == mmsi) {
+ index = i;
+ return true;
+ }
+ if (_list[i].last_update_ms == 0 && !found_empty) {
+ found_empty = true;
+ empty = i;
+ }
+ }
+
+ // got through the list without a match
+ if (found_empty) {
+ index = empty;
+ _list[index].info.MMSI = mmsi;
+ return true;
+ }
+
+ // no space in the list
+ if (list_size < _max_list) {
+ // if we can try and expand
+ if (_list.expand(1)) {
+ index = list_size;
+ _list[index].info.MMSI = mmsi;
+ return true;
+ }
+ }
+
+ // could not expand list, either because of memory or max list param
+ // if we have a valid incoming location we can bump a further item from the list
+ if (lat == 0 && lon == 0) {
+ return false;
+ }
+
+ struct Location current_loc;
+ if (!AP::ahrs().get_position(current_loc)) {
+ return false;
+ }
+
+ struct Location loc;
+ float dist;
+ float max_dist = 0;
+ for (uint16_t i = 0; i < list_size; i++) {
+ loc.lat = _list[i].info.lat;
+ loc.lng = _list[i].info.lon;
+ dist = loc.get_distance(current_loc);
+ if (dist > max_dist) {
+ max_dist = dist;
+ index = i;
+ }
+ }
+
+ // find the current distance
+ loc.lat = lat;
+ loc.lng = lon;
+ dist = loc.get_distance(current_loc);
+
+ if (dist < max_dist) {
+ clear_list_item(index);
+ _list[index].info.MMSI = mmsi;
+ return true;
+ }
+
+ return false;
+}
+
+void AP_AIS::clear_list_item(uint16_t index)
+{
+ if (index < _list.max_items()) {
+ memset(&_list[index],0,sizeof(ais_vehicle_t));
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Functions for decoding AIVDM payload mesages
+
+bool AP_AIS::payload_decode(const char *payload)
+{
+ // the mesage type is defined by the first character
+ const uint8_t type = payload_char_decode(payload[0]);
+
+ switch (type) {
+ case 1: // Position Report Class A
+ case 2: // Position Report Class A (Assigned schedule)
+ case 3: // Position Report Class A (Response to interrogation)
+ return decode_position_report(payload, type);
+ case 5: // Static and Voyage Related Data
+ return decode_static_and_voyage_data(payload);
+
+ default:
+ return false;
+ }
+}
+
+bool AP_AIS::decode_position_report(const char *payload, uint8_t type)
+{
+ if (strlen(payload) != 28) {
+ return false;
+ }
+
+ uint8_t repeat = get_bits(payload, 6, 7);
+ uint32_t mmsi = get_bits(payload, 8, 37);
+ uint8_t nav = get_bits(payload, 38, 41);
+ int8_t rot = get_bits_signed(payload, 42, 49);
+ uint16_t sog = get_bits(payload, 50, 59);
+ bool pos_acc = get_bits(payload, 60, 60);
+ int32_t lon = get_bits_signed(payload, 61, 88) * ((1.0f / 600000.0f)*1e7);
+ int32_t lat = get_bits_signed(payload, 89, 115) * ((1.0f / 600000.0f)*1e7);
+ uint16_t cog = get_bits(payload, 116, 127) * 10;
+ uint16_t head = get_bits(payload, 128, 136) * 100;
+ uint8_t sec_utc = get_bits(payload, 137, 142);
+ uint8_t maneuver = get_bits(payload, 143, 144);
+ // 145 - 147: spare
+ bool raim = get_bits(payload, 148, 148);
+ uint32_t radio = get_bits(payload, 149, 167);
+
+ // log the raw infomation
+ if ((_log_options & AIS_OPTIONS_LOG_DECODED) != 0) {
+ const struct log_AIS_msg1 pkt{
+ LOG_PACKET_HEADER_INIT(LOG_AIS_MSG1),
+ time_us : AP_HAL::micros64(),
+ type : type,
+ repeat : repeat,
+ mmsi : mmsi,
+ nav : nav,
+ rot : rot,
+ sog : sog,
+ pos_acc : pos_acc,
+ lon : lon,
+ lat : lat,
+ cog : cog,
+ head : head,
+ sec_utc : sec_utc,
+ maneuver : maneuver,
+ raim : raim,
+ radio : radio
+ };
+ AP::logger().WriteBlock(&pkt, sizeof(pkt));
+ }
+
+ uint16_t index;
+ if (!get_vessel_index(mmsi, index, lat, lon)) {
+ // no room in the vessel list
+ return true;
+ }
+
+ // mask of flags that we receive in this message
+ const uint16_t mask = ~(AIS_FLAGS_POSITION_ACCURACY | AIS_FLAGS_VALID_COG | AIS_FLAGS_VALID_VELOCITY | AIS_FLAGS_VALID_TURN_RATE | AIS_FLAGS_TURN_RATE_SIGN_ONLY);
+ uint16_t flags = _list[index].info.flags & mask; // clear all flags that will be updated
+ if (pos_acc) {
+ flags |= AIS_FLAGS_POSITION_ACCURACY;
+ }
+ if (cog < 36000) {
+ flags |= AIS_FLAGS_VALID_COG;
+ }
+ if (sog < 1023) {
+ flags |= AIS_FLAGS_VALID_VELOCITY;
+ }
+ if (sog == 1022) {
+ flags |= AIS_FLAGS_HIGH_VELOCITY;
+ }
+ if (rot > -128) {
+ flags |= AIS_FLAGS_VALID_TURN_RATE;
+ }
+ if (rot == 127 || rot == -127) {
+ flags |= AIS_FLAGS_TURN_RATE_SIGN_ONLY;
+ } else {
+ rot = powf((rot / 4.733f),2.0f) / 6.0f;
+ }
+
+ _list[index].info.lat = lat; // int32_t [degE7] Latitude
+ _list[index].info.lon = lon; // int32_t [degE7] Longitude
+ _list[index].info.COG = cog; // uint16_t [cdeg] Course over ground
+ _list[index].info.heading = head; // uint16_t [cdeg] True heading
+ _list[index].info.velocity = sog; // uint16_t [cm/s] Speed over ground
+ _list[index].info.flags = flags; // uint16_t Bitmask to indicate various statuses including valid data fields
+ _list[index].info.turn_rate = rot; // int8_t [cdeg/s] Turn rate
+ _list[index].info.navigational_status = nav; // uint8_t Navigational status
+ _list[index].last_update_ms = AP_HAL::millis();
+
+ return true;
+}
+
+bool AP_AIS::decode_static_and_voyage_data(const char *payload)
+{
+ if (strlen(payload) != 71) {
+ return false;
+ }
+
+ char call_sign[8];
+ char name[21];
+ char dest[21];
+
+ uint8_t repeat = get_bits(payload, 6, 7);
+ uint32_t mmsi = get_bits(payload, 8, 37);
+ uint8_t ver = get_bits(payload, 38, 39);
+ uint32_t imo = get_bits(payload, 40, 69);
+ get_char(payload, call_sign, 70, 111);
+ get_char(payload, name, 112, 231);
+ uint8_t vessel_type = get_bits(payload, 232, 239);
+ uint16_t bow_dim = get_bits(payload, 240, 248);
+ uint16_t stern_dim = get_bits(payload, 249, 257);
+ uint8_t port_dim = get_bits(payload, 258, 263);
+ uint8_t star_dim = get_bits(payload, 264, 269);
+ uint8_t fix = get_bits(payload, 270, 273);
+ //uint8_t month = get_bits(payload, 274, 277); // too much for a single log
+ //uint8_t day = get_bits(payload, 278, 282);
+ //uint8_t hour = get_bits(payload, 283, 287);
+ //uint8_t minute = get_bits(payload, 288, 293);
+ uint8_t draught = get_bits(payload, 294, 301);
+ get_char(payload, dest, 302, 421);
+ bool dte = get_bits(payload, 422, 422);
+ // 423 - 426: spare
+
+ // log the raw infomation
+ if ((_log_options & AIS_OPTIONS_LOG_DECODED) != 0) {
+ struct log_AIS_msg5 pkt {
+ LOG_PACKET_HEADER_INIT(LOG_AIS_MSG5),
+ time_us : AP_HAL::micros64(),
+ repeat : repeat,
+ mmsi : mmsi,
+ ver : ver,
+ imo : imo,
+ call_sign : {},
+ name : {},
+ vessel_type : vessel_type,
+ bow_dim : bow_dim,
+ stern_dim : stern_dim,
+ port_dim : port_dim,
+ star_dim : star_dim,
+ fix : fix,
+ draught : draught,
+ dest : {},
+ dte : dte
+ };
+ strncpy(pkt.call_sign, call_sign, sizeof(pkt.call_sign));
+ strncpy(pkt.name, name, sizeof(pkt.name));
+ strncpy(pkt.dest, dest, sizeof(pkt.dest));
+ AP::logger().WriteBlock(&pkt, sizeof(pkt));
+ }
+
+ uint16_t index;
+ if (!get_vessel_index(mmsi, index)) {
+ return true;
+ }
+
+ // mask of flags that we receive in this message
+ const uint16_t mask = ~(AIS_FLAGS_VALID_DIMENSIONS | AIS_FLAGS_LARGE_BOW_DIMENSION | AIS_FLAGS_LARGE_STERN_DIMENSION | AIS_FLAGS_LARGE_STARBOARD_DIMENSION | AIS_FLAGS_VALID_CALLSIGN | AIS_FLAGS_VALID_NAME);
+ uint16_t flags = _list[index].info.flags & mask; // clear all flags that will be updated
+ if (bow_dim != 0 && stern_dim != 0 && port_dim != 0 && star_dim != 0) {
+ flags |= AIS_FLAGS_VALID_DIMENSIONS;
+ if (bow_dim == 511) {
+ flags |= AIS_FLAGS_LARGE_BOW_DIMENSION;
+ }
+ if (stern_dim == 511) {
+ flags |= AIS_FLAGS_LARGE_STERN_DIMENSION;
+ }
+ if (port_dim == 63) {
+ flags |= AIS_FLAGS_LARGE_PORT_DIMENSION;
+ }
+ if (star_dim == 63) {
+ flags |= AIS_FLAGS_LARGE_STARBOARD_DIMENSION;
+ }
+ }
+ if (strlen(call_sign) != 0) {
+ flags |= AIS_FLAGS_VALID_CALLSIGN;
+ }
+ if (strlen(name) != 0) {
+ flags |= AIS_FLAGS_VALID_NAME;
+ }
+
+ _list[index].info.dimension_bow = bow_dim; // uint16_t [m] Distance from lat/lon location to bow
+ _list[index].info.dimension_stern = stern_dim; // uint16_t [m] Distance from lat/lon location to stern
+ _list[index].info.flags = flags; // uint16_t Bitmask to indicate various statuses including valid data fields
+ _list[index].info.type = vessel_type; // uint8_t Type of vessels
+ _list[index].info.dimension_port = port_dim; // uint8_t [m] Distance from lat/lon location to port side
+ _list[index].info.dimension_starboard = star_dim; // uint8_t [m] Distance from lat/lon location to starboard side
+ memcpy(_list[index].info.callsign,call_sign,sizeof(_list[index].info.callsign)); // char The vessel callsign
+ memcpy(_list[index].info.name,name,sizeof(_list[index].info.name)); // char The vessel name
+
+ // 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
+
+ return true;
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Functions for decoding AIVDM payload bits
+
+// decode bits to a char array
+void AP_AIS::get_char(const char *payload, char *array, uint16_t low, uint16_t high)
+{
+ bool found_char = false;
+ uint8_t length = ((high - low) + 1)/6;
+ for (uint8_t i = length; i > 0; i--) {
+ uint8_t ascii = get_bits(payload, low + (i-1)*6, (low + (i*6)) - 1);
+ if (ascii < 32) {
+ ascii += 64;
+ }
+ if (ascii == 64 || (ascii == 32 && !found_char)) { // '@' marks end of string, remove trailing spaces
+ array[i-1] = 0;
+ } else {
+ found_char = true;
+ array[i-1] = ascii;
+ }
+ }
+ array[length] = 0; // always null terminate
+}
+
+// read the specified bits from the char array each char giving 6 bits
+uint32_t AP_AIS::get_bits(const char *payload, uint16_t low, uint16_t high)
+{
+ uint8_t char_low = low / 6;
+ uint8_t bit_low = low % 6;
+
+ uint8_t char_high = high / 6;
+ uint8_t bit_high = (high % 6) + 1;
+
+ uint32_t val = 0;
+ for (uint8_t index = 0; index <= char_high - char_low; index++) {
+ uint8_t value = payload_char_decode(payload[char_low + index]);
+ uint8_t mask = 0b111111;
+ if (index == 0) {
+ mask = mask >> bit_low;
+ }
+ value &= mask;
+ if (index == char_high - char_low) {
+ value = value >> (6 - bit_high);
+ val = val << bit_high;
+ } else {
+ val = val << 6;
+ }
+
+ val |= value;
+ }
+
+ return val;
+}
+
+// read the specified bits from the char array each char giving 6 bits
+// As the values are a arbitrary length the sign bit is in the wrong place for standard length varables
+int32_t AP_AIS::get_bits_signed(const char *payload, uint16_t low, uint16_t high)
+{
+ uint32_t value = get_bits(payload, low, high);
+ if (get_bits(payload, low, low)) { // check sign bit
+ // negative number
+ return value | (UINT32_MAX << (high - low));
+ }
+ return value;
+}
+
+// Convert payload chars to bits
+uint8_t AP_AIS::payload_char_decode(const char c)
+{
+ uint8_t value = c;
+ value -= 48;
+ if (value > 40) {
+ value -= 8;
+ }
+ return value;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Functions for decoding and logging AIVDM NMEA sentence
+
+// log a raw AIVDM a message
+void AP_AIS::log_raw(const AIVDM *msg)
+{
+ struct log_AIS_raw pkt{
+ LOG_PACKET_HEADER_INIT(LOG_AIS_RAW_MSG),
+ time_us : AP_HAL::micros64(),
+ num : msg->num,
+ total : msg->total,
+ ID : msg->ID,
+ payload : {}
+ };
+ memcpy(pkt.payload, msg->payload, sizeof(pkt.payload));
+ AP::logger().WriteBlock(&pkt, sizeof(pkt));
+}
+
+// add a single character to the buffer and attempt to decode
+// returns true if a complete sentence was successfully decoded
+bool AP_AIS::decode(char c)
+{
+ switch (c) {
+ case ',':
+ // end of a term, add to checksum
+ _checksum ^= c;
+ FALLTHROUGH;
+ case '\r':
+ case '\n':
+ case '*':
+ {
+ if (_sentence_done) {
+ return false;
+ }
+
+ // null terminate and decode latest term
+ _term[_term_offset] = 0;
+ bool valid_sentence = decode_latest_term();
+
+ // move onto next term
+ _term_number++;
+ _term_offset = 0;
+ _term_is_checksum = (c == '*');
+ return valid_sentence;
+ }
+
+ case '!': // sentence begin
+ _sentence_valid = false;
+ _term_number = 0;
+ _term_offset = 0;
+ _checksum = 0;
+ _term_is_checksum = false;
+ _sentence_done = false;
+ return false;
+ }
+
+ // ordinary characters are added to term
+ if (_term_offset < sizeof(_term) - 1) {
+ _term[_term_offset++] = c;
+ }
+ if (!_term_is_checksum) {
+ _checksum ^= c;
+ }
+
+ return false;
+}
+
+// decode the most recently consumed term
+// returns true if new sentence has just passed checksum test and is validated
+bool AP_AIS::decode_latest_term()
+{
+ // handle the last term in a message
+ if (_term_is_checksum) {
+ _sentence_done = true;
+ uint8_t checksum = 16 * char_to_hex(_term[0]) + char_to_hex(_term[1]);
+ return ((checksum == _checksum) && _sentence_valid);
+ }
+
+ // the first term determines the sentence type
+ if (_term_number == 0) {
+ if (strcmp(_term, "AIVDM") == 0) {
+ // we found the sentence type for AIS
+ _sentence_valid = true;
+ }
+ return false;
+ }
+
+ // if this is not the sentence we want then wait for another
+ if (!_sentence_valid) {
+ return false;
+ }
+
+ switch (_term_number) {
+ case 1:
+ _incoming.total = strtol(_term, NULL, 10);
+ break;
+
+ case 2:
+ _incoming.num = strtol(_term, NULL, 10);
+ break;
+
+ case 3:
+ _incoming.ID = 0;
+ if (strlen(_term) > 0) {
+ _incoming.ID = strtol(_term, NULL, 10);
+ } else if (_incoming.num != 1 || _incoming.total != 1) {
+ // only allow no ID if this is a single part message
+ _sentence_valid = false;
+ }
+ break;
+
+ // case 4, chanel, either A or B, discarded
+
+ case 5:
+ if (strlen(_term) == 0) {
+ _sentence_valid = false;
+ } else {
+ strcpy(_incoming.payload,_term);
+ }
+ break;
+
+ //case 5, number of fill bits, discarded
+ }
+ return false;
+}
+
+// return the numeric value of an ascii hex character
+int16_t AP_AIS::char_to_hex(char a)
+{
+ if (a >= 'A' && a <= 'F')
+ return a - 'A' + 10;
+ else if (a >= 'a' && a <= 'f')
+ return a - 'a' + 10;
+ else
+ return a - '0';
+}
+
+#endif // HAL_AIS_ENABLED
diff --git a/libraries/AP_AIS/AP_AIS.h b/libraries/AP_AIS/AP_AIS.h
new file mode 100644
index 0000000000..2f52a13850
--- /dev/null
+++ b/libraries/AP_AIS/AP_AIS.h
@@ -0,0 +1,138 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+#pragma once
+
+#include
+#include
+#include
+
+#ifndef HAL_AIS_ENABLED
+#define HAL_AIS_ENABLED !HAL_MINIMIZE_FEATURES
+#endif
+
+#if HAL_AIS_ENABLED
+
+#define AIVDM_BUFFER_SIZE 10
+#define AIVDM_PAYLOAD_SIZE 65
+
+class AP_AIS
+{
+public:
+ AP_AIS();
+
+ /* Do not allow copies */
+ AP_AIS(const AP_AIS &other) = delete;
+ AP_AIS &operator=(const AP_AIS&) = delete;
+
+ // return true if AIS is enabled
+ bool enabled() const { return AISType(_type.get()) != AISType::NONE; }
+
+ // Initialize the AIS object and prepare it for use
+ void init();
+
+ // update AIS, expected to be called at 20hz
+ void update();
+
+ // send mavlink AIS message
+ void send(mavlink_channel_t chan);
+
+ // parameter block
+ static const struct AP_Param::GroupInfo var_info[];
+
+private:
+
+ // parameters
+ AP_Int8 _type; // type of AIS receiver
+ AP_Int16 _max_list; // maximum number of vessels to track at once
+ AP_Int16 _time_out; // time in seconds that a vessel will be dropped from the list
+ AP_Int16 _log_options; // logging options bitmask
+
+ enum class AISType {
+ NONE = 0,
+ NMEA = 1,
+ };
+
+ enum options {
+ AIS_OPTIONS_LOG_ALL_RAW = 1<<0,
+ AIS_OPTIONS_LOG_UNSUPPORTED_RAW = 1<<1,
+ AIS_OPTIONS_LOG_DECODED = 1<<2,
+ };
+
+ struct AIVDM {
+ uint8_t num;
+ uint8_t total;
+ uint8_t ID;
+ char payload[AIVDM_PAYLOAD_SIZE];
+ };
+ AIVDM _incoming;
+ AIVDM _AIVDM_buffer[AIVDM_BUFFER_SIZE];
+
+ struct ais_vehicle_t {
+ mavlink_ais_vessel_t info;
+ uint32_t last_update_ms; // last time this was refreshed, allows timeouts
+ uint32_t last_send_ms; // last time this message was sent via mavlink, stops us spamming the link
+ };
+
+ // list of the vessels that are being tracked
+ AP_ExpandingArray _list {8};
+
+ AP_HAL::UARTDriver *_uart;
+
+ uint16_t _send_index; // index of the last vessel send over mavlink
+
+ // removed the given index from the AIVDM buffer shift following elements
+ void buffer_shift(uint8_t i);
+
+ // find vessel in existing list, if not then return new index if possible
+ bool get_vessel_index(uint32_t mmsi, uint16_t &index, uint32_t lat = 0, uint32_t lon = 0) WARN_IF_UNUSED;
+ void clear_list_item(uint16_t index);
+
+ // decode the payload
+ bool payload_decode(const char *payload) WARN_IF_UNUSED;
+
+ // decode specific message types
+ bool decode_position_report(const char *payload, uint8_t type) WARN_IF_UNUSED;
+ bool decode_static_and_voyage_data(const char *payload) WARN_IF_UNUSED;
+
+ // read the specified bits from the char array each char giving 6 bits
+ void get_char(const char *payload, char *array, uint16_t low, uint16_t high);
+ uint32_t get_bits(const char *payload, uint16_t low, uint16_t high);
+ int32_t get_bits_signed(const char *payload, uint16_t low, uint16_t high);
+ // un-encode the ASCII payload armoring
+ uint8_t payload_char_decode(const char c);
+
+ // log a raw AIVDM message
+ void log_raw(const AIVDM *msg);
+
+ // try and decode NMEA message
+ bool decode(char c) WARN_IF_UNUSED;
+
+ // decode each term
+ bool decode_latest_term() WARN_IF_UNUSED;
+
+ // convert from char to hex value for checksum
+ int16_t char_to_hex(char a);
+
+ // varables for decoding NMEA sentence
+ char _term[AIVDM_PAYLOAD_SIZE]; // buffer for the current term within the current sentence
+ uint8_t _term_offset; // offset within the _term buffer where the next character should be placed
+ uint8_t _term_number; // term index within the current sentence
+ uint8_t _checksum; // checksum accumulator
+ bool _term_is_checksum; // current term is the checksum
+ bool _sentence_valid; // is current sentence valid so far
+ bool _sentence_done; // true if this sentence has already been decoded
+};
+
+#endif // HAL_AIS_ENABLED