Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_AIS/AP_AIS.cpp

908 lines
29 KiB
C++
Raw Blame History

/*
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 AP_AIS_ENABLED
#include <AP_Vehicle/AP_Vehicle_Type.h>
#define AP_AIS_DUMMY_METHODS_ENABLED ((AP_AIS_ENABLED == 2) && !APM_BUILD_TYPE(APM_BUILD_Rover))
#if !AP_AIS_DUMMY_METHODS_ENABLED
#include <AP_Logger/AP_Logger.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_AHRS/AP_AHRS.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()
{
if (_singleton != nullptr) {
AP_HAL::panic("AIS must be singleton");
}
_singleton = this;
AP_Param::setup_object_defaults(this, var_info);
}
// return true if AIS is enabled
bool AP_AIS::enabled() const
{
return AISType(_type.get()) != AISType::NONE;
}
// 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 = MIN(_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 HAL_LOGGING_ENABLED
if (log_all || log_unsupported) {
log_raw(&_incoming);
}
#endif
break;
}
#if HAL_LOGGING_ENABLED
if (log_all) {
log_raw(&_incoming);
}
#endif
if (_incoming.num == 1 && _incoming.total == 1) {
// single part message
if (!payload_decode(_incoming.payload) && log_unsupported) {
#if HAL_LOGGING_ENABLED
// could not decode so log
log_raw(&_incoming);
#endif
}
} 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 message 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 HAL_LOGGING_ENABLED
if (log_unsupported) {
for (uint8_t i = 0; i < index; i++) {
log_raw(&_AIVDM_buffer[msg_parts[i]]);
}
log_raw(&_incoming);
}
#endif
// 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));
#if HAL_LOGGING_ENABLED
const bool decoded = payload_decode(multi);
#endif
for (uint8_t i = 0; i < _incoming.total; i++) {
#if HAL_LOGGING_ENABLED
// unsupported type, log and discard
if (!decoded && log_unsupported) {
log_raw(&_AIVDM_buffer[msg_parts[i]]);
}
#endif
buffer_shift(msg_parts[i]);
}
#if HAL_LOGGING_ENABLED
if (!decoded && log_unsupported) {
log_raw(&_incoming);
}
#endif
} 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 HAL_LOGGING_ENABLED
if (log_unsupported) {
// log the unused message before removing it
log_raw(&_AIVDM_buffer[0]);
}
#endif
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_NOTHROW 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;
}
Location current_loc;
if (!AP::ahrs().get_location(current_loc)) {
return false;
}
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 messages
bool AP_AIS::payload_decode(const char *payload)
{
// the message 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 4: // Base Station Report
return decode_base_station_report(payload);
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);
#if HAL_LOGGING_ENABLED
// 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));
}
#else
(void)repeat;
(void)nav;
(void)rot;
(void)sog;
(void)pos_acc;
(void)cog;
(void)head;
(void)sec_utc;
(void)maneuver;
(void)raim;
(void)radio;
#endif
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_base_station_report(const char *payload)
{
if (strlen(payload) != 28) {
return false;
}
uint8_t repeat = get_bits(payload, 6, 7);
uint32_t mmsi = get_bits(payload, 8, 37);
uint16_t year = get_bits(payload, 38, 51);
uint8_t month = get_bits(payload, 52, 55);
uint8_t day = get_bits(payload, 56, 60);
uint8_t hour = get_bits(payload, 61, 65);
uint8_t minute = get_bits(payload, 66, 71);
uint8_t second = get_bits(payload, 72, 77);
bool fix = get_bits(payload, 78, 78);
int32_t lon = get_bits_signed(payload, 79, 106) * ((1.0f / 600000.0f)*1e7);
int32_t lat = get_bits_signed(payload, 107, 133) * ((1.0f / 600000.0f)*1e7);
uint8_t epfd = get_bits(payload, 134, 137);
// 138 - 147: spare
bool raim = get_bits(payload, 148, 148);
uint32_t radio = get_bits(payload, 149, 167);
#if HAL_LOGGING_ENABLED
// log the raw infomation
if ((_log_options & AIS_OPTIONS_LOG_DECODED) != 0) {
struct log_AIS_msg4 pkt {
LOG_PACKET_HEADER_INIT(LOG_AIS_MSG4),
time_us : AP_HAL::micros64(),
repeat : repeat,
mmsi : mmsi,
year : year,
month : month,
day : day,
hour : hour,
minute : minute,
second : second,
fix : fix,
lon : lon,
lat : lat,
epfd : epfd,
raim : raim,
radio : radio
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
#else
(void)repeat;
(void)year;
(void)month;
(void)day;
(void)hour;
(void)minute;
(void)second;
(void)fix;
(void)epfd;
(void)raim;
(void)radio;
#endif
uint16_t index;
if (!get_vessel_index(mmsi, index)) {
return true;
}
_list[index].info.lat = lat; // int32_t [degE7] Latitude
_list[index].info.lon = lon; // int32_t [degE7] Longitude
_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
#if HAL_LOGGING_ENABLED
// 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));
}
#else
(void)repeat;
(void)ver;
(void)imo;
(void)fix;
(void)draught;
(void)dte;
#endif
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 variables
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
#if HAL_LOGGING_ENABLED
// 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));
}
#endif
// 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;
}
// get singleton instance
AP_AIS *AP_AIS::get_singleton() {
return _singleton;
}
#else
// Dummy methods are required to allow functionality to be enabled for Rover.
// It is not possible to compile in or out the full code based on vehicle type due to limitations
// of the handling of `APM_BUILD_TYPE` define.
// These dummy methods minimise flash cost in that case.
const AP_Param::GroupInfo AP_AIS::var_info[] = { AP_GROUPEND };
AP_AIS::AP_AIS() {};
bool AP_AIS::enabled() const { return false; }
void AP_AIS::init() {};
void AP_AIS::update() {};
void AP_AIS::send(mavlink_channel_t chan) {};
AP_AIS *AP_AIS::get_singleton() { return nullptr; }
#endif // AP_AIS_DUMMY_METHODS_ENABLED
AP_AIS *AP_AIS::_singleton;
#endif // AP_AIS_ENABLED
Reference in New Issue View Git Blame Copy Permalink