1641 lines
67 KiB
C++
1641 lines
67 KiB
C++
/*
|
||
N2kDataToNMEA0183.cpp
|
||
|
||
Copyright (c) 2015-2018 Timo Lappalainen, Kave Oy, www.kave.fi
|
||
|
||
Permission is hereby granted, free of charge, to any person obtaining a copy of
|
||
this software and associated documentation files (the "Software"), to deal in
|
||
the Software without restriction, including without limitation the rights to use,
|
||
copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
|
||
Software, and to permit persons to whom the Software is furnished to do so,
|
||
subject to the following conditions:
|
||
|
||
The above copyright notice and this permission notice shall be included in all
|
||
copies or substantial portions of the Software.
|
||
|
||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF
|
||
CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE
|
||
OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||
*/
|
||
|
||
#include <N2kMessages.h>
|
||
#include <NMEA0183Messages.h>
|
||
#include <math.h>
|
||
|
||
#include "N2kDataToNMEA0183.h"
|
||
#include "NMEA0183AISMessages.h"
|
||
#include "ConverterList.h"
|
||
#include "GwJsonDocument.h"
|
||
|
||
|
||
|
||
N2kDataToNMEA0183::N2kDataToNMEA0183(GwLog * logger, GwBoatData *boatData,
|
||
SendNMEA0183MessageCallback callback, String talkerId)
|
||
{
|
||
this->sendNMEA0183MessageCallback=callback;
|
||
strncpy(this->talkerId,talkerId.c_str(),2);
|
||
this->talkerId[2]=0;
|
||
}
|
||
|
||
|
||
|
||
//*****************************************************************************
|
||
void N2kDataToNMEA0183::loop(unsigned long) {
|
||
}
|
||
|
||
//*****************************************************************************
|
||
void N2kDataToNMEA0183::SendMessage(const tNMEA0183Msg &NMEA0183Msg) {
|
||
sendNMEA0183MessageCallback(NMEA0183Msg, sourceId);
|
||
}
|
||
|
||
/**
|
||
* The class N2kToNMEA0183Functions is intended to be used for implementing all converter
|
||
* functions
|
||
* For easy extendability the implementation can be done completely within
|
||
* this header file (java like) without the need to change multiple files
|
||
*/
|
||
|
||
class N2kToNMEA0183Functions : public N2kDataToNMEA0183
|
||
{
|
||
|
||
private:
|
||
GwXDRMappings *xdrMappings;
|
||
ConverterList<N2kToNMEA0183Functions,tN2kMsg> converters;
|
||
std::map<String,unsigned long> lastSendTransducers;
|
||
tNMEA0183Msg xdrMessage;
|
||
bool xdrOpened=false;
|
||
int xdrCount=0;
|
||
unsigned long lastRmcSent=0;
|
||
|
||
bool addToXdr(GwXDRFoundMapping::XdrEntry entry){
|
||
auto it=lastSendTransducers.find(entry.transducer);
|
||
unsigned long now=millis();
|
||
if (it != lastSendTransducers.end()){
|
||
if ((it->second + config.minXdrInterval) > now) return false;
|
||
}
|
||
lastSendTransducers[entry.transducer]=now;
|
||
if (! xdrOpened){
|
||
xdrMessage.Init("XDR",talkerId);
|
||
xdrOpened=true;
|
||
xdrCount=0;
|
||
}
|
||
int len=entry.entry.length();
|
||
if (! xdrMessage.AddStrField(entry.entry.c_str())){
|
||
SendMessage(xdrMessage);
|
||
xdrMessage.Init("XDR",talkerId);
|
||
xdrMessage.AddStrField(entry.entry.c_str());
|
||
xdrCount=1;
|
||
}
|
||
else{
|
||
xdrCount++;
|
||
}
|
||
return true;
|
||
}
|
||
bool finalizeXdr(){
|
||
if (! xdrOpened) return false;
|
||
if ( xdrCount < 1){
|
||
xdrOpened=false;
|
||
return false;
|
||
}
|
||
SendMessage(xdrMessage);
|
||
xdrOpened=false;
|
||
return true;
|
||
}
|
||
|
||
void setMax(GwBoatItem<double> *maxItem, GwBoatItem<double> *item)
|
||
{
|
||
unsigned long now = millis();
|
||
if (!item->isValid(now))
|
||
return;
|
||
maxItem->updateMax(item->getData(),sourceId);
|
||
}
|
||
bool updateDouble(GwBoatItem<double> *item, double value)
|
||
{
|
||
if (value == N2kDoubleNA)
|
||
return false;
|
||
if (!item)
|
||
return false;
|
||
return item->update(value,sourceId);
|
||
}
|
||
bool updateDouble(GwXDRFoundMapping * mapping, const double &value){
|
||
if (mapping->empty) return false;
|
||
if (value == N2kDoubleNA)
|
||
return false;
|
||
return boatData->update(value,sourceId,mapping);
|
||
}
|
||
bool updateDouble(GwXDRFoundMapping * mapping, const int8_t &value){
|
||
if (mapping->empty) return false;
|
||
if (value == N2kInt8NA)
|
||
return false;
|
||
return boatData->update((double)value,sourceId,mapping);
|
||
}
|
||
|
||
|
||
virtual unsigned long *handledPgns()
|
||
{
|
||
LOG_DEBUG(GwLog::LOG,"CONV: # %d handled PGNS", converters.numConverters());
|
||
return converters.handledPgns();
|
||
}
|
||
virtual String handledKeys(){
|
||
return converters.handledKeys();
|
||
}
|
||
virtual void HandleMsg(const tN2kMsg &N2kMsg, int sourceId)
|
||
{
|
||
this->sourceId=sourceId;
|
||
String key=String(N2kMsg.PGN);
|
||
bool rt=converters.handleMessage(key,N2kMsg,this);
|
||
if (! rt){
|
||
LOG_DEBUG(GwLog::DEBUG+1,"no handler for %ld",N2kMsg.PGN);
|
||
}
|
||
else{
|
||
LOG_DEBUG(GwLog::DEBUG+1,"handled %ld",N2kMsg.PGN);
|
||
}
|
||
}
|
||
virtual void toJson(GwJsonDocument *json)
|
||
{
|
||
converters.toJson(String(F("cnv")),json);
|
||
}
|
||
virtual int numPgns()
|
||
{
|
||
return converters.numConverters();
|
||
}
|
||
|
||
//*************** the converters ***********************
|
||
void HandleHeading(const tN2kMsg &N2kMsg)
|
||
{
|
||
unsigned char SID;
|
||
tN2kHeadingReference ref;
|
||
double _Deviation = 0;
|
||
double Variation;
|
||
tNMEA0183Msg NMEA0183Msg;
|
||
double Heading;
|
||
//if we have heading and variation we send HDG (mag.) and HDT
|
||
//if we have no variation we send either HDM or HDT
|
||
if (ParseN2kHeading(N2kMsg, SID, Heading, _Deviation, Variation, ref))
|
||
{
|
||
updateDouble(boatData->VAR,Variation);
|
||
updateDouble(boatData->DEV,_Deviation);
|
||
if (N2kIsNA(Variation)){
|
||
//maybe we still have a valid variation
|
||
Variation=boatData->VAR->getDataWithDefault(N2kDoubleNA);
|
||
}
|
||
if (N2kIsNA(Variation)){
|
||
//no variation
|
||
if (ref == N2khr_magnetic){
|
||
updateDouble(boatData->HDM,Heading);
|
||
if (NMEA0183SetHDM(NMEA0183Msg,Heading,talkerId)){
|
||
SendMessage(NMEA0183Msg);
|
||
}
|
||
}
|
||
if (ref == N2khr_true){
|
||
updateDouble(boatData->HDT,Heading);
|
||
if (NMEA0183SetHDT(NMEA0183Msg,Heading,talkerId)){
|
||
SendMessage(NMEA0183Msg);
|
||
}
|
||
}
|
||
}
|
||
else{
|
||
double MagneticHeading=N2kDoubleNA;
|
||
if (ref == N2khr_magnetic){
|
||
MagneticHeading=Heading;
|
||
Heading+=Variation;
|
||
}
|
||
if (ref == N2khr_true){
|
||
MagneticHeading=Heading-Variation;
|
||
}
|
||
updateDouble(boatData->HDM,MagneticHeading);
|
||
updateDouble(boatData->HDT,Heading);
|
||
if (!N2kIsNA(MagneticHeading)){
|
||
if (NMEA0183SetHDG(NMEA0183Msg, MagneticHeading,_Deviation,
|
||
Variation,talkerId))
|
||
{
|
||
SendMessage(NMEA0183Msg);
|
||
}
|
||
}
|
||
if (!N2kIsNA(Heading)){
|
||
if (NMEA0183SetHDT(NMEA0183Msg, Heading,talkerId))
|
||
{
|
||
SendMessage(NMEA0183Msg);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
//*****************************************************************************
|
||
void HandleVariation(const tN2kMsg &N2kMsg)
|
||
{
|
||
unsigned char SID;
|
||
tN2kMagneticVariation Source;
|
||
uint16_t DaysSince1970;
|
||
double Variation;
|
||
ParseN2kMagneticVariation(N2kMsg, SID, Source, DaysSince1970, Variation);
|
||
updateDouble(boatData->VAR, Variation);
|
||
if (DaysSince1970 != N2kUInt16NA && DaysSince1970 != 0)
|
||
boatData->GPSD->update(DaysSince1970,sourceId);
|
||
}
|
||
|
||
//*****************************************************************************
|
||
void HandleBoatSpeed(const tN2kMsg &N2kMsg)
|
||
{
|
||
unsigned char SID;
|
||
double WaterReferenced;
|
||
double GroundReferenced;
|
||
tN2kSpeedWaterReferenceType SWRT;
|
||
|
||
if (ParseN2kBoatSpeed(N2kMsg, SID, WaterReferenced, GroundReferenced, SWRT))
|
||
{
|
||
tNMEA0183Msg NMEA0183Msg;
|
||
updateDouble(boatData->STW, WaterReferenced);
|
||
unsigned long now = millis();
|
||
double MagneticHeading = (boatData->HDT->isValid(now) && boatData->VAR->isValid(now)) ? boatData->HDT->getData() + boatData->VAR->getData() : NMEA0183DoubleNA;
|
||
if (NMEA0183SetVHW(NMEA0183Msg, boatData->HDT->getDataWithDefault(NMEA0183DoubleNA), MagneticHeading, WaterReferenced,talkerId))
|
||
{
|
||
SendMessage(NMEA0183Msg);
|
||
}
|
||
}
|
||
}
|
||
|
||
//*****************************************************************************
|
||
void HandleDepth(const tN2kMsg &N2kMsg)
|
||
{
|
||
unsigned char SID;
|
||
double DepthBelowTransducer;
|
||
double Offset;
|
||
double Range;
|
||
double WaterDepth;
|
||
if (ParseN2kWaterDepth(N2kMsg, SID, DepthBelowTransducer, Offset, Range))
|
||
{
|
||
|
||
WaterDepth = DepthBelowTransducer + Offset;
|
||
updateDouble(boatData->DBS, WaterDepth);
|
||
updateDouble(boatData->DBT,DepthBelowTransducer);
|
||
tNMEA0183Msg NMEA0183Msg;
|
||
if (NMEA0183SetDPT(NMEA0183Msg, DepthBelowTransducer, Offset,talkerId))
|
||
{
|
||
SendMessage(NMEA0183Msg);
|
||
}
|
||
if (NMEA0183SetDBx(NMEA0183Msg, DepthBelowTransducer, Offset,talkerId))
|
||
{
|
||
SendMessage(NMEA0183Msg);
|
||
}
|
||
}
|
||
}
|
||
|
||
//*****************************************************************************
|
||
void HandlePosition(const tN2kMsg &N2kMsg)
|
||
{
|
||
double Latitude;
|
||
double Longitude;
|
||
if (ParseN2kPGN129025(N2kMsg, Latitude, Longitude))
|
||
{
|
||
updateDouble(boatData->LAT, Latitude);
|
||
updateDouble(boatData->LON, Longitude);
|
||
}
|
||
}
|
||
|
||
//*****************************************************************************
|
||
void HandleCOGSOG(const tN2kMsg &N2kMsg)
|
||
{
|
||
unsigned char SID;
|
||
tN2kHeadingReference HeadingReference;
|
||
tNMEA0183Msg NMEA0183Msg;
|
||
double COG;
|
||
double SOG;
|
||
if (ParseN2kCOGSOGRapid(N2kMsg, SID, HeadingReference, COG, SOG))
|
||
{
|
||
updateDouble(boatData->COG, COG);
|
||
updateDouble(boatData->SOG, SOG);
|
||
double MCOG = (!N2kIsNA(COG) && boatData->VAR->isValid()) ? COG - boatData->VAR->getData() : NMEA0183DoubleNA;
|
||
if (HeadingReference == N2khr_magnetic)
|
||
{
|
||
MCOG = COG;
|
||
if (boatData->VAR->isValid())
|
||
{
|
||
COG -= boatData->VAR->getData();
|
||
updateDouble(boatData->COG, COG);
|
||
}
|
||
}
|
||
if (NMEA0183SetVTG(NMEA0183Msg, COG, MCOG, SOG,talkerId))
|
||
{
|
||
SendMessage(NMEA0183Msg);
|
||
}
|
||
}
|
||
}
|
||
void sendGSA(bool autoMode, int fixMode)
|
||
{
|
||
if (boatData->SatInfo->isValid())
|
||
{
|
||
tNMEA0183Msg nmeaMsg;
|
||
nmeaMsg.Init("GSA", talkerId);
|
||
nmeaMsg.AddStrField(autoMode ? "A" : "M");
|
||
if (fixMode < 1)
|
||
fixMode = 1;
|
||
if (fixMode > 3)
|
||
fixMode = 1;
|
||
nmeaMsg.AddUInt32Field(fixMode);
|
||
for (int i = 0; i < 12; i++)
|
||
{
|
||
GwSatInfo *info = boatData->SatInfo->getAt(i);
|
||
if (info)
|
||
nmeaMsg.AddUInt32Field(info->PRN);
|
||
else
|
||
nmeaMsg.AddEmptyField();
|
||
}
|
||
nmeaMsg.AddDoubleField(boatData->PDOP->getDataWithDefault(NMEA0183DoubleNA));
|
||
nmeaMsg.AddDoubleField(boatData->HDOP->getDataWithDefault(NMEA0183DoubleNA));
|
||
nmeaMsg.AddDoubleField(boatData->VDOP->getDataWithDefault(NMEA0183DoubleNA));
|
||
SendMessage(nmeaMsg);
|
||
}
|
||
}
|
||
|
||
//*****************************************************************************
|
||
void HandleGNSS(const tN2kMsg &N2kMsg)
|
||
{
|
||
unsigned char SID;
|
||
tN2kGNSStype GNSStype;
|
||
tN2kGNSSmethod GNSSmethod;
|
||
unsigned char nSatellites;
|
||
double HDOP=N2kDoubleNA;
|
||
double PDOP=N2kDoubleNA;
|
||
double GeoidalSeparation=N2kDoubleNA;
|
||
unsigned char nReferenceStations;
|
||
tN2kGNSStype ReferenceStationType;
|
||
uint16_t ReferenceSationID;
|
||
double AgeOfCorrection=N2kDoubleNA;
|
||
double Latitude=N2kDoubleNA;
|
||
double Longitude=N2kDoubleNA;
|
||
double Altitude=N2kDoubleNA;
|
||
uint16_t DaysSince1970;
|
||
double GpsTime;
|
||
if (ParseN2kGNSS(N2kMsg, SID, DaysSince1970, GpsTime, Latitude, Longitude, Altitude, GNSStype, GNSSmethod,
|
||
nSatellites, HDOP, PDOP, GeoidalSeparation,
|
||
nReferenceStations, ReferenceStationType, ReferenceSationID, AgeOfCorrection))
|
||
{
|
||
updateDouble(boatData->LAT, Latitude);
|
||
updateDouble(boatData->LON, Longitude);
|
||
updateDouble(boatData->ALT, Altitude);
|
||
updateDouble(boatData->GPST, GpsTime);
|
||
updateDouble(boatData->HDOP,HDOP);
|
||
updateDouble(boatData->PDOP,PDOP);
|
||
if (DaysSince1970 != N2kUInt16NA && DaysSince1970 != 0)
|
||
boatData->GPSD->update(DaysSince1970,sourceId);
|
||
int quality=0;
|
||
if ((int)GNSSmethod <= 5) quality=(int)GNSSmethod;
|
||
tNMEA0183AISMsg nmeaMsg;
|
||
if (NMEA0183SetGGA(nmeaMsg,GpsTime,Latitude,Longitude,
|
||
quality,nSatellites,HDOP,Altitude,GeoidalSeparation,AgeOfCorrection,
|
||
ReferenceSationID,talkerId)){
|
||
SendMessage(nmeaMsg);
|
||
}
|
||
}
|
||
}
|
||
void HandleDop(const tN2kMsg &msg){
|
||
double HDOP=N2kDoubleNA;
|
||
double VDOP=N2kDoubleNA;
|
||
double TDOP=N2kDoubleNA;
|
||
tN2kGNSSDOPmode DesiredMode;
|
||
tN2kGNSSDOPmode ActualMode;
|
||
unsigned char SID;
|
||
if (ParseN2kGNSSDOPData(msg,SID, DesiredMode, ActualMode,
|
||
HDOP, VDOP, TDOP)){
|
||
updateDouble(boatData->HDOP,HDOP);
|
||
updateDouble(boatData->VDOP,VDOP);
|
||
sendGSA(DesiredMode==N2kGNSSdm_Auto,(int)ActualMode);
|
||
}
|
||
}
|
||
void HandleSats(const tN2kMsg &msg){
|
||
unsigned char SID;
|
||
tN2kRangeResidualMode Mode;
|
||
uint8_t NumberOfSVs;
|
||
tSatelliteInfo info;
|
||
if (ParseN2kPGNSatellitesInView(msg,SID,Mode,NumberOfSVs)){
|
||
for (int i=0;i<NumberOfSVs;i++){
|
||
if (ParseN2kPGNSatellitesInView(msg,i,info)){
|
||
GwSatInfo satInfo;
|
||
satInfo.PRN=info.PRN;
|
||
satInfo.Elevation=RadToDeg(info.Elevation);
|
||
satInfo.Azimut=RadToDeg(info.Azimuth);
|
||
satInfo.SNR=info.SNR;
|
||
if (! boatData->SatInfo->update(satInfo,sourceId)) return;
|
||
}
|
||
}
|
||
NumberOfSVs=boatData->SatInfo->getNumSats();
|
||
if (NumberOfSVs > 0){
|
||
LOG_DEBUG(GwLog::DEBUG+1,"send GSV for %d sats",NumberOfSVs);
|
||
tNMEA0183Msg nmeaMsg;
|
||
int numGSV=NumberOfSVs/4;
|
||
if (numGSV*4 < NumberOfSVs) numGSV++;
|
||
if (numGSV > 9) numGSV=9;
|
||
for (int i=0;i<numGSV ;i++){
|
||
int idx=i*4;
|
||
GwSatInfo *i0=boatData->SatInfo->getAt(idx);
|
||
GwSatInfo *i1=boatData->SatInfo->getAt(idx+1);
|
||
GwSatInfo *i2=boatData->SatInfo->getAt(idx+2);
|
||
GwSatInfo *i3=boatData->SatInfo->getAt(idx+3);
|
||
if (NMEA0183SetGSV(nmeaMsg,numGSV,i+1,NumberOfSVs,
|
||
i0?i0->PRN:NMEA0183UInt32NA,
|
||
i0?i0->Elevation:NMEA0183UInt32NA,
|
||
i0?i0->Azimut:NMEA0183UInt32NA,
|
||
i0?i0->SNR:NMEA0183UInt32NA,
|
||
i1?i1->PRN:NMEA0183UInt32NA,
|
||
i1?i1->Elevation:NMEA0183UInt32NA,
|
||
i1?i1->Azimut:NMEA0183UInt32NA,
|
||
i1?i1->SNR:NMEA0183UInt32NA,
|
||
i2?i2->PRN:NMEA0183UInt32NA,
|
||
i2?i2->Elevation:NMEA0183UInt32NA,
|
||
i2?i2->Azimut:NMEA0183UInt32NA,
|
||
i2?i2->SNR:NMEA0183UInt32NA,
|
||
i3?i3->PRN:NMEA0183UInt32NA,
|
||
i3?i3->Elevation:NMEA0183UInt32NA,
|
||
i3?i3->Azimut:NMEA0183UInt32NA,
|
||
i3?i3->SNR:NMEA0183UInt32NA,
|
||
talkerId
|
||
)){
|
||
SendMessage(nmeaMsg);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
//*****************************************************************************
|
||
void HandleWind(const tN2kMsg &N2kMsg)
|
||
{
|
||
unsigned char SID;
|
||
tN2kWindReference WindReference;
|
||
double WindAngle=N2kDoubleNA, WindSpeed=N2kDoubleNA;
|
||
tNMEA0183WindReference NMEA0183Reference;
|
||
if (ParseN2kWindSpeed(N2kMsg, SID, WindSpeed, WindAngle, WindReference)) {
|
||
tNMEA0183Msg NMEA0183Msg;
|
||
GwConverterConfig::WindMapping mapping=config.findWindMapping(WindReference);
|
||
bool shouldSend = false;
|
||
|
||
// MWV sentence contains apparent/true ANGLE and SPEED
|
||
// https://gpsd.gitlab.io/gpsd/NMEA.html#_mwv_wind_speed_and_angle
|
||
// https://docs.vaisala.com/r/M211109EN-L/en-US/GUID-7402DEF8-5E82-446F-B63E-998F49F3D743/GUID-C77934C7-2A72-466E-BC52-CE6B8CC7ACB6
|
||
if (mapping.valid)
|
||
{
|
||
if (mapping.nmea0183Type == GwConverterConfig::WindMapping::AWA_AWS)
|
||
{
|
||
NMEA0183Reference = NMEA0183Wind_Apparent;
|
||
updateDouble(boatData->AWA, WindAngle);
|
||
updateDouble(boatData->AWS, WindSpeed);
|
||
setMax(boatData->MaxAws, boatData->AWS);
|
||
shouldSend = true;
|
||
}
|
||
if (mapping.nmea0183Type == GwConverterConfig::WindMapping::TWA_TWS)
|
||
{
|
||
NMEA0183Reference = NMEA0183Wind_True;
|
||
updateDouble(boatData->TWA, WindAngle);
|
||
updateDouble(boatData->TWS, WindSpeed);
|
||
setMax(boatData->MaxTws, boatData->TWS);
|
||
shouldSend = true;
|
||
if (boatData->HDT->isValid())
|
||
{
|
||
double twd = WindAngle + boatData->HDT->getData();
|
||
if (twd > 2 * M_PI)
|
||
{
|
||
twd -= 2 * M_PI;
|
||
}
|
||
updateDouble(boatData->TWD, twd);
|
||
}
|
||
}
|
||
if (mapping.nmea0183Type == GwConverterConfig::WindMapping::TWD_TWS)
|
||
{
|
||
NMEA0183Reference = NMEA0183Wind_True;
|
||
updateDouble(boatData->TWD, WindAngle);
|
||
updateDouble(boatData->TWS, WindSpeed);
|
||
setMax(boatData->MaxTws, boatData->TWS);
|
||
if (boatData->HDT->isValid())
|
||
{
|
||
shouldSend = true;
|
||
double twa = WindAngle - boatData->HDT->getData();
|
||
if (twa > 2 * M_PI)
|
||
{
|
||
twa -= 2 * M_PI;
|
||
}
|
||
updateDouble(boatData->TWA, twa);
|
||
WindAngle=twa;
|
||
}
|
||
}
|
||
|
||
if (shouldSend && NMEA0183SetMWV(NMEA0183Msg, formatCourse(WindAngle), NMEA0183Reference, WindSpeed, talkerId))
|
||
{
|
||
SendMessage(NMEA0183Msg);
|
||
}
|
||
|
||
if (shouldSend && NMEA0183Reference == NMEA0183Wind_Apparent)
|
||
{
|
||
if (!NMEA0183Msg.Init("VWR", talkerId))
|
||
return;
|
||
if (!NMEA0183Msg.AddUInt32Field((abs(WindAngle) > 180 ) ? 360-abs(WindAngle) : abs(WindAngle) ))
|
||
return;
|
||
if (!NMEA0183Msg.AddStrField((WindAngle >= 0 && WindAngle <= 180) ? 'R' : 'L'))
|
||
return;
|
||
if (!NMEA0183Msg.AddDoubleField(formatKnots(WindSpeed)))
|
||
return;
|
||
if (!NMEA0183Msg.AddStrField("N"))
|
||
return;
|
||
if (!NMEA0183Msg.AddDoubleField(WindSpeed))
|
||
return;
|
||
if (!NMEA0183Msg.AddStrField("M"))
|
||
return;
|
||
if (!NMEA0183Msg.AddDoubleField(formatKmh(WindSpeed)))
|
||
return;
|
||
if (!NMEA0183Msg.AddStrField("K"))
|
||
return;
|
||
|
||
SendMessage(NMEA0183Msg);
|
||
}
|
||
}
|
||
|
||
/* if (WindReference == N2kWind_Apparent && boatData->SOG->isValid())
|
||
{ // Lets calculate and send TWS/TWA if SOG is available
|
||
|
||
double x = WindSpeed * cos(WindAngle);
|
||
double y = WindSpeed * sin(WindAngle);
|
||
|
||
updateDouble(boatData->TWD, atan2(y, -boatData->SOG->getData() + x));
|
||
updateDouble(boatData->TWS, sqrt((y * y) + ((-boatData->SOG->getData() + x) * (-boatData->SOG->getData() + x))));
|
||
|
||
setMax(boatData->MaxTws, boatData->TWS);
|
||
|
||
NMEA0183Reference = NMEA0183Wind_True;
|
||
if (NMEA0183SetMWV(NMEA0183Msg,
|
||
formatCourse(boatData->TWD->getData()),
|
||
NMEA0183Reference,
|
||
boatData->TWS->getDataWithDefault(NMEA0183DoubleNA),talkerId))
|
||
SendMessage(NMEA0183Msg);
|
||
double magnetic = boatData->TWD->getData();
|
||
if (boatData->VAR->isValid())
|
||
magnetic -= boatData->VAR->getData();
|
||
if (!NMEA0183Msg.Init("MWD", talkerId))
|
||
return;
|
||
if (!NMEA0183Msg.AddDoubleField(formatCourse(boatData->TWD->getData())))
|
||
return;
|
||
if (!NMEA0183Msg.AddStrField("T"))
|
||
return;
|
||
if (!NMEA0183Msg.AddDoubleField(formatCourse(magnetic)))
|
||
return;
|
||
if (!NMEA0183Msg.AddStrField("M"))
|
||
return;
|
||
if (!NMEA0183Msg.AddDoubleField(boatData->TWS->getData() / 0.514444))
|
||
return;
|
||
if (!NMEA0183Msg.AddStrField("N"))
|
||
return;
|
||
if (!NMEA0183Msg.AddDoubleField(boatData->TWS->getData()))
|
||
return;
|
||
if (!NMEA0183Msg.AddStrField("M"))
|
||
return;
|
||
|
||
SendMessage(NMEA0183Msg);
|
||
} */
|
||
}
|
||
}
|
||
//*****************************************************************************
|
||
void SendRMC()
|
||
{
|
||
long now = millis();
|
||
if (boatData->LAT->isValid(now) && boatData->LON->isValid(now))
|
||
{
|
||
lastRmcSent=now;
|
||
tNMEA0183Msg NMEA0183Msg;
|
||
if (NMEA0183SetRMC(NMEA0183Msg,
|
||
|
||
boatData->GPST->getDataWithDefault(NMEA0183DoubleNA),
|
||
boatData->LAT->getDataWithDefault(NMEA0183DoubleNA),
|
||
boatData->LON->getDataWithDefault(NMEA0183DoubleNA),
|
||
boatData->COG->getDataWithDefault(NMEA0183DoubleNA),
|
||
boatData->SOG->getDataWithDefault(NMEA0183DoubleNA),
|
||
boatData->GPSD->getDataWithDefault(NMEA0183UInt32NA),
|
||
boatData->VAR->getDataWithDefault(NMEA0183DoubleNA),
|
||
talkerId))
|
||
{
|
||
SendMessage(NMEA0183Msg);
|
||
}
|
||
}
|
||
}
|
||
|
||
//*****************************************************************************
|
||
void HandleLog(const tN2kMsg &N2kMsg)
|
||
{
|
||
uint16_t DaysSince1970;
|
||
double GpsTime=N2kDoubleNA;
|
||
uint32_t Log, TripLog;
|
||
if (ParseN2kDistanceLog(N2kMsg, DaysSince1970, GpsTime, Log, TripLog))
|
||
{
|
||
if (Log != N2kUInt32NA)
|
||
boatData->Log->update(Log,sourceId);
|
||
if (TripLog != N2kUInt32NA)
|
||
boatData->TripLog->update(TripLog,sourceId);
|
||
if (DaysSince1970 != N2kUInt16NA && DaysSince1970 != 0)
|
||
boatData->GPSD->update(DaysSince1970,sourceId);
|
||
tNMEA0183Msg NMEA0183Msg;
|
||
|
||
if (!NMEA0183Msg.Init("VLW", talkerId))
|
||
return;
|
||
if (!NMEA0183Msg.AddDoubleField(Log / 1852.0))
|
||
return;
|
||
if (!NMEA0183Msg.AddStrField("N"))
|
||
return;
|
||
if (!NMEA0183Msg.AddDoubleField(TripLog / 1852.0))
|
||
return;
|
||
if (!NMEA0183Msg.AddStrField("N"))
|
||
return;
|
||
|
||
SendMessage(NMEA0183Msg);
|
||
}
|
||
}
|
||
|
||
//*****************************************************************************
|
||
void HandleRudder(const tN2kMsg &N2kMsg)
|
||
{
|
||
|
||
unsigned char Instance;
|
||
tN2kRudderDirectionOrder RudderDirectionOrder;
|
||
double AngleOrder=N2kDoubleNA;
|
||
double RudderPosition=N2kDoubleNA;
|
||
|
||
if (ParseN2kRudder(N2kMsg, RudderPosition, Instance, RudderDirectionOrder, AngleOrder))
|
||
{
|
||
if (Instance == config.starboardRudderInstance){
|
||
updateDouble(boatData->RPOS, RudderPosition);
|
||
}
|
||
else if (Instance == config.portRudderInstance){
|
||
updateDouble(boatData->PRPOS, RudderPosition);
|
||
}
|
||
else{
|
||
return;
|
||
}
|
||
|
||
tNMEA0183Msg NMEA0183Msg;
|
||
|
||
if (!NMEA0183Msg.Init("RSA", talkerId))
|
||
return;
|
||
auto rpos=boatData->RPOS;
|
||
if (rpos->isValid()){
|
||
if (!NMEA0183Msg.AddDoubleField(formatWind(rpos->getData())))return;
|
||
if (!NMEA0183Msg.AddStrField("A"))return;
|
||
}
|
||
else{
|
||
if (!NMEA0183Msg.AddDoubleField(0.0))return;
|
||
if (!NMEA0183Msg.AddStrField("V"))return;
|
||
}
|
||
auto prpos=boatData->PRPOS;
|
||
if (prpos->isValid()){
|
||
if (!NMEA0183Msg.AddDoubleField(formatWind(prpos->getData())))return;
|
||
if (!NMEA0183Msg.AddStrField("A"))return;
|
||
}
|
||
else{
|
||
if (!NMEA0183Msg.AddDoubleField(0.0))return;
|
||
if (!NMEA0183Msg.AddStrField("V"))return;
|
||
}
|
||
SendMessage(NMEA0183Msg);
|
||
}
|
||
}
|
||
|
||
//*****************************************************************************
|
||
// 129038 AIS Class A Position Report (Message 1, 2, 3)
|
||
void HandleAISClassAPosReport(const tN2kMsg &N2kMsg)
|
||
{
|
||
|
||
unsigned char SID;
|
||
tN2kAISRepeat _Repeat;
|
||
uint32_t _UserID; // MMSI
|
||
double _Latitude =N2kDoubleNA;
|
||
double _Longitude=N2kDoubleNA;
|
||
bool _Accuracy;
|
||
bool _RAIM;
|
||
uint8_t _Seconds;
|
||
double _COG=N2kDoubleNA;
|
||
double _SOG=N2kDoubleNA;
|
||
double _Heading=N2kDoubleNA;
|
||
double _ROT=N2kDoubleNA;
|
||
tN2kAISNavStatus _NavStatus;
|
||
tN2kAISTransceiverInformation _AISTransceiverInformation;
|
||
uint8_t _SID;
|
||
|
||
uint8_t _MessageType = 1;
|
||
tNMEA0183AISMsg NMEA0183AISMsg;
|
||
|
||
if (ParseN2kPGN129038(N2kMsg, SID, _Repeat, _UserID, _Latitude, _Longitude, _Accuracy, _RAIM, _Seconds,
|
||
_COG, _SOG, _Heading, _ROT, _NavStatus,_AISTransceiverInformation,_SID))
|
||
{
|
||
|
||
// Debug
|
||
#ifdef SERIAL_PRINT_AIS_FIELDS
|
||
Serial.println("–––––––––––––––––––––––– Msg 1 ––––––––––––––––––––––––––––––––");
|
||
|
||
const double pi = 3.1415926535897932384626433832795;
|
||
const double radToDeg = 180.0 / pi;
|
||
const double msTokn = 3600.0 / 1852.0;
|
||
const double radsToDegMin = 60 * 360.0 / (2 * pi); // [rad/s -> degree/minute]
|
||
Serial.print("Repeat: ");
|
||
Serial.println(_Repeat);
|
||
Serial.print("UserID: ");
|
||
Serial.println(_UserID);
|
||
Serial.print("Latitude: ");
|
||
Serial.println(_Latitude);
|
||
Serial.print("Longitude: ");
|
||
Serial.println(_Longitude);
|
||
Serial.print("Accuracy: ");
|
||
Serial.println(_Accuracy);
|
||
Serial.print("RAIM: ");
|
||
Serial.println(_RAIM);
|
||
Serial.print("Seconds: ");
|
||
Serial.println(_Seconds);
|
||
Serial.print("COG: ");
|
||
Serial.println(_COG * radToDeg);
|
||
Serial.print("SOG: ");
|
||
Serial.println(_SOG * msTokn);
|
||
Serial.print("Heading: ");
|
||
Serial.println(_Heading * radToDeg);
|
||
Serial.print("ROT: ");
|
||
Serial.println(_ROT * radsToDegMin);
|
||
Serial.print("NavStatus: ");
|
||
Serial.println(_NavStatus);
|
||
#endif
|
||
|
||
if (SetAISClassABMessage1(NMEA0183AISMsg, _MessageType, _Repeat, _UserID, _Latitude, _Longitude, _Accuracy,
|
||
_RAIM, _Seconds, _COG, _SOG, _Heading, _ROT, _NavStatus))
|
||
{
|
||
|
||
SendMessage(NMEA0183AISMsg);
|
||
|
||
#ifdef SERIAL_PRINT_AIS_NMEA
|
||
// Debug Print AIS-NMEA
|
||
Serial.print(NMEA0183AISMsg.GetPrefix());
|
||
Serial.print(NMEA0183AISMsg.Sender());
|
||
Serial.print(NMEA0183AISMsg.MessageCode());
|
||
for (int i = 0; i < NMEA0183AISMsg.FieldCount(); i++)
|
||
{
|
||
Serial.print(",");
|
||
Serial.print(NMEA0183AISMsg.Field(i));
|
||
}
|
||
char buf[7];
|
||
sprintf(buf, "*%02X\r\n", NMEA0183AISMsg.GetCheckSum());
|
||
Serial.print(buf);
|
||
#endif
|
||
}
|
||
}
|
||
} // end 129038 AIS Class A Position Report Message 1/3
|
||
|
||
//*****************************************************************************
|
||
// 129039 AIS Class B Position Report -> AIS Message Type 5: Static and Voyage Related Data
|
||
void HandleAISClassAMessage5(const tN2kMsg &N2kMsg)
|
||
{
|
||
uint8_t _MessageID;
|
||
tN2kAISRepeat _Repeat;
|
||
uint32_t _UserID; // MMSI
|
||
uint32_t _IMONumber;
|
||
char _Callsign[8];
|
||
char _Name[21];
|
||
uint8_t _VesselType;
|
||
double _Length=N2kDoubleNA;
|
||
double _Beam=N2kDoubleNA;
|
||
double _PosRefStbd=N2kDoubleNA;
|
||
double _PosRefBow=N2kDoubleNA;
|
||
uint16_t _ETAdate;
|
||
double _ETAtime=N2kDoubleNA;
|
||
double _Draught=N2kDoubleNA;
|
||
char _Destination[21];
|
||
tN2kAISVersion _AISversion;
|
||
tN2kGNSStype _GNSStype;
|
||
tN2kAISTransceiverInformation _AISinfo;
|
||
tN2kAISDTE _DTE;
|
||
uint8_t _SID;
|
||
|
||
tNMEA0183AISMsg NMEA0183AISMsg;
|
||
|
||
if (ParseN2kPGN129794(N2kMsg, _MessageID, _Repeat, _UserID, _IMONumber, _Callsign, 8, _Name,21, _VesselType,
|
||
_Length, _Beam, _PosRefStbd, _PosRefBow, _ETAdate, _ETAtime, _Draught, _Destination,21,
|
||
_AISversion, _GNSStype, _DTE, _AISinfo,_SID))
|
||
{
|
||
|
||
#ifdef SERIAL_PRINT_AIS_FIELDS
|
||
// Debug Print N2k Values
|
||
Serial.println("––––––––––––––––––––––– Msg 5 –––––––––––––––––––––––––––––––––");
|
||
Serial.print("MessageID: ");
|
||
Serial.println(_MessageID);
|
||
Serial.print("Repeat: ");
|
||
Serial.println(_Repeat);
|
||
Serial.print("UserID: ");
|
||
Serial.println(_UserID);
|
||
Serial.print("IMONumber: ");
|
||
Serial.println(_IMONumber);
|
||
Serial.print("Callsign: ");
|
||
Serial.println(_Callsign);
|
||
Serial.print("VesselType: ");
|
||
Serial.println(_VesselType);
|
||
Serial.print("Name: ");
|
||
Serial.println(_Name);
|
||
Serial.print("Length: ");
|
||
Serial.println(_Length);
|
||
Serial.print("Beam: ");
|
||
Serial.println(_Beam);
|
||
Serial.print("PosRefStbd: ");
|
||
Serial.println(_PosRefStbd);
|
||
Serial.print("PosRefBow: ");
|
||
Serial.println(_PosRefBow);
|
||
Serial.print("ETAdate: ");
|
||
Serial.println(_ETAdate);
|
||
Serial.print("ETAtime: ");
|
||
Serial.println(_ETAtime);
|
||
Serial.print("Draught: ");
|
||
Serial.println(_Draught);
|
||
Serial.print("Destination: ");
|
||
Serial.println(_Destination);
|
||
Serial.print("GNSStype: ");
|
||
Serial.println(_GNSStype);
|
||
Serial.print("DTE: ");
|
||
Serial.println(_DTE);
|
||
Serial.println("––––––––––––––––––––––– Msg 5 –––––––––––––––––––––––––––––––––");
|
||
#endif
|
||
|
||
if (SetAISClassAMessage5(NMEA0183AISMsg, _MessageID, _Repeat, _UserID, _IMONumber, _Callsign, _Name, _VesselType,
|
||
_Length, _Beam, _PosRefStbd, _PosRefBow, _ETAdate, _ETAtime, _Draught, _Destination,
|
||
_GNSStype, _DTE))
|
||
{
|
||
|
||
SendMessage(NMEA0183AISMsg.BuildMsg5Part1(NMEA0183AISMsg));
|
||
|
||
#ifdef SERIAL_PRINT_AIS_NMEA
|
||
// Debug Print AIS-NMEA Message Type 5, Part 1
|
||
char buf[7];
|
||
Serial.print(NMEA0183AISMsg.GetPrefix());
|
||
Serial.print(NMEA0183AISMsg.Sender());
|
||
Serial.print(NMEA0183AISMsg.MessageCode());
|
||
for (int i = 0; i < NMEA0183AISMsg.FieldCount(); i++)
|
||
{
|
||
Serial.print(",");
|
||
Serial.print(NMEA0183AISMsg.Field(i));
|
||
}
|
||
sprintf(buf, "*%02X\r\n", NMEA0183AISMsg.GetCheckSum());
|
||
Serial.print(buf);
|
||
#endif
|
||
|
||
SendMessage(NMEA0183AISMsg.BuildMsg5Part2(NMEA0183AISMsg));
|
||
|
||
#ifdef SERIAL_PRINT_AIS_NMEA
|
||
// Print AIS-NMEA Message Type 5, Part 2
|
||
Serial.print(NMEA0183AISMsg.GetPrefix());
|
||
Serial.print(NMEA0183AISMsg.Sender());
|
||
Serial.print(NMEA0183AISMsg.MessageCode());
|
||
for (int i = 0; i < NMEA0183AISMsg.FieldCount(); i++)
|
||
{
|
||
Serial.print(",");
|
||
Serial.print(NMEA0183AISMsg.Field(i));
|
||
}
|
||
sprintf(buf, "*%02X\r\n", NMEA0183AISMsg.GetCheckSum());
|
||
Serial.print(buf);
|
||
#endif
|
||
}
|
||
}
|
||
}
|
||
|
||
//
|
||
//*****************************************************************************
|
||
// 129039 AIS Class B Position Report (Message 18)
|
||
void HandleAISClassBMessage18(const tN2kMsg &N2kMsg)
|
||
{
|
||
uint8_t _MessageID;
|
||
tN2kAISRepeat _Repeat;
|
||
uint32_t _UserID; // MMSI
|
||
double _Latitude=N2kDoubleNA;
|
||
double _Longitude=N2kDoubleNA;
|
||
bool _Accuracy;
|
||
bool _RAIM;
|
||
uint8_t _Seconds;
|
||
double _COG=N2kDoubleNA;
|
||
double _SOG=N2kDoubleNA;
|
||
double _Heading=N2kDoubleNA;
|
||
tN2kAISUnit _Unit;
|
||
bool _Display, _DSC, _Band, _Msg22, _State;
|
||
tN2kAISMode _Mode;
|
||
tN2kAISTransceiverInformation _AISTranceiverInformation;
|
||
uint8_t _SID;
|
||
|
||
if (ParseN2kPGN129039(N2kMsg, _MessageID, _Repeat, _UserID, _Latitude, _Longitude, _Accuracy, _RAIM,
|
||
_Seconds, _COG, _SOG, _AISTranceiverInformation, _Heading, _Unit, _Display, _DSC, _Band, _Msg22, _Mode, _State,_SID))
|
||
{
|
||
|
||
tNMEA0183AISMsg NMEA0183AISMsg;
|
||
|
||
if (SetAISClassBMessage18(NMEA0183AISMsg, _MessageID, _Repeat, _UserID, _Latitude, _Longitude, _Accuracy, _RAIM,
|
||
_Seconds, _COG, _SOG, _Heading, _Unit, _Display, _DSC, _Band, _Msg22, _Mode, _State))
|
||
{
|
||
|
||
SendMessage(NMEA0183AISMsg);
|
||
|
||
#ifdef SERIAL_PRINT_AIS_NMEA
|
||
// Debug Print AIS-NMEA
|
||
Serial.print(NMEA0183AISMsg.GetPrefix());
|
||
Serial.print(NMEA0183AISMsg.Sender());
|
||
Serial.print(NMEA0183AISMsg.MessageCode());
|
||
for (int i = 0; i < NMEA0183AISMsg.FieldCount(); i++)
|
||
{
|
||
Serial.print(",");
|
||
Serial.print(NMEA0183AISMsg.Field(i));
|
||
}
|
||
char buf[7];
|
||
sprintf(buf, "*%02X\r\n", NMEA0183AISMsg.GetCheckSum());
|
||
Serial.print(buf);
|
||
#endif
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
//*****************************************************************************
|
||
// PGN 129809 AIS Class B "CS" Static Data Report, Part A
|
||
void HandleAISClassBMessage24A(const tN2kMsg &N2kMsg)
|
||
{
|
||
|
||
uint8_t _MessageID;
|
||
tN2kAISRepeat _Repeat;
|
||
uint32_t _UserID; // MMSI
|
||
char _Name[21];
|
||
tN2kAISTransceiverInformation _AISInfo;
|
||
uint8_t _SID;
|
||
|
||
if (ParseN2kPGN129809(N2kMsg, _MessageID, _Repeat, _UserID, _Name,21,_AISInfo,_SID))
|
||
{
|
||
|
||
tNMEA0183AISMsg NMEA0183AISMsg;
|
||
if (SetAISClassBMessage24PartA(NMEA0183AISMsg, _MessageID, _Repeat, _UserID, _Name))
|
||
{
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
//*****************************************************************************
|
||
// PGN 129810 AIS Class B "CS" Static Data Report, Part B -> AIS Message 24 (2 Parts)
|
||
void HandleAISClassBMessage24B(const tN2kMsg &N2kMsg)
|
||
{
|
||
|
||
uint8_t _MessageID;
|
||
tN2kAISRepeat _Repeat;
|
||
uint32_t _UserID, _MothershipID; // MMSI
|
||
char _Callsign[8];
|
||
char _Vendor[4];
|
||
uint8_t _VesselType;
|
||
double _Length=N2kDoubleNA;
|
||
double _Beam=N2kDoubleNA;
|
||
double _PosRefStbd=N2kDoubleNA;
|
||
double _PosRefBow=N2kDoubleNA;
|
||
tN2kAISTransceiverInformation _AISInfo;
|
||
uint8_t _SID;
|
||
|
||
if (ParseN2kPGN129810(N2kMsg, _MessageID, _Repeat, _UserID, _VesselType, _Vendor,4, _Callsign,8,
|
||
_Length, _Beam, _PosRefStbd, _PosRefBow, _MothershipID,_AISInfo,_SID))
|
||
{
|
||
|
||
//
|
||
#ifdef SERIAL_PRINT_AIS_FIELDS
|
||
// Debug Print N2k Values
|
||
Serial.println("––––––––––––––––––––––– Msg 24 ––––––––––––––––––––––––––––––––");
|
||
Serial.print("MessageID: ");
|
||
Serial.println(_MessageID);
|
||
Serial.print("Repeat: ");
|
||
Serial.println(_Repeat);
|
||
Serial.print("UserID: ");
|
||
Serial.println(_UserID);
|
||
Serial.print("VesselType: ");
|
||
Serial.println(_VesselType);
|
||
Serial.print("Vendor: ");
|
||
Serial.println(_Vendor);
|
||
Serial.print("Callsign: ");
|
||
Serial.println(_Callsign);
|
||
Serial.print("Length: ");
|
||
Serial.println(_Length);
|
||
Serial.print("Beam: ");
|
||
Serial.println(_Beam);
|
||
Serial.print("PosRefStbd: ");
|
||
Serial.println(_PosRefStbd);
|
||
Serial.print("PosRefBow: ");
|
||
Serial.println(_PosRefBow);
|
||
Serial.print("MothershipID: ");
|
||
Serial.println(_MothershipID);
|
||
Serial.println("––––––––––––––––––––––– Msg 24 ––––––––––––––––––––––––––––––––");
|
||
#endif
|
||
|
||
tNMEA0183AISMsg NMEA0183AISMsg;
|
||
|
||
if (SetAISClassBMessage24(NMEA0183AISMsg, _MessageID, _Repeat, _UserID, _VesselType, _Vendor, _Callsign,
|
||
_Length, _Beam, _PosRefStbd, _PosRefBow, _MothershipID))
|
||
{
|
||
|
||
SendMessage(NMEA0183AISMsg.BuildMsg24PartA(NMEA0183AISMsg));
|
||
|
||
#ifdef SERIAL_PRINT_AIS_NMEA
|
||
// Debug Print AIS-NMEA
|
||
char buf[7];
|
||
Serial.print(NMEA0183AISMsg.GetPrefix());
|
||
Serial.print(NMEA0183AISMsg.Sender());
|
||
Serial.print(NMEA0183AISMsg.MessageCode());
|
||
for (int i = 0; i < NMEA0183AISMsg.FieldCount(); i++)
|
||
{
|
||
Serial.print(",");
|
||
Serial.print(NMEA0183AISMsg.Field(i));
|
||
}
|
||
sprintf(buf, "*%02X\r\n", NMEA0183AISMsg.GetCheckSum());
|
||
Serial.print(buf);
|
||
#endif
|
||
|
||
SendMessage(NMEA0183AISMsg.BuildMsg24PartB(NMEA0183AISMsg));
|
||
|
||
#ifdef SERIAL_PRINT_AIS_NMEA
|
||
Serial.print(NMEA0183AISMsg.GetPrefix());
|
||
Serial.print(NMEA0183AISMsg.Sender());
|
||
Serial.print(NMEA0183AISMsg.MessageCode());
|
||
for (int i = 0; i < NMEA0183AISMsg.FieldCount(); i++)
|
||
{
|
||
Serial.print(",");
|
||
Serial.print(NMEA0183AISMsg.Field(i));
|
||
}
|
||
sprintf(buf, "*%02X\r\n", NMEA0183AISMsg.GetCheckSum());
|
||
Serial.print(buf);
|
||
#endif
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
void HandleSystemTime(const tN2kMsg &msg){
|
||
unsigned char sid=-1;
|
||
uint16_t DaysSince1970=N2kUInt16NA;
|
||
double GpsTime=N2kDoubleNA;
|
||
tN2kTimeSource TimeSource;
|
||
|
||
if (! ParseN2kSystemTime(msg,sid,DaysSince1970,GpsTime,TimeSource)){
|
||
LOG_DEBUG(GwLog::DEBUG,"unable to parse PGN %d",msg.PGN);
|
||
return;
|
||
}
|
||
updateDouble(boatData->GPST,GpsTime);
|
||
if (DaysSince1970 != N2kUInt16NA) boatData->GPSD->update(DaysSince1970,sourceId);
|
||
if (boatData->GPSD->isValid() && boatData->GPST->isValid()){
|
||
tNMEA0183Msg nmeaMsg;
|
||
nmeaMsg.Init("ZDA",talkerId);
|
||
char utc[7];
|
||
double seconds=boatData->GPST->getData();
|
||
int hours=floor(seconds/3600.0);
|
||
int minutes=floor(seconds/60) - hours *60;
|
||
int sec=floor(seconds)-60*minutes-3600*hours;
|
||
snprintf(utc,7,"%02d%02d%02d",hours,minutes,sec);
|
||
nmeaMsg.AddStrField(utc);
|
||
tmElements_t timeParts;
|
||
tNMEA0183Msg::breakTime(tNMEA0183Msg::daysToTime_t(boatData->GPSD->getData()),timeParts);
|
||
nmeaMsg.AddUInt32Field(tNMEA0183Msg::GetDay(timeParts));
|
||
nmeaMsg.AddUInt32Field(tNMEA0183Msg::GetMonth(timeParts));
|
||
nmeaMsg.AddUInt32Field(tNMEA0183Msg::GetYear(timeParts));
|
||
if (boatData->TZ->isValid()){
|
||
int hours=boatData->TZ->getData()/60;
|
||
int minutes=boatData->TZ->getData() - 60 *hours;
|
||
nmeaMsg.AddDoubleField(hours,1,"%02.0f");
|
||
nmeaMsg.AddDoubleField(minutes,1,"%02.0f");
|
||
}
|
||
else{
|
||
nmeaMsg.AddEmptyField();
|
||
nmeaMsg.AddEmptyField();
|
||
}
|
||
SendMessage(nmeaMsg);
|
||
}
|
||
}
|
||
|
||
void HandleTimeOffset(const tN2kMsg &msg){
|
||
uint16_t DaysSince1970 =N2kUInt16NA;
|
||
double GpsTime=N2kDoubleNA;
|
||
int16_t LocalOffset=N2kInt16NA;
|
||
if (!ParseN2kLocalOffset(msg,DaysSince1970,GpsTime,LocalOffset)){
|
||
LOG_DEBUG(GwLog::DEBUG,"unable to parse PGN %d",msg.PGN);
|
||
return;
|
||
}
|
||
updateDouble(boatData->GPST,GpsTime);
|
||
if (DaysSince1970 != N2kUInt16NA) boatData->GPSD->update(DaysSince1970,sourceId);
|
||
if (LocalOffset != N2kInt16NA) boatData->TZ->update(LocalOffset,sourceId);
|
||
}
|
||
void HandleROT(const tN2kMsg &msg){
|
||
unsigned char SID=0;
|
||
double ROT=N2kDoubleNA;
|
||
if (! ParseN2kRateOfTurn(msg,SID,ROT)){
|
||
LOG_DEBUG(GwLog::DEBUG,"unable to parse PGN %d",msg.PGN);
|
||
return;
|
||
}
|
||
if (!updateDouble(boatData->ROT,ROT)) return;
|
||
tNMEA0183Msg nmeamsg;
|
||
if (NMEA0183SetROT(nmeamsg,ROT * ROT_WA_FACTOR,talkerId)){
|
||
SendMessage(nmeamsg);
|
||
}
|
||
}
|
||
void HandleXTE(const tN2kMsg &msg){
|
||
unsigned char SID=0;
|
||
tN2kXTEMode XTEMode;
|
||
bool NavigationTerminated=false;
|
||
double XTE=N2kDoubleNA;
|
||
if (! ParseN2kXTE(msg,SID,XTEMode,NavigationTerminated,XTE)){
|
||
LOG_DEBUG(GwLog::DEBUG,"unable to parse PGN %d",msg.PGN);
|
||
return;
|
||
}
|
||
if (NavigationTerminated) return;
|
||
if (! updateDouble(boatData->XTE,XTE)) return;
|
||
tNMEA0183Msg nmeamsg;
|
||
if (!nmeamsg.Init("XTE",talkerId)) return;
|
||
const char *mode="A";
|
||
switch(XTEMode){
|
||
case N2kxtem_Differential:
|
||
mode="D";
|
||
break;
|
||
case N2kxtem_Estimated:
|
||
mode="E";
|
||
break;
|
||
case N2kxtem_Simulator:
|
||
mode="S";
|
||
break;
|
||
case N2kxtem_Manual:
|
||
mode="M";
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
if (!nmeamsg.AddStrField("A")) return;
|
||
if (!nmeamsg.AddStrField("A")) return;
|
||
double val=mtr2nm(XTE);
|
||
const char *dir="L";
|
||
if (val < 0) {
|
||
dir="R";
|
||
val=-val;
|
||
}
|
||
if (! nmeamsg.AddDoubleField(val,1.0,"%.4f")) return;
|
||
if (! nmeamsg.AddStrField(dir)) return;
|
||
if (! nmeamsg.AddStrField("N")) return;
|
||
if (! nmeamsg.AddStrField(mode)) return;
|
||
SendMessage(nmeamsg);
|
||
}
|
||
|
||
void HandleNavigation(const tN2kMsg &msg){
|
||
unsigned char SID=0;
|
||
double DistanceToWaypoint=N2kDoubleNA;
|
||
tN2kHeadingReference BearingReference;
|
||
bool PerpendicularCrossed=false;
|
||
bool ArrivalCircleEntered=false;
|
||
tN2kDistanceCalculationType CalculationType;
|
||
double ETATime=N2kDoubleNA;
|
||
int16_t ETADate=0;
|
||
double BearingOriginToDestinationWaypoint=N2kDoubleNA;
|
||
double BearingPositionToDestinationWaypoint=N2kDoubleNA;
|
||
uint32_t OriginWaypointNumber;
|
||
uint32_t DestinationWaypointNumber;
|
||
double DestinationLatitude=N2kDoubleNA;
|
||
double DestinationLongitude=N2kDoubleNA;
|
||
double WaypointClosingVelocity=N2kDoubleNA;
|
||
if (! ParseN2kNavigationInfo(msg,SID,DistanceToWaypoint,BearingReference,
|
||
PerpendicularCrossed, ArrivalCircleEntered,CalculationType,
|
||
ETATime, ETADate, BearingOriginToDestinationWaypoint, BearingPositionToDestinationWaypoint,
|
||
OriginWaypointNumber, DestinationWaypointNumber,
|
||
DestinationLatitude, DestinationLongitude,WaypointClosingVelocity)){
|
||
LOG_DEBUG(GwLog::DEBUG,"unable to parse PGN %d",msg.PGN);
|
||
return;
|
||
}
|
||
if (CalculationType != N2kdct_GreatCircle){
|
||
LOG_DEBUG(GwLog::DEBUG,"can only handle %d with great circle type",msg.PGN);
|
||
return;
|
||
}
|
||
if (BearingReference != N2khr_true && BearingReference != N2khr_magnetic){
|
||
LOG_DEBUG(GwLog::DEBUG,"invalid bearing ref %d for %d ",(int)BearingReference, msg.PGN);
|
||
return;
|
||
}
|
||
if (BearingReference == N2khr_magnetic){
|
||
if (! boatData->VAR->isValid()){
|
||
LOG_DEBUG(GwLog::DEBUG,"missing variation to compute true heading for %d ", msg.PGN);
|
||
return;
|
||
}
|
||
BearingPositionToDestinationWaypoint-=boatData->VAR->getData();
|
||
|
||
}
|
||
if (! updateDouble(boatData->DTW,DistanceToWaypoint)) return;
|
||
if (! updateDouble(boatData->BTW,BearingPositionToDestinationWaypoint)) return;
|
||
if (! updateDouble(boatData->WPLat,DestinationLatitude)) return;
|
||
if (! updateDouble(boatData->WPLon,DestinationLongitude)) return;
|
||
tNMEA0183Msg nmeaMsg;
|
||
if (! nmeaMsg.Init("RMB",talkerId)) return;
|
||
if (! nmeaMsg.AddStrField("A")) return;
|
||
const char *dir="L";
|
||
double xte=boatData->XTE->getDataWithDefault(NMEA0183DoubleNA);
|
||
if (xte != NMEA0183DoubleNA){
|
||
if (xte < 0){
|
||
xte=-xte;
|
||
dir="R";
|
||
}
|
||
xte=mtr2nm(xte);
|
||
if (xte > 9.99) xte=9.99;
|
||
}
|
||
if (! nmeaMsg.AddDoubleField(xte,1.0,"%.2f",dir)) return;
|
||
//TODO: handle waypoint names
|
||
if (! nmeaMsg.AddUInt32Field(OriginWaypointNumber)) return;
|
||
if (! nmeaMsg.AddUInt32Field(DestinationWaypointNumber)) return;
|
||
if (! nmeaMsg.AddLatitudeField(DestinationLatitude)) return;
|
||
if (! nmeaMsg.AddLongitudeField(DestinationLongitude)) return;
|
||
double distance=mtr2nm(DistanceToWaypoint);
|
||
if (distance > 999.9) distance=999.9;
|
||
if (! nmeaMsg.AddDoubleField(distance,1,"%.1f")) return;
|
||
if (! nmeaMsg.AddDoubleField(formatCourse(BearingPositionToDestinationWaypoint))) return;
|
||
if (WaypointClosingVelocity != N2kDoubleNA){
|
||
if (!nmeaMsg.AddDoubleField(msToKnots(WaypointClosingVelocity))) return;
|
||
}
|
||
else{
|
||
if (!nmeaMsg.AddEmptyField()) return;
|
||
}
|
||
if (! nmeaMsg.AddStrField(ArrivalCircleEntered?"A":"V")) return;
|
||
SendMessage(nmeaMsg);
|
||
}
|
||
|
||
void HandleFluidLevel(const tN2kMsg &N2kMsg)
|
||
{
|
||
unsigned char Instance;
|
||
tN2kFluidType FluidType;
|
||
double Level=N2kDoubleNA;
|
||
double Capacity=N2kDoubleNA;
|
||
if (ParseN2kPGN127505(N2kMsg,Instance,FluidType,Level,Capacity)) {
|
||
GwXDRFoundMapping mapping=xdrMappings->getMapping(XDRFLUID,FluidType,0,Instance);
|
||
if (updateDouble(&mapping,Level)){
|
||
LOG_DEBUG(GwLog::DEBUG+1,"found fluidlevel mapping %s",mapping.definition->toString().c_str());
|
||
addToXdr(mapping.buildXdrEntry(Level));
|
||
}
|
||
mapping=xdrMappings->getMapping(XDRFLUID,FluidType,1,Instance);
|
||
if (updateDouble(&mapping,Capacity)){
|
||
LOG_DEBUG(GwLog::DEBUG+1,"found fluid capacity mapping %s",mapping.definition->toString().c_str());
|
||
addToXdr(mapping.buildXdrEntry(Capacity));
|
||
}
|
||
finalizeXdr();
|
||
}
|
||
}
|
||
|
||
void HandleBatteryStatus(const tN2kMsg &N2kMsg)
|
||
{
|
||
unsigned char SID=-1;
|
||
unsigned char BatteryInstance;
|
||
double BatteryVoltage=N2kDoubleNA;
|
||
double BatteryCurrent=N2kDoubleNA;
|
||
double BatteryTemperature=N2kDoubleNA;
|
||
if (ParseN2kPGN127508(N2kMsg,BatteryInstance,BatteryVoltage,BatteryCurrent,BatteryTemperature,SID)) {
|
||
int i=0;
|
||
GwXDRFoundMapping mapping=xdrMappings->getMapping(XDRBAT,0,0,BatteryInstance);
|
||
if (updateDouble(&mapping,BatteryVoltage)){
|
||
LOG_DEBUG(GwLog::DEBUG+1,"found BatteryVoltage mapping %s",mapping.definition->toString().c_str());
|
||
addToXdr(mapping.buildXdrEntry(BatteryVoltage));
|
||
i++;
|
||
}
|
||
mapping=xdrMappings->getMapping(XDRBAT,0,1,BatteryInstance);
|
||
if (updateDouble(&mapping,BatteryCurrent)){
|
||
LOG_DEBUG(GwLog::DEBUG+1,"found BatteryCurrent mapping %s",mapping.definition->toString().c_str());
|
||
addToXdr(mapping.buildXdrEntry(BatteryCurrent));
|
||
i++;
|
||
}
|
||
mapping=xdrMappings->getMapping(XDRBAT,0,2,BatteryInstance);
|
||
if (updateDouble(&mapping,BatteryTemperature)){
|
||
LOG_DEBUG(GwLog::DEBUG+1,"found BatteryTemperature mapping %s",mapping.definition->toString().c_str());
|
||
addToXdr(mapping.buildXdrEntry(BatteryTemperature));
|
||
i++;
|
||
}
|
||
if (i>0) finalizeXdr();
|
||
}
|
||
}
|
||
|
||
void Handle130310(const tN2kMsg &N2kMsg)
|
||
{
|
||
|
||
unsigned char SID=-1;
|
||
double OutsideAmbientAirTemperature=N2kDoubleNA;
|
||
double AtmosphericPressure=N2kDoubleNA;
|
||
double WaterTemperature=N2kDoubleNA;
|
||
if (ParseN2kPGN130310(N2kMsg, SID, WaterTemperature, OutsideAmbientAirTemperature, AtmosphericPressure))
|
||
{
|
||
updateDouble(boatData->WTemp, WaterTemperature);
|
||
tNMEA0183Msg NMEA0183Msg;
|
||
|
||
if (!NMEA0183Msg.Init("MTW", talkerId))
|
||
return;
|
||
if (!NMEA0183Msg.AddDoubleField(KelvinToC(WaterTemperature)))
|
||
return;
|
||
if (!NMEA0183Msg.AddStrField("C"))
|
||
return;
|
||
|
||
SendMessage(NMEA0183Msg);
|
||
}
|
||
int i=0;
|
||
GwXDRFoundMapping mapping=xdrMappings->getMapping(XDRTEMP,N2kts_OutsideTemperature,0,0);
|
||
if (updateDouble(&mapping,OutsideAmbientAirTemperature)){
|
||
LOG_DEBUG(GwLog::DEBUG+1,"found temperature mapping %s",mapping.definition->toString().c_str());
|
||
addToXdr(mapping.buildXdrEntry(OutsideAmbientAirTemperature));
|
||
i++;
|
||
}
|
||
mapping=xdrMappings->getMapping(XDRPRESSURE,N2kps_Atmospheric,0,0);
|
||
if (updateDouble(&mapping,AtmosphericPressure)){
|
||
LOG_DEBUG(GwLog::DEBUG+1,"found pressure mapping %s",mapping.definition->toString().c_str());
|
||
addToXdr(mapping.buildXdrEntry(AtmosphericPressure));
|
||
i++;
|
||
}
|
||
if (i>0) finalizeXdr();
|
||
}
|
||
|
||
void Handle130311(const tN2kMsg &msg){
|
||
unsigned char SID=-1;
|
||
tN2kTempSource TempSource;
|
||
double Temperature=N2kDoubleNA;
|
||
tN2kHumiditySource HumiditySource;
|
||
double Humidity=N2kDoubleNA;
|
||
double AtmosphericPressure=N2kDoubleNA;
|
||
if (!ParseN2kPGN130311(msg,SID,TempSource,Temperature,HumiditySource,Humidity,AtmosphericPressure)) {
|
||
LOG_DEBUG(GwLog::DEBUG,"unable to parse PGN %d",msg.PGN);
|
||
return;
|
||
}
|
||
int i=0;
|
||
if (TempSource == N2kts_SeaTemperature) {
|
||
updateDouble(boatData->WTemp, Temperature);
|
||
tNMEA0183Msg NMEA0183Msg;
|
||
|
||
if (!NMEA0183Msg.Init("MTW", talkerId))
|
||
return;
|
||
if (!NMEA0183Msg.AddDoubleField(KelvinToC(Temperature)))
|
||
return;
|
||
if (!NMEA0183Msg.AddStrField("C"))
|
||
return;
|
||
|
||
SendMessage(NMEA0183Msg);
|
||
}
|
||
|
||
GwXDRFoundMapping mapping=xdrMappings->getMapping(XDRTEMP,TempSource,0,0);
|
||
if (updateDouble(&mapping,Temperature)){
|
||
LOG_DEBUG(GwLog::DEBUG+1,"found temperature mapping %s",mapping.definition->toString().c_str());
|
||
addToXdr(mapping.buildXdrEntry(Temperature));
|
||
i++;
|
||
}
|
||
mapping=xdrMappings->getMapping(XDRHUMIDITY,HumiditySource,0,0);
|
||
if (updateDouble(&mapping,Humidity)){
|
||
LOG_DEBUG(GwLog::DEBUG+1,"found humidity mapping %s",mapping.definition->toString().c_str());
|
||
addToXdr(mapping.buildXdrEntry(Humidity));
|
||
i++;
|
||
}
|
||
mapping=xdrMappings->getMapping(XDRPRESSURE,N2kps_Atmospheric,0,0);
|
||
if (updateDouble(&mapping,AtmosphericPressure)){
|
||
LOG_DEBUG(GwLog::DEBUG+1,"found pressure mapping %s",mapping.definition->toString().c_str());
|
||
addToXdr(mapping.buildXdrEntry(AtmosphericPressure));
|
||
i++;
|
||
}
|
||
if (i>0) finalizeXdr();
|
||
|
||
}
|
||
|
||
void Handle130312(const tN2kMsg &msg){
|
||
unsigned char SID=-1;
|
||
unsigned char TemperatureInstance=0;
|
||
tN2kTempSource TemperatureSource;
|
||
double Temperature=N2kDoubleNA;
|
||
double setTemperature=N2kDoubleNA;
|
||
if (!ParseN2kPGN130312(msg,SID,TemperatureInstance,TemperatureSource,Temperature,setTemperature)){
|
||
LOG_DEBUG(GwLog::DEBUG,"unable to parse PGN %d",msg.PGN);
|
||
return;
|
||
}
|
||
if (TemperatureSource == N2kts_SeaTemperature &&
|
||
(config.winst312 == TemperatureInstance || config.winst312 == 256)) {
|
||
updateDouble(boatData->WTemp, Temperature);
|
||
tNMEA0183Msg NMEA0183Msg;
|
||
|
||
if (!NMEA0183Msg.Init("MTW", talkerId))
|
||
return;
|
||
if (!NMEA0183Msg.AddDoubleField(KelvinToC(Temperature)))
|
||
return;
|
||
if (!NMEA0183Msg.AddStrField("C"))
|
||
return;
|
||
|
||
SendMessage(NMEA0183Msg);
|
||
}
|
||
|
||
GwXDRFoundMapping mapping=xdrMappings->getMapping(XDRTEMP,(int)TemperatureSource,0,TemperatureInstance);
|
||
if (updateDouble(&mapping,Temperature)){
|
||
LOG_DEBUG(GwLog::DEBUG+1,"found temperature mapping %s",mapping.definition->toString().c_str());
|
||
addToXdr(mapping.buildXdrEntry(Temperature));
|
||
}
|
||
mapping=xdrMappings->getMapping(XDRTEMP,(int)TemperatureSource,1,TemperatureInstance);
|
||
if (updateDouble(&mapping,setTemperature)){
|
||
LOG_DEBUG(GwLog::DEBUG+1,"found temperature mapping %s",mapping.definition->toString().c_str());
|
||
addToXdr(mapping.buildXdrEntry(setTemperature));
|
||
}
|
||
finalizeXdr();
|
||
}
|
||
|
||
void Handle130313(const tN2kMsg &msg){
|
||
unsigned char SID=-1;
|
||
unsigned char HumidityInstance=0;
|
||
tN2kHumiditySource HumiditySource;
|
||
double ActualHumidity=N2kDoubleNA;
|
||
double SetHumidity=N2kDoubleNA;
|
||
if (!ParseN2kPGN130313(msg,SID,HumidityInstance,HumiditySource,ActualHumidity,SetHumidity)){
|
||
LOG_DEBUG(GwLog::DEBUG,"unable to parse PGN %d",msg.PGN);
|
||
return;
|
||
}
|
||
GwXDRFoundMapping mapping=xdrMappings->getMapping(XDRHUMIDITY,(int)HumiditySource,0,HumidityInstance);
|
||
if (updateDouble(&mapping,ActualHumidity)){
|
||
LOG_DEBUG(GwLog::DEBUG+1,"found humidity mapping %s",mapping.definition->toString().c_str());
|
||
addToXdr(mapping.buildXdrEntry(ActualHumidity));
|
||
}
|
||
mapping=xdrMappings->getMapping(XDRHUMIDITY,(int)HumiditySource,1,HumidityInstance);
|
||
if (updateDouble(&mapping,SetHumidity)){
|
||
LOG_DEBUG(GwLog::DEBUG+1,"found humidity mapping %s",mapping.definition->toString().c_str());
|
||
addToXdr(mapping.buildXdrEntry(SetHumidity));
|
||
}
|
||
finalizeXdr();
|
||
}
|
||
|
||
void Handle130314(const tN2kMsg &msg){
|
||
unsigned char SID=-1;
|
||
unsigned char PressureInstance=0;
|
||
tN2kPressureSource PressureSource;
|
||
double ActualPressure=N2kDoubleNA;
|
||
if (! ParseN2kPGN130314(msg,SID, PressureInstance,
|
||
PressureSource, ActualPressure)){
|
||
LOG_DEBUG(GwLog::DEBUG,"unable to parse PGN %d",msg.PGN);
|
||
return;
|
||
}
|
||
GwXDRFoundMapping mapping=xdrMappings->getMapping(XDRPRESSURE,(int)PressureSource,0,PressureInstance);
|
||
if (! updateDouble(&mapping,ActualPressure)) return;
|
||
LOG_DEBUG(GwLog::DEBUG+1,"found pressure mapping %s",mapping.definition->toString().c_str());
|
||
addToXdr(mapping.buildXdrEntry(ActualPressure));
|
||
finalizeXdr();
|
||
}
|
||
void Handle127489(const tN2kMsg &msg){
|
||
unsigned char instance=-1;
|
||
double values[8];
|
||
for (int i=0;i<8;i++) values[i]=N2kDoubleNA;
|
||
int8_t ivalues[2];
|
||
if (! ParseN2kPGN127489(msg,instance,
|
||
values[0],values[1],values[2],values[3],values[4],values[5],
|
||
values[6],values[7],ivalues[0],ivalues[1]
|
||
)){
|
||
LOG_DEBUG(GwLog::DEBUG,"unable to parse PGN %d",msg.PGN);
|
||
}
|
||
for (int i=0;i<8;i++){
|
||
GwXDRFoundMapping mapping=xdrMappings->getMapping(XDRENGINE,0,i,instance);
|
||
if (! updateDouble(&mapping,values[i])) continue;
|
||
addToXdr(mapping.buildXdrEntry(values[i]));
|
||
}
|
||
for (int i=0;i< 2;i++){
|
||
GwXDRFoundMapping mapping=xdrMappings->getMapping(XDRENGINE,0,i+8,instance);
|
||
if (! updateDouble(&mapping,ivalues[i])) continue;
|
||
addToXdr(mapping.buildXdrEntry((double)ivalues[i]));
|
||
}
|
||
finalizeXdr();
|
||
}
|
||
void Handle127257(const tN2kMsg &msg){
|
||
unsigned char instance=-1;
|
||
double values[3];
|
||
for (int i=0;i<3;i++) values[i]=N2kDoubleNA;
|
||
//yaw,pitch,roll
|
||
if (! ParseN2kPGN127257(msg,instance,
|
||
values[0],values[1],values[2])){
|
||
LOG_DEBUG(GwLog::DEBUG,"unable to parse PGN %d",msg.PGN);
|
||
}
|
||
for (int i=0;i<3;i++){
|
||
GwXDRFoundMapping mapping=xdrMappings->getMapping(XDRATTITUDE,0,i,instance);
|
||
if (! updateDouble(&mapping,values[i])) continue;
|
||
addToXdr(mapping.buildXdrEntry(values[i]));
|
||
}
|
||
finalizeXdr();
|
||
}
|
||
void Handle127488(const tN2kMsg &msg){
|
||
unsigned char instance=-1;
|
||
double speed=N2kDoubleNA,pressure=N2kDoubleNA;
|
||
int8_t tilt;
|
||
if (! ParseN2kPGN127488(msg,instance,
|
||
speed,pressure,tilt)){
|
||
LOG_DEBUG(GwLog::DEBUG,"unable to parse PGN %d",msg.PGN);
|
||
}
|
||
GwXDRFoundMapping mapping=xdrMappings->getMapping(XDRENGINE,0,10,instance);
|
||
if (updateDouble(&mapping,speed)){
|
||
addToXdr(mapping.buildXdrEntry(speed));
|
||
}
|
||
mapping=xdrMappings->getMapping(XDRENGINE,0,11,instance);
|
||
if (updateDouble(&mapping,pressure)){
|
||
addToXdr(mapping.buildXdrEntry(pressure));
|
||
}
|
||
mapping=xdrMappings->getMapping(XDRENGINE,0,12,instance);
|
||
if (updateDouble(&mapping,tilt)){
|
||
addToXdr(mapping.buildXdrEntry((double)tilt));
|
||
}
|
||
finalizeXdr();
|
||
if (speed == N2kDoubleNA) return;
|
||
tNMEA0183Msg nmeaMsg;
|
||
if (! nmeaMsg.Init("RPM",talkerId)) return;
|
||
if (! nmeaMsg.AddStrField("E")) return;
|
||
if (! nmeaMsg.AddDoubleField(instance)) return;
|
||
if (! nmeaMsg.AddDoubleField(speed)) return;
|
||
if (! nmeaMsg.AddEmptyField()) return;
|
||
if (! nmeaMsg.AddStrField("V")) return;
|
||
SendMessage(nmeaMsg);
|
||
}
|
||
|
||
void Handle130316(const tN2kMsg &msg){
|
||
unsigned char SID=-1;
|
||
unsigned char TemperatureInstance=0;
|
||
tN2kTempSource TemperatureSource;
|
||
double Temperature=N2kDoubleNA;
|
||
double setTemperature=N2kDoubleNA;
|
||
if (!ParseN2kPGN130316(msg,SID,TemperatureInstance,TemperatureSource,Temperature,setTemperature)){
|
||
LOG_DEBUG(GwLog::DEBUG,"unable to parse PGN %d",msg.PGN);
|
||
return;
|
||
}
|
||
GwXDRFoundMapping mapping=xdrMappings->getMapping(XDRTEMP,(int)TemperatureSource,0,TemperatureInstance);
|
||
if (updateDouble(&mapping,Temperature)){
|
||
LOG_DEBUG(GwLog::DEBUG+1,"found temperature mapping %s",mapping.definition->toString().c_str());
|
||
addToXdr(mapping.buildXdrEntry(Temperature));
|
||
}
|
||
mapping=xdrMappings->getMapping(XDRTEMP,(int)TemperatureSource,1,TemperatureInstance);
|
||
if (updateDouble(&mapping,setTemperature)){
|
||
LOG_DEBUG(GwLog::DEBUG+1,"found temperature mapping %s",mapping.definition->toString().c_str());
|
||
addToXdr(mapping.buildXdrEntry(setTemperature));
|
||
}
|
||
finalizeXdr();
|
||
}
|
||
|
||
void registerConverters()
|
||
{
|
||
//register all converter functions
|
||
//for each vonverter you should have a member with the N2KMsg as parameter
|
||
//and register it here
|
||
//with this approach we easily have a list of all handled
|
||
//pgns
|
||
converters.registerConverter(127250UL, &N2kToNMEA0183Functions::HandleHeading);
|
||
converters.registerConverter(127258UL, &N2kToNMEA0183Functions::HandleVariation);
|
||
converters.registerConverter(128259UL, &N2kToNMEA0183Functions::HandleBoatSpeed);
|
||
converters.registerConverter(128267UL, &N2kToNMEA0183Functions::HandleDepth);
|
||
converters.registerConverter(129025UL, &N2kToNMEA0183Functions::HandlePosition);
|
||
converters.registerConverter(129026UL, &N2kToNMEA0183Functions::HandleCOGSOG);
|
||
converters.registerConverter(129029UL, &N2kToNMEA0183Functions::HandleGNSS);
|
||
converters.registerConverter(130306UL, &N2kToNMEA0183Functions::HandleWind);
|
||
converters.registerConverter(128275UL, &N2kToNMEA0183Functions::HandleLog);
|
||
converters.registerConverter(127245UL, &N2kToNMEA0183Functions::HandleRudder);
|
||
converters.registerConverter(126992UL, &N2kToNMEA0183Functions::HandleSystemTime);
|
||
converters.registerConverter(129033UL, &N2kToNMEA0183Functions::HandleTimeOffset);
|
||
converters.registerConverter(129539UL, &N2kToNMEA0183Functions::HandleDop);
|
||
converters.registerConverter(129540UL, &N2kToNMEA0183Functions::HandleSats);
|
||
converters.registerConverter(127251UL, &N2kToNMEA0183Functions::HandleROT);
|
||
converters.registerConverter(127505UL, &N2kToNMEA0183Functions::HandleFluidLevel);
|
||
converters.registerConverter(127508UL, &N2kToNMEA0183Functions::HandleBatteryStatus);
|
||
converters.registerConverter(129283UL, &N2kToNMEA0183Functions::HandleXTE);
|
||
converters.registerConverter(129284UL, &N2kToNMEA0183Functions::HandleNavigation);
|
||
converters.registerConverter(130310UL, &N2kToNMEA0183Functions::Handle130310);
|
||
converters.registerConverter(130311UL, &N2kToNMEA0183Functions::Handle130311);
|
||
converters.registerConverter(130312UL, &N2kToNMEA0183Functions::Handle130312);
|
||
converters.registerConverter(130313UL, &N2kToNMEA0183Functions::Handle130313);
|
||
converters.registerConverter(130314UL, &N2kToNMEA0183Functions::Handle130314);
|
||
converters.registerConverter(127489UL, &N2kToNMEA0183Functions::Handle127489);
|
||
converters.registerConverter(127488UL, &N2kToNMEA0183Functions::Handle127488);
|
||
converters.registerConverter(130316UL, &N2kToNMEA0183Functions::Handle130316);
|
||
converters.registerConverter(127257UL, &N2kToNMEA0183Functions::Handle127257);
|
||
#define HANDLE_AIS
|
||
#ifdef HANDLE_AIS
|
||
converters.registerConverter(129038UL, &N2kToNMEA0183Functions::HandleAISClassAPosReport); // AIS Class A Position Report, Message Type 1
|
||
converters.registerConverter(129039UL, &N2kToNMEA0183Functions::HandleAISClassBMessage18); // AIS Class B Position Report, Message Type 18
|
||
converters.registerConverter(129794UL, &N2kToNMEA0183Functions::HandleAISClassAMessage5); // AIS Class A Ship Static and Voyage related data, Message Type 5
|
||
converters.registerConverter(129809UL, &N2kToNMEA0183Functions::HandleAISClassBMessage24A); // AIS Class B "CS" Static Data Report, Part A
|
||
converters.registerConverter(129810UL, &N2kToNMEA0183Functions::HandleAISClassBMessage24B); // AIS Class B "CS" Static Data Report, Part B
|
||
#endif
|
||
}
|
||
|
||
public:
|
||
N2kToNMEA0183Functions(GwLog *logger, GwBoatData *boatData,
|
||
SendNMEA0183MessageCallback callback,
|
||
String talkerId, GwXDRMappings *xdrMappings, const GwConverterConfig &cfg)
|
||
: N2kDataToNMEA0183(logger, boatData, callback,talkerId)
|
||
{
|
||
this->logger = logger;
|
||
this->boatData = boatData;
|
||
this->xdrMappings=xdrMappings;
|
||
this->config=cfg;
|
||
registerConverters();
|
||
}
|
||
virtual void loop(unsigned long lastExtRmc) override
|
||
{
|
||
N2kDataToNMEA0183::loop(lastExtRmc);
|
||
unsigned long now = millis();
|
||
if (config.rmcInterval > 0 && (lastExtRmc + config.rmcCheckTime) <= now && (lastRmcSent + config.rmcInterval) <= now){
|
||
SendRMC();
|
||
}
|
||
}
|
||
};
|
||
|
||
|
||
N2kDataToNMEA0183* N2kDataToNMEA0183::create(GwLog *logger, GwBoatData *boatData,
|
||
SendNMEA0183MessageCallback callback, String talkerId, GwXDRMappings *xdrMappings,
|
||
const GwConverterConfig &cfg){
|
||
LOG_DEBUG(GwLog::LOG,"creating N2kToNMEA0183");
|
||
return new N2kToNMEA0183Functions(logger,boatData,callback, talkerId,xdrMappings,cfg);
|
||
}
|
||
//*****************************************************************************
|