diff --git a/lib/aisparser/ais_decoder.cpp b/lib/aisparser/ais_decoder.cpp index 558c12a..6fece95 100644 --- a/lib/aisparser/ais_decoder.cpp +++ b/lib/aisparser/ais_decoder.cpp @@ -842,11 +842,18 @@ bool AisDecoder::checkTalkerId(const StringRef &_strTalkerId) Decode next sentence (starts reading from input buffer with the specified offset; returns the number of bytes processed, or 0 when no more messages can be decoded). Has to be called until it returns 0, to ensure that any buffered multi-line strings are backed up properly. */ -size_t AisDecoder::decodeMsg(const char *_pNmeaBuffer, size_t _uBufferSize, size_t _uOffset, const SentenceParser &_parser) +size_t AisDecoder::decodeMsg(const char *_pNmeaBuffer, size_t _uBufferSize, size_t _uOffset, + const SentenceParser &_parser, bool treatAsComplete) { // process and decode AIS strings StringRef strLine; - size_t n = getLine(strLine, _pNmeaBuffer, _uBufferSize, _uOffset); + size_t n = 0; + if (treatAsComplete){ + strLine=StringRef(_pNmeaBuffer+_uOffset,_uBufferSize); + } + else{ + n=getLine(strLine, _pNmeaBuffer, _uBufferSize, _uOffset); + } if (strLine.size() > 2) // ignore empty lines { // clear user data diff --git a/lib/aisparser/ais_decoder.h b/lib/aisparser/ais_decoder.h index 9537a1e..c908839 100644 --- a/lib/aisparser/ais_decoder.h +++ b/lib/aisparser/ais_decoder.h @@ -234,7 +234,8 @@ namespace AIS Decode next sentence (starts reading from input buffer with the specified offset; returns the number of bytes processed, or 0 when no more messages can be decoded). Has to be called until it returns 0, to ensure that any buffered multi-line strings are backed up properly. */ - size_t decodeMsg(const char *_pNmeaBuffer, size_t _uBufferSize, size_t _uOffset, const SentenceParser &_parser); + size_t decodeMsg(const char *_pNmeaBuffer, size_t _uBufferSize, size_t _uOffset, + const SentenceParser &_parser, bool treatAsComplete=false); /// returns the total number of messages processed uint64_t getTotalMessageCount() const {return m_uTotalMessages;} diff --git a/lib/nmea0183ton2k/NMEA0183AIStoNMEA2000.h b/lib/nmea0183ton2k/NMEA0183AIStoNMEA2000.h index 0593bee..4c4b440 100644 --- a/lib/nmea0183ton2k/NMEA0183AIStoNMEA2000.h +++ b/lib/nmea0183ton2k/NMEA0183AIStoNMEA2000.h @@ -18,6 +18,7 @@ #include "default_sentence_parser.h" #include "NMEA0183DataToN2K.h" #include "NMEA0183.h" +#include "GwLog.h" const double pi = 3.1415926535897932384626433832795; const double knToms = 1852.0 / 3600.0; @@ -30,17 +31,46 @@ class MyAisDecoder : public AIS::AisDecoder { private: NMEA0183DataToN2K::N2kSender sender; + GwLog *logger; void send(const tN2kMsg &msg){ (*sender)(msg); } AIS::DefaultSentenceParser parser; public: - MyAisDecoder(NMEA0183DataToN2K::N2kSender sender) + MyAisDecoder(GwLog *logger,NMEA0183DataToN2K::N2kSender sender) { + this->logger=logger; this->sender=sender; } protected: + double decodeRot(int iRot){ + //see https://gpsd.gitlab.io/gpsd/AIVDM.html#_type_5_static_and_voyage_related_data + //and https://opencpn.org/wiki/dokuwiki/doku.php?id=opencpn:supplementary_software:nmea2000 + double rot=N2kDoubleNA; + if (iRot == 127) rot=10; + else if (iRot == -127) rot=-10; + else if (iRot == 0) rot=0; + else if ( 1<= iRot && iRot <= 126 ) rot= iRot *iRot / 22.401289; + else if ( iRot >= -126 && iRot <= -1) rot= iRot * iRot / -22.401289 ; + //rot now in deg/minute + rot=rot* degToRad / 60.0; //N"K expects rot in radian/s + return rot; + } + double decodeCog(int iCog){ + double cog = N2kDoubleNA; + if (iCog >= 0 && iCog < 3600){ + cog= iCog/10.0 * degToRad; + } + return cog; + } + double decodeHeading(int iHeading){ + double heading=N2kDoubleNA; + if ( iHeading >=0 && iHeading <=359){ + heading=iHeading *degToRad; + } + return heading; + } virtual void onType123(unsigned int _uMsgType, unsigned int _uMmsi, unsigned int _uNavstatus, int _iRot, unsigned int _uSog, bool _bPosAccuracy, long _iPosLon, long _iPosLat, int _iCog, int _iHeading, int _Repeat, bool _Raim, @@ -54,8 +84,8 @@ class MyAisDecoder : public AIS::AisDecoder SetN2kAISClassAPosition(N2kMsg, _uMsgType, (tN2kAISRepeat)_Repeat, _uMmsi, _iPosLat / 600000.0, _iPosLon / 600000.0, _bPosAccuracy, _Raim, _timestamp, - _iCog * degToRad, _uSog * knToms / 10.0, - _iHeading * degToRad, _iRot, (tN2kAISNavStatus)_uNavstatus); + decodeCog(_iCog) , _uSog * knToms / 10.0, + decodeHeading(_iHeading), decodeRot(_iRot), (tN2kAISNavStatus)_uNavstatus); send(N2kMsg); } @@ -96,9 +126,12 @@ class MyAisDecoder : public AIS::AisDecoder char Name[30]; char Dest[30]; - strncpy(CS, _strCallsign.c_str(), sizeof(CS)); - strncpy(Name, _strName.c_str(), sizeof(Name)); - strncpy(Dest, _strDestination.c_str(), sizeof(Dest)); + strncpy(CS, _strCallsign.c_str(), sizeof(CS)-1); + CS[29]=0; + strncpy(Name, _strName.c_str(), sizeof(Name)-1); + Name[29]=0; + strncpy(Dest, _strDestination.c_str(), sizeof(Dest)-1); + Dest[29]=0; // PGN129794 SetN2kAISClassAStatic(N2kMsg, _uMsgType, (tN2kAISRepeat) _repeat, _uMmsi, @@ -120,7 +153,8 @@ class MyAisDecoder : public AIS::AisDecoder tN2kMsg N2kMsg; char Text[162]; - strncpy(Text, _strText.c_str(), sizeof(Text)); + strncpy(Text, _strText.c_str(), sizeof(Text)-1); + Text[161]=0; N2kMsg.SetPGN(129802UL); N2kMsg.Priority = 4; @@ -151,8 +185,8 @@ class MyAisDecoder : public AIS::AisDecoder // PGN129039 SetN2kAISClassBPosition(N2kMsg, _uMsgType, (tN2kAISRepeat) _repeat, _uMmsi, _iPosLat / 600000.0, _iPosLon / 600000.0, _bPosAccuracy, _raim, - _timestamp, _iCog * degToRad, _uSog * knToms / 10.0, - _iHeading * degToRad, (tN2kAISUnit) _unit, + _timestamp, decodeCog(_iCog), _uSog * knToms / 10.0, + decodeHeading(_iHeading), (tN2kAISUnit) _unit, _diplay, _dsc, _band, _msg22, (tN2kAISMode) _assigned, _state); send(N2kMsg); @@ -170,7 +204,8 @@ class MyAisDecoder : public AIS::AisDecoder // PGN129040 char Name[21] = ""; - strncpy(Name, _strName.c_str(), sizeof(Name)); + strncpy(Name, _strName.c_str(), sizeof(Name)-1); + Name[20]=0; N2kMsg.SetPGN(129040UL); N2kMsg.Priority = 4; @@ -179,12 +214,12 @@ class MyAisDecoder : public AIS::AisDecoder N2kMsg.Add4ByteDouble(_iPosLon / 600000.0, 1e-07); N2kMsg.Add4ByteDouble(_iPosLat / 600000.0, 1e-07); N2kMsg.AddByte((_timestamp & 0x3f) << 2 | (_raim & 0x01) << 1 | (_bPosAccuracy & 0x01)); - N2kMsg.Add2ByteUDouble(_iCog * degToRad, 1e-04); + N2kMsg.Add2ByteUDouble(decodeCog(_iCog), 1e-04); N2kMsg.Add2ByteUDouble(_uSog * knToms / 10.0, 0.01); N2kMsg.AddByte(0xff); // Regional Application N2kMsg.AddByte(0xff); // Regional Application N2kMsg.AddByte(_uType ); - N2kMsg.Add2ByteUDouble(_iHeading * degToRad, 1e-04); + N2kMsg.Add2ByteUDouble(decodeHeading(_iHeading), 1e-04); N2kMsg.AddByte(_fixtype << 4); N2kMsg.Add2ByteDouble(_uToBow + _uToStern, 0.1); N2kMsg.Add2ByteDouble(_uToPort + _uToStarboard, 0.1); @@ -208,7 +243,8 @@ class MyAisDecoder : public AIS::AisDecoder tN2kMsg N2kMsg; char Name[30]; - strncpy(Name, _strName.c_str(), sizeof(Name)); + strncpy(Name, _strName.c_str(), sizeof(Name)-1); + Name[29]=0; // PGN129809 SetN2kAISClassBStaticPartA(N2kMsg, _uMsgType, (tN2kAISRepeat) _repeat, _uMmsi, Name); @@ -228,8 +264,10 @@ class MyAisDecoder : public AIS::AisDecoder char CS[30]; char Vendor[30]; - strncpy(CS, _strCallsign.c_str(), sizeof(CS)); - strncpy(Vendor, _strVendor.c_str(), sizeof(Vendor)); + strncpy(CS, _strCallsign.c_str(), sizeof(CS)-1); + CS[29]=0; + strncpy(Vendor, _strVendor.c_str(), sizeof(Vendor)-1); + Vendor[29]=0; // PGN129810 SetN2kAISClassBStaticPartB(N2kMsg, _uMsgType, (tN2kAISRepeat)_repeat, _uMmsi, @@ -255,24 +293,17 @@ class MyAisDecoder : public AIS::AisDecoder std::string msg(_strMessage.data(), _strMessage.size()); AIS::stripTrailingWhitespace(msg); - Serial.printf("%s [%s]\n", _strError.c_str(), msg.c_str()); + LOG_DEBUG(GwLog::ERROR,"%s [%s]\n", _strError.c_str(), msg.c_str()); } virtual void onParseError(const AIS::StringRef &_strMessage, const std::string &_strError) override { std::string msg(_strMessage.data(), _strMessage.size()); AIS::stripTrailingWhitespace(msg); - Serial.printf("%s [%s]\n", _strError.c_str(), msg.c_str()); + LOG_DEBUG(GwLog::ERROR,"%s [%s]\n", _strError.c_str(), msg.c_str()); } public: void handleMessage(const char * msg){ - int len=strlen(msg); - char buffer[len+1]; - memcpy(buffer,msg,len); - strcat(buffer,"\n"); - size_t i=0; - do { - i=decodeMsg(buffer,len+1,i,parser); - } while (i != 0) ; + size_t i=decodeMsg(msg,strlen(msg),0,parser,true); } }; diff --git a/lib/nmea0183ton2k/NMEA0183DataToN2K.cpp b/lib/nmea0183ton2k/NMEA0183DataToN2K.cpp index 8135ccf..b15ba02 100644 --- a/lib/nmea0183ton2k/NMEA0183DataToN2K.cpp +++ b/lib/nmea0183ton2k/NMEA0183DataToN2K.cpp @@ -38,6 +38,7 @@ class SNMEA0183Msg : public tNMEA0183Msg{ if (!isAis) return MessageCode(); char buf[6]; strncpy(buf,line+1,5); + buf[5]=0; return String(buf); } @@ -233,7 +234,7 @@ private: converters.registerConverter( 126992UL,129025UL,129026UL,127258UL, String(F("RMC")), &NMEA0183DataToN2KFunctions::convertRMC); - unsigned long aispgns[7]{129810UL,129809UL,129040UL,129039UL,129802UL,129794UL,129038UL}; + unsigned long *aispgns=new unsigned long[7]{129810UL,129809UL,129040UL,129039UL,129802UL,129794UL,129038UL}; converters.registerConverter(7,&aispgns[0], String(F("AIVDM")),&NMEA0183DataToN2KFunctions::convertAIVDX); converters.registerConverter(7,&aispgns[0], @@ -256,7 +257,7 @@ public: bool rt = converters.handleMessage(code, msg, this); if (!rt) { - LOG_DEBUG(GwLog::DEBUG, "NMEA0183DataToN2K[%d] no handler for %s", sourceId, buffer); + LOG_DEBUG(GwLog::DEBUG, "NMEA0183DataToN2K[%d] no handler for (%s) %s", sourceId, code.c_str(), buffer); } else{ LOG_DEBUG(GwLog::DEBUG+1, "NMEA0183DataToN2K[%d] handler done ", sourceId); @@ -276,7 +277,7 @@ public: NMEA0183DataToN2KFunctions(GwLog *logger, GwBoatData *boatData, N2kSender callback) : NMEA0183DataToN2K(logger, boatData, callback) { - aisDecoder= new MyAisDecoder(this->sender); + aisDecoder= new MyAisDecoder(logger,this->sender); registerConverters(); LOG_DEBUG(GwLog::LOG, "NMEA0183DataToN2KFunctions: registered %d converters", converters.numConverters()); } diff --git a/src/main.cpp b/src/main.cpp index 136f051..d40c638 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -14,8 +14,8 @@ #define VERSION "0.3.1" -//#define GW_MESSAGE_DEBUG_ENABLED -//#define FALLBACK_SERIAL +// #define GW_MESSAGE_DEBUG_ENABLED +// #define FALLBACK_SERIAL const unsigned long HEAP_REPORT_TIME=2000; //set to 0 to disable heap reporting #include "GwHardware.h"