mirror of
https://github.com/peterantypas/maiana.git
synced 2025-05-15 23:10:11 -07:00
516 lines
15 KiB
C++
516 lines
15 KiB
C++
/*
|
|
* NMEA0183Consumer.cpp
|
|
*
|
|
* Created on: Jun 30, 2021
|
|
* Author: peter
|
|
*/
|
|
|
|
#include "NMEA0183Consumer.hpp"
|
|
#include "EventQueue.hpp"
|
|
#include <bsp/bsp.hpp>
|
|
#include <string.h>
|
|
#include "NMEASentence.hpp"
|
|
#include "Utils.hpp"
|
|
#include <list>
|
|
#include <decode_body.h>
|
|
#include <ais.h>
|
|
#include <N2kMessages.h>
|
|
#include <math.h>
|
|
|
|
using namespace std;
|
|
using namespace libais;
|
|
|
|
|
|
static const int MonthDays[] = {
|
|
31, // Jan
|
|
28, // Feb, but what about leap years?
|
|
31, // Mar
|
|
30, // Apr
|
|
31, // May
|
|
30, // Jun
|
|
31, // Jul
|
|
31, // Aug
|
|
30, // Sep
|
|
31, // Oct
|
|
30, // Nov
|
|
31, // Dec
|
|
};
|
|
|
|
NMEA0183Consumer::NMEA0183Consumer()
|
|
{
|
|
|
|
}
|
|
|
|
NMEA0183Consumer &NMEA0183Consumer::instance()
|
|
{
|
|
static NMEA0183Consumer __instance;
|
|
return __instance;
|
|
}
|
|
|
|
void onRxIRQ(char c)
|
|
{
|
|
NMEA0183Consumer::instance().onRX(c);
|
|
}
|
|
|
|
void NMEA0183Consumer::init(tNMEA2000 *n2k)
|
|
{
|
|
mN2K = n2k;
|
|
bsp_set_uart_irq_cb(onRxIRQ);
|
|
EventQueue::instance().addObserver(this, GPS_NMEA_SENTENCE | AIS_NMEA_SENTENCE);
|
|
}
|
|
|
|
void NMEA0183Consumer::onRX(char c)
|
|
{
|
|
mBuff[mBuffPos++] = c;
|
|
if (mBuffPos > 90)
|
|
{
|
|
// We screwed up!
|
|
mBuffPos = 0;
|
|
mBuff[mBuffPos] = 0;
|
|
}
|
|
else if ( c == '\r' )
|
|
{
|
|
// Do nothing
|
|
}
|
|
else if (c == '\n')
|
|
{
|
|
mBuff[mBuffPos] = 0;
|
|
Event *e = nullptr;
|
|
if ( strstr(mBuff, "$G") == mBuff )
|
|
{
|
|
e = EventPool::instance().newEvent(GPS_NMEA_SENTENCE);
|
|
}
|
|
else if ( strstr(mBuff, "!AI") == mBuff )
|
|
{
|
|
e = EventPool::instance().newEvent(AIS_NMEA_SENTENCE);
|
|
}
|
|
|
|
if ( e )
|
|
{
|
|
strlcpy(e->nmeaBuffer.sentence, mBuff, sizeof e->nmeaBuffer.sentence);
|
|
EventQueue::instance().push(e);
|
|
}
|
|
mBuffPos = 0;
|
|
mBuff[mBuffPos] = 0;
|
|
}
|
|
}
|
|
|
|
void NMEA0183Consumer::processEvent(const Event &e)
|
|
{
|
|
switch(e.type)
|
|
{
|
|
case GPS_NMEA_SENTENCE:
|
|
processGPSSentence(e.nmeaBuffer.sentence);
|
|
break;
|
|
case AIS_NMEA_SENTENCE:
|
|
processAISSentence(e.nmeaBuffer.sentence);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void NMEA0183Consumer::processGPSSentence(const char *str)
|
|
{
|
|
NMEASentence sentence(str);
|
|
|
|
if ( mUTC && (sentence.code() == "GPGGA" || sentence.code() == "GNGGA") )
|
|
{
|
|
if (sentence.fields()[2].length() == 0 || sentence.fields()[4].length() == 0)
|
|
return;
|
|
|
|
double lat = Utils::latitudeFromNMEA (sentence.fields()[2], sentence.fields()[3]);
|
|
double lng = Utils::longitudeFromNMEA (sentence.fields()[4], sentence.fields()[5]);
|
|
|
|
|
|
tN2kMsg pos;
|
|
SetN2kPGN129029(pos,
|
|
1,
|
|
(mUTC-leapSecondsSince1970()-1)/86400.0f,
|
|
fmod(mUTC-leapSecondsSince1970()-1, 86400.0f),
|
|
lat,
|
|
lng,
|
|
atof(sentence.fields()[9].c_str()), // Altitude
|
|
N2kGNSSt_GPSGLONASS, // Hard-coded, since this is MAIANA
|
|
N2kGNSSm_DGNSS, // Also hard-coded, it's always going to be this
|
|
Utils::toInt(sentence.fields()[7]), // Number of satellites
|
|
atof(sentence.fields()[8].c_str()), // HDOP
|
|
0.01, // PDOP
|
|
atof(sentence.fields()[11].c_str()), // Geoid height here
|
|
0xff, // We don't specify stations
|
|
N2kGNSSt_GPSGLONASS,
|
|
1,
|
|
1);
|
|
|
|
if ( mN2K->SendMsg(pos) )
|
|
{
|
|
printf("Sent PGN129029\r\n");
|
|
}
|
|
|
|
}
|
|
|
|
if ( sentence.code() == "GNRMC" || sentence.code() == "GPRMC" )
|
|
{
|
|
const vector<string> &fields = sentence.fields ();
|
|
|
|
if (fields.size () < 10)
|
|
return;
|
|
|
|
if (fields[1].length () < 6 || fields[9].length () < 6)
|
|
return;
|
|
|
|
double lat, lng, cog, sog;
|
|
if (sentence.fields()[3].length() > 0 && sentence.fields()[5].length() > 0)
|
|
{
|
|
lat = Utils::latitudeFromNMEA (sentence.fields()[3], sentence.fields()[4]);
|
|
lng = Utils::longitudeFromNMEA (sentence.fields()[5], sentence.fields()[6]);
|
|
tN2kMsg pos;
|
|
SetN2kLatLonRapid(pos, lat, lng);
|
|
if ( mN2K->SendMsg(pos) )
|
|
{
|
|
printf("Sent PGN129025\r\n");
|
|
}
|
|
|
|
sog = atof(sentence.fields()[7].c_str());
|
|
cog = atof(sentence.fields()[8].c_str());
|
|
|
|
const string ×tr = fields[1].substr (0, 6);
|
|
const string &datestr = fields[9].substr (0, 6);
|
|
mTime.tm_hour = Utils::toInt (timestr.substr (0, 2));
|
|
mTime.tm_min = Utils::toInt (timestr.substr (2, 2));
|
|
mTime.tm_sec = Utils::toInt (timestr.substr (4, 2));
|
|
mTime.tm_mday = Utils::toInt (datestr.substr (0, 2));
|
|
mTime.tm_mon = Utils::toInt (datestr.substr (2, 2)) - 1; // Month is 0-based
|
|
mTime.tm_year = 100 + Utils::toInt (datestr.substr (4, 2)); // Year is 1900-based
|
|
mUTC = mktime (&mTime);
|
|
|
|
|
|
tN2kMsg cogsog;
|
|
SetN2kCOGSOGRapid(cogsog,
|
|
1,
|
|
N2khr_true,
|
|
DegToRad(cog),
|
|
KnotsToms(sog));
|
|
if ( mN2K->SendMsg(cogsog) )
|
|
{
|
|
printf("Sent PGN129026\r\n");
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
int NMEA0183Consumer::leapSecondsSince1970()
|
|
{
|
|
// TODO: This needs to be updated going forward ...
|
|
return 27;
|
|
}
|
|
|
|
tN2kAISTranceiverInfo NMEA0183Consumer::transceiverInfo(const string &channel, const string &msgCode)
|
|
{
|
|
if ( msgCode == "AIVDM" )
|
|
{
|
|
return channel=="A" ? N2kaisti_Channel_A_VDL_reception : N2kaisti_Channel_B_VDL_reception;
|
|
}
|
|
else if ( msgCode == "AIVDO" )
|
|
{
|
|
return channel=="A" ? N2kaisti_Channel_A_VDL_transmission : N2kaisti_Channel_B_VDL_transmission;
|
|
}
|
|
|
|
return N2kaisti_Reserved;
|
|
}
|
|
|
|
void NMEA0183Consumer::processAISSentence(const char *str)
|
|
{
|
|
//printf("processAISSentence()\r\n");
|
|
NMEASentence s(str);
|
|
|
|
const string &chStr = s.fields()[4];
|
|
const string &code = s.code();
|
|
|
|
tN2kAISTranceiverInfo aisInfo = transceiverInfo(chStr, code);
|
|
|
|
int fragmentCount = Utils::toInt(s.fields()[1]);
|
|
if ( fragmentCount == 1 )
|
|
{
|
|
// Just process this as a complete message
|
|
list<NMEASentence> l;
|
|
l.push_back(s);
|
|
processAISBody(aisInfo, s.fields()[5], Utils::toInt(s.fields()[6]));
|
|
}
|
|
else
|
|
{
|
|
int fragmentNum = Utils::toInt(s.fields()[2]);
|
|
int messageId = Utils::toInt(s.fields()[3]);
|
|
|
|
// Accumulate the fragment
|
|
NMEAFragments::iterator it = mNMEAFragments.find(messageId);
|
|
if ( it == mNMEAFragments.end() )
|
|
{
|
|
list<NMEASentence> l;
|
|
l.push_back(s);
|
|
mNMEAFragments[messageId] = l;
|
|
}
|
|
else
|
|
{
|
|
it->second.push_back(s);
|
|
}
|
|
|
|
if ( fragmentNum == fragmentCount )
|
|
{
|
|
// This is a complete message now, process it
|
|
string body = "";
|
|
int padding = 0;
|
|
for ( list<NMEASentence>::iterator sIt = it->second.begin(); sIt != it->second.end(); ++sIt )
|
|
{
|
|
body += sIt->fields()[5];
|
|
padding = Utils::toInt(sIt->fields()[6]); // All but the last fragment will have zero bit padding
|
|
}
|
|
|
|
//printf("Body: %s\r\n", body.c_str());
|
|
processAISBody(aisInfo, body, padding);
|
|
mNMEAFragments.erase(it);
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* TODO: This function will be off by a day on leap years.
|
|
* Once GNSS data flows through this system regularly, then this can be fixed.
|
|
*/
|
|
|
|
uint16_t NMEA0183Consumer::calcETADate(int month, int day)
|
|
{
|
|
// TODO: This must really return "today" to deal with unspecified ETA, otherwise it will be
|
|
// interpreted as January 1st. I'm sure NMEA 2000 defines an "unspecified" value here
|
|
// but it's not 0xffff :(
|
|
if ( month == 0 || day == 0 )
|
|
return N2kUInt16NA;
|
|
|
|
uint16_t res = 0;
|
|
for ( int i = 0; i < month-1; i++ )
|
|
{
|
|
res += MonthDays[i];
|
|
}
|
|
res += day-1;
|
|
return res;
|
|
}
|
|
|
|
uint32_t NMEA0183Consumer::calcETATime(int hour, int minute)
|
|
{
|
|
if ( hour >= 24 || minute >= 60 )
|
|
return 0;
|
|
|
|
// This is seconds since midnight
|
|
return hour * 3600 + minute * 60;
|
|
}
|
|
|
|
void NMEA0183Consumer::processAISBody(tN2kAISTranceiverInfo aisInfo, const string &body, int bitPadding)
|
|
{
|
|
// This will delete itself when it goes out of scope
|
|
std::unique_ptr<AisMsg> msg = CreateAisMsg(body, bitPadding);
|
|
if ( !msg.get() )
|
|
{
|
|
//printf("Failed to parse AIS body\r\n");
|
|
return;
|
|
}
|
|
|
|
// printf("Parsed AIS body, msg type = %d\r\n", msg->message_id);
|
|
|
|
switch(msg->message_id)
|
|
{
|
|
case 1:
|
|
case 2:
|
|
case 3:
|
|
{
|
|
// Position report, class A => PGN 129038
|
|
Ais1_2_3 *m = static_cast<Ais1_2_3*>(msg.get());
|
|
tN2kMsg n2kmsg;
|
|
|
|
SetN2kAISClassAPosition(n2kmsg,
|
|
m->message_id,
|
|
(tN2kAISRepeat)m->repeat_indicator,
|
|
m->mmsi,
|
|
m->position.lat_deg,
|
|
m->position.lng_deg,
|
|
m->position_accuracy,
|
|
m->raim,
|
|
m->timestamp%60,
|
|
DegToRad(m->cog),
|
|
KnotsToms(m->sog),
|
|
(m->true_heading == 511 ? N2kDoubleNA : DegToRad(m->true_heading)),
|
|
(m->rot_over_range ? 3.125E-05 : m->rot),
|
|
(tN2kAISNavStatus)m->nav_status,
|
|
aisInfo);
|
|
|
|
if ( mN2K->SendMsg(n2kmsg) )
|
|
{
|
|
printf("Sent PGN129038, aisInfo=%d:\r\n", aisInfo);
|
|
}
|
|
}
|
|
break;
|
|
case 5:
|
|
{
|
|
// Static report, class A => PGN 129794
|
|
Ais5 *m = static_cast<Ais5*>(msg.get());
|
|
tN2kMsg n2kmsg;
|
|
|
|
SetN2kAISClassAStatic(n2kmsg,
|
|
m->message_id,
|
|
(tN2kAISRepeat)m->repeat_indicator,
|
|
m->mmsi,
|
|
m->imo_num,
|
|
m->callsign.c_str(),
|
|
m->name.c_str(),
|
|
m->type_and_cargo, // Does this need some bit magic, or is it type + cargo ?
|
|
m->dim_a+m->dim_b,
|
|
m->dim_c+m->dim_d,
|
|
m->dim_d,
|
|
m->dim_a,
|
|
calcETADate(m->eta_month, m->eta_day),
|
|
calcETATime(m->eta_hour, m->eta_minute),
|
|
m->draught,
|
|
m->destination.c_str(),
|
|
(tN2kAISVersion)m->ais_version,
|
|
(tN2kGNSStype)m->fix_type,
|
|
(tN2kAISDTE)m->dte,
|
|
aisInfo);
|
|
|
|
if ( mN2K->SendMsg(n2kmsg) )
|
|
{
|
|
printf("Sent PGN129794:\r\n");
|
|
}
|
|
else
|
|
{
|
|
printf("Failed to send PGN 129794\r\n");
|
|
}
|
|
}
|
|
break;
|
|
case 18:
|
|
{
|
|
// Position report, class B => PGN 12039
|
|
Ais18 *m = static_cast<Ais18*>(msg.get());
|
|
tN2kMsg n2kmsg;
|
|
|
|
SetN2kAISClassBPosition(n2kmsg,
|
|
m->message_id,
|
|
(tN2kAISRepeat)m->repeat_indicator,
|
|
m->mmsi,
|
|
m->position.lat_deg,
|
|
m->position.lng_deg,
|
|
m->position_accuracy,
|
|
m->raim,
|
|
m->timestamp%60,
|
|
DegToRad(m->cog),
|
|
KnotsToms(m->sog),
|
|
(m->true_heading == 511 ? N2kDoubleNA : DegToRad(m->true_heading)),
|
|
(tN2kAISUnit)m->unit_flag,
|
|
m->display_flag,
|
|
m->dsc_flag,
|
|
m->band_flag,
|
|
m->m22_flag,
|
|
(tN2kAISMode)m->mode_flag,
|
|
m->commstate_flag,
|
|
aisInfo);
|
|
if ( mN2K->SendMsg(n2kmsg) )
|
|
{
|
|
printf("Sent PGN129039\r\n");
|
|
}
|
|
else
|
|
{
|
|
printf("Failed to send PGN 129039\r\n");
|
|
}
|
|
}
|
|
break;
|
|
case 21:
|
|
{
|
|
// ATON report => PGN 129041
|
|
Ais21 *m = static_cast<Ais21*>(msg.get());
|
|
tN2kMsg n2kmsg;
|
|
|
|
SetN2kPGN129041(n2kmsg,
|
|
m->message_id,
|
|
(tN2kAISRepeat)m->repeat_indicator,
|
|
m->mmsi,
|
|
m->position.lat_deg,
|
|
m->position.lng_deg,
|
|
m->position_accuracy,
|
|
m->raim,
|
|
m->timestamp,
|
|
(tN2kAtoNType)m->aton_type,
|
|
m->name.c_str(),
|
|
m->dim_a+m->dim_b,
|
|
m->dim_c+m->dim_d,
|
|
m->dim_d,
|
|
m->dim_a,
|
|
m->off_pos,
|
|
m->virtual_aton,
|
|
m->assigned_mode,
|
|
(tN2kGNSStype)m->fix_type,
|
|
aisInfo);
|
|
|
|
if ( mN2K->SendMsg(n2kmsg) )
|
|
{
|
|
printf("Sent PGN129041\r\n");
|
|
}
|
|
else
|
|
{
|
|
printf("Failed to send PGN 129041\r\n");
|
|
}
|
|
}
|
|
break;
|
|
case 24:
|
|
{
|
|
// Static report, class B => PGN 129809 (part A) or PGN 129810 (part B)
|
|
Ais24 *m = static_cast<Ais24*>(msg.get());
|
|
tN2kMsg n2kmsg;
|
|
|
|
if ( m->part_num == 0 )
|
|
{
|
|
SetN2kAISClassBStaticPartA(n2kmsg,
|
|
m->message_id,
|
|
(tN2kAISRepeat)m->repeat_indicator,
|
|
m->mmsi,
|
|
m->name.c_str(),
|
|
aisInfo);
|
|
}
|
|
else
|
|
{
|
|
SetN2kAISClassBStaticPartB(n2kmsg,
|
|
m->message_id,
|
|
(tN2kAISRepeat)m->repeat_indicator,
|
|
m->mmsi,
|
|
m->type_and_cargo,
|
|
m->vendor_id.c_str(),
|
|
m->callsign.c_str(),
|
|
m->dim_a + m->dim_b,
|
|
m->dim_c+m->dim_d,
|
|
m->dim_d,
|
|
m->dim_a,
|
|
0,
|
|
aisInfo);
|
|
|
|
|
|
}
|
|
|
|
if ( mN2K->SendMsg(n2kmsg) )
|
|
{
|
|
if ( m->part_num == 0 )
|
|
{
|
|
printf("Sent PGN129809\r\n");
|
|
}
|
|
else
|
|
printf("Sent PGN129810\r\n");
|
|
}
|
|
else
|
|
{
|
|
printf("Failed to send PGN129809/810\r\n");
|
|
}
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
|