diff --git a/lib/boatData/GwBoatData.h b/lib/boatData/GwBoatData.h index 4c39689..5ffab7a 100644 --- a/lib/boatData/GwBoatData.h +++ b/lib/boatData/GwBoatData.h @@ -146,8 +146,10 @@ class GwBoatData{ GWBOATDATA(double,MaxTws,4000,&formatKnots) GWBOATDATA(double,MaxAws,4000,&formatKnots) GWBOATDATA(double,AWA,4000,&formatWind) - GWBOATDATA(double,Heading,4000,&formatCourse) + GWBOATDATA(double,Heading,4000,&formatCourse) //true + GWBOATDATA(double,MagneticHeading,4000,&formatCourse) GWBOATDATA(double,Variation,4000,&formatCourse) + GWBOATDATA(double,Deviation,4000,&formatCourse) GWBOATDATA(double,RudderPosition,4000,&formatCourse) GWBOATDATA(double,Latitude,4000,NULL) GWBOATDATA(double,Longitude,4000,NULL) diff --git a/lib/nmea0183ton2k/NMEA0183DataToN2K.cpp b/lib/nmea0183ton2k/NMEA0183DataToN2K.cpp index 741df73..29157b6 100644 --- a/lib/nmea0183ton2k/NMEA0183DataToN2K.cpp +++ b/lib/nmea0183ton2k/NMEA0183DataToN2K.cpp @@ -345,6 +345,66 @@ private: } } + void convertHDM(const SNMEA0183Msg &msg){ + double MagneticHeading=NMEA0183DoubleNA; + if (!NMEA0183ParseHDM_nc(msg, MagneticHeading)) + { + logger->logDebug(GwLog::DEBUG, "failed to parse HDM %s", msg.line); + return; + } + if (! UD(MagneticHeading)) return; + tN2kMsg n2kMsg; + SetN2kMagneticHeading(n2kMsg,1,MagneticHeading, + boatData->Variation->getDataWithDefault(N2kDoubleNA), + boatData->Deviation->getDataWithDefault(N2kDoubleNA) + ); + send(n2kMsg); + } + + void convertHDT(const SNMEA0183Msg &msg){ + double Heading=NMEA0183DoubleNA; + if (!NMEA0183ParseHDT_nc(msg, Heading)) + { + logger->logDebug(GwLog::DEBUG, "failed to parse HDT %s", msg.line); + return; + } + if (! UD(Heading)) return; + tN2kMsg n2kMsg; + SetN2kTrueHeading(n2kMsg,1,Heading); + send(n2kMsg); + } + void convertHDG(const SNMEA0183Msg &msg){ + double MagneticHeading=NMEA0183DoubleNA; + double Variation=NMEA0183DoubleNA; + double Deviation=NMEA0183DoubleNA; + if (msg.FieldCount() < 5) + { + logger->logDebug(GwLog::DEBUG, "failed to parse HDG %s", msg.line); + return; + } + if (msg.FieldLen(0)>0){ + MagneticHeading=formatDegToRad(atof(msg.Field(0))); + } + else{ + return; + } + if (msg.FieldLen(1)>0){ + Deviation=formatDegToRad(atof(msg.Field(1))); + if (msg.Field(2)[0] == 'W') Deviation=-Deviation; + } + if (msg.FieldLen(3)>0){ + Variation=formatDegToRad(atof(msg.Field(3))); + if (msg.Field(4)[0] == 'W') Variation=-Variation; + } + + if (! UD(MagneticHeading)) return; + UD(Variation); + UD(Deviation); + tN2kMsg n2kMsg; + SetN2kMagneticHeading(n2kMsg,1,MagneticHeading,Deviation,Variation); + send(n2kMsg); + } + //shortcut for lambda converters #define CVL [](const SNMEA0183Msg &msg, NMEA0183DataToN2KFunctions *p) -> void void registerConverters() @@ -362,7 +422,16 @@ private: String(F("MWD")),&NMEA0183DataToN2KFunctions::convertMWD); converters.registerConverter( 130306UL, - String(F("VWR")),&NMEA0183DataToN2KFunctions::convertVWR); + String(F("VWR")),&NMEA0183DataToN2KFunctions::convertVWR); + converters.registerConverter( + 127250UL, + String(F("HDM")),&NMEA0183DataToN2KFunctions::convertHDM); + converters.registerConverter( + 127250UL, + String(F("HDT")),&NMEA0183DataToN2KFunctions::convertHDT); + converters.registerConverter( + 127250UL, + String(F("HDG")),&NMEA0183DataToN2KFunctions::convertHDG); unsigned long *aispgns=new unsigned long[7]{129810UL,129809UL,129040UL,129039UL,129802UL,129794UL,129038UL}; converters.registerConverter(7,&aispgns[0], String(F("AIVDM")),&NMEA0183DataToN2KFunctions::convertAIVDX); diff --git a/lib/nmea2kto0183/N2kDataToNMEA0183.cpp b/lib/nmea2kto0183/N2kDataToNMEA0183.cpp index 04b2860..62a0330 100644 --- a/lib/nmea2kto0183/N2kDataToNMEA0183.cpp +++ b/lib/nmea2kto0183/N2kDataToNMEA0183.cpp @@ -122,20 +122,58 @@ private: 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)) { - if (ref == N2khr_magnetic) - { - updateDouble(boatData->Variation, Variation); // Update Variation - if (!N2kIsNA(Heading) && boatData->Variation->isValid(millis())) - Heading -= boatData->Variation->getData(); + updateDouble(boatData->Variation,Variation); + updateDouble(boatData->Deviation,_Deviation); + if (N2kIsNA(Variation)){ + //maybe we still have a valid variation + Variation=boatData->Variation->getDataWithDefault(N2kDoubleNA); } - updateDouble(boatData->Heading, Heading); - if (NMEA0183SetHDG(NMEA0183Msg, boatData->Heading->getDataWithDefault(NMEA0183DoubleNA), _Deviation, boatData->Variation->getDataWithDefault(NMEA0183DoubleNA))) - { - SendMessage(NMEA0183Msg); + if (N2kIsNA(Variation)){ + //no variation + if (ref == N2khr_magnetic){ + updateDouble(boatData->MagneticHeading,Heading); + if (NMEA0183SetHDM(NMEA0183Msg,Heading)){ + SendMessage(NMEA0183Msg); + } + } + if (ref == N2khr_true){ + updateDouble(boatData->Heading,Heading); + if (NMEA0183SetHDT(NMEA0183Msg,Heading)){ + SendMessage(NMEA0183Msg); + } + } } + else{ + double MagneticHeading=N2kDoubleNA; + if (ref == N2khr_magnetic){ + MagneticHeading=Heading; + Heading+=Variation; + } + if (ref == N2khr_true){ + MagneticHeading=Heading-Variation; + } + updateDouble(boatData->MagneticHeading,MagneticHeading); + updateDouble(boatData->Heading,Heading); + if (!N2kIsNA(MagneticHeading)){ + if (NMEA0183SetHDG(NMEA0183Msg, MagneticHeading,_Deviation, + Variation)) + { + SendMessage(NMEA0183Msg); + } + } + if (!N2kIsNA(Heading)){ + if (NMEA0183SetHDT(NMEA0183Msg, Heading)) + { + SendMessage(NMEA0183Msg); + } + } + } } + } //*****************************************************************************