diff --git a/lib/boatData/GwBoatData.h b/lib/boatData/GwBoatData.h index 8063a42..5eabb79 100644 --- a/lib/boatData/GwBoatData.h +++ b/lib/boatData/GwBoatData.h @@ -159,7 +159,7 @@ class GwSatInfoList{ return sats.size(); } GwSatInfo *getAt(int idx){ - if (idx > 0 && idx < sats.size()) return &sats.at(idx); + if (idx >= 0 && idx < sats.size()) return &sats.at(idx); return NULL; } }; @@ -191,6 +191,14 @@ public: data.houseKeeping(); GwBoatItem::toJsonDoc(doc,minTime); } + GwSatInfo *getAt(int idx){ + if (! isValid()) return NULL; + return data.getAt(idx); + } + int getNumSats(){ + if (! isValid()) return 0; + return data.getNumSats(); + } }; diff --git a/lib/nmea0183ton2k/NMEA0183DataToN2K.cpp b/lib/nmea0183ton2k/NMEA0183DataToN2K.cpp index 785b86f..3f17c4b 100644 --- a/lib/nmea0183ton2k/NMEA0183DataToN2K.cpp +++ b/lib/nmea0183ton2k/NMEA0183DataToN2K.cpp @@ -631,6 +631,11 @@ private: if (msg.Field(0)[0] == 'A') rmode=N2kGNSSdm_Auto; double HDOP=N2kDoubleNA; double VDOP=N2kDoubleNA; + double PDOP=N2kDoubleNA; + if (msg.FieldLen(14)> 0){ + PDOP=atof(msg.Field(14)); + if (!updateDouble(boatData->PDOP,PDOP,msg.sourceId)) return; + } if (msg.FieldLen(15)> 0){ HDOP=atof(msg.Field(15)); if (!updateDouble(boatData->HDOP,HDOP,msg.sourceId)) return; @@ -642,6 +647,57 @@ private: SetN2kGNSSDOPData(n2kMsg,1,rmode,mode,HDOP,VDOP,N2kDoubleNA); send(n2kMsg); } + void convertGSV(const SNMEA0183Msg &msg){ + if (msg.FieldCount() < 7){ + LOG_DEBUG(GwLog::DEBUG,"unable to parse GSV %s",msg.line); + return; + } + uint32_t total=atoi(msg.Field(0)); + if (total < 1 || total > 9) { + LOG_DEBUG(GwLog::DEBUG,"GSV invalid total %u %s",total,msg.line); + return; + } + uint32_t current=atoi(msg.Field(1)); + if (current < 1 || current > total) { + LOG_DEBUG(GwLog::DEBUG,"GSV invalid current %u %s",current,msg.line); + return; + } + for (int idx=2;idx < msg.FieldCount();idx+=4){ + if (msg.FieldLen(idx) < 1 || + msg.FieldLen(idx+1) < 1 || + msg.FieldLen(idx+2) < 1 || + msg.FieldLen(idx+3) < 1 + ) continue; + GwSatInfo info; + info.PRN=atoi(msg.Field(idx)); + info.Elevation=atoi(msg.Field(idx+1)); + info.Azimut=atoi(msg.Field(idx+2)); + info.SNR=atoi(msg.Field(idx+3)); + if (!boatData->SatInfo->update(info,msg.sourceId)) return; + } + int numSat=boatData->SatInfo->getNumSats(); + if (current == total && numSat > 0){ + tN2kMsg n2kMsg; + SetN2kGNSSSatellitesInView(n2kMsg,1); + bool hasInfos=false; + for (int i=0;iSatInfo->getAt(i); + if (gwInfo){ + hasInfos=true; + satInfo.PRN=gwInfo->PRN; + satInfo.SNR=gwInfo->SNR; + satInfo.Elevation=DegToRad(gwInfo->Elevation); + satInfo.Azimuth=DegToRad(gwInfo->Azimut); + AppendSatelliteInfo(n2kMsg,satInfo); + } + } + if (hasInfos){ + send(n2kMsg); + } + + } + } //shortcut for lambda converters #define CVL [](const SNMEA0183Msg &msg, NMEA0183DataToN2KFunctions *p) -> void @@ -699,7 +755,10 @@ private: String(F("GGA")), &NMEA0183DataToN2KFunctions::convertGGA); converters.registerConverter( 129539UL, - String(F("GSA")), &NMEA0183DataToN2KFunctions::convertGSA); + String(F("GSA")), &NMEA0183DataToN2KFunctions::convertGSA); + converters.registerConverter( + 129540UL, + String(F("GSV")), &NMEA0183DataToN2KFunctions::convertGSV); 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 b6efb74..af1bad8 100644 --- a/lib/nmea2kto0183/N2kDataToNMEA0183.cpp +++ b/lib/nmea2kto0183/N2kDataToNMEA0183.cpp @@ -278,6 +278,32 @@ private: } } } + 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) @@ -331,6 +357,7 @@ private: HDOP, VDOP, TDOP)){ updateDouble(boatData->HDOP,HDOP); updateDouble(boatData->VDOP,VDOP); + sendGSA(DesiredMode==N2kGNSSdm_Auto,(int)ActualMode); } } void HandleSats(const tN2kMsg &msg){ @@ -343,12 +370,48 @@ private: if (ParseN2kPGNSatellitesInView(msg,i,info)){ GwSatInfo satInfo; satInfo.PRN=info.PRN; - satInfo.Elevation=info.Elevation; - satInfo.Azimut=info.Azimuth; + 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;iSatInfo->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); + } + } + } } }