mirror of https://github.com/ArduPilot/ardupilot
AP_AIS: add
This commit is contained in:
Peter Hall
Randy Mackay
parent
2a58e7097f
commit
c539f0cff5
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
// 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 <AP_Logger/AP_Logger.h>
|
||||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
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
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
#include <AP_Common/AP_ExpandingArray.h>
|
||||
|
||||
#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<ais_vehicle_t> _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
|
||||
Loading…
Reference in New Issue