From 28abd1674917f9a2acd94b5657c7c2067383daa6 Mon Sep 17 00:00:00 2001 From: andreas Date: Fri, 12 Nov 2021 21:14:38 +0100 Subject: [PATCH] intermediate: handle sat status --- lib/boatData/GwBoatData.cpp | 3 + lib/boatData/GwBoatData.h | 75 ++++++++++++++++++++++++- lib/nmea0183ton2k/NMEA0183DataToN2K.cpp | 31 +++++++++- lib/nmea2kto0183/N2kDataToNMEA0183.cpp | 33 +++++++++++ 4 files changed, 140 insertions(+), 2 deletions(-) diff --git a/lib/boatData/GwBoatData.cpp b/lib/boatData/GwBoatData.cpp index e8ec13d..f3874a3 100644 --- a/lib/boatData/GwBoatData.cpp +++ b/lib/boatData/GwBoatData.cpp @@ -61,3 +61,6 @@ double mtr2nm(double m) return m / 1852; } +bool convertToJson(const GwSatInfoList &si,JsonVariant &variant){ + return variant.set(si.getNumSats()); +} \ No newline at end of file diff --git a/lib/boatData/GwBoatData.h b/lib/boatData/GwBoatData.h index a7fa511..218545d 100644 --- a/lib/boatData/GwBoatData.h +++ b/lib/boatData/GwBoatData.h @@ -55,7 +55,7 @@ class GwBoatItemBase{ }; class GwBoatData; template class GwBoatItem : public GwBoatItemBase{ - private: + protected: T data; int lastUpdateSource; public: @@ -106,9 +106,80 @@ double formatKnots(double cv); uint32_t mtr2nm(uint32_t m); double mtr2nm(double m); +class GwSatInfo{ + public: + unsigned char PRN; + uint32_t Elevation; + uint32_t Azimut; + uint32_t SNR; + unsigned long timestamp; +}; +class GwSatInfoList{ + public: + static const unsigned long lifeTime=32000; + std::vector sats; + void houseKeeping(unsigned long ts=0){ + if (ts == 0) ts=millis(); + sats.erase(std::remove_if( + sats.begin(), + sats.end(), + [ts,this](const GwSatInfo &info){ + return (info.timestamp + lifeTime) < ts; + } + ),sats.end()); + } + void update(GwSatInfo entry){ + unsigned long now=millis(); + entry.timestamp=now; + for (auto it=sats.begin();it!=sats.end();it++){ + if (it->PRN == entry.PRN){ + *it=entry; + houseKeeping(); + return; + } + } + houseKeeping(); + sats.push_back(entry); + } + int getNumSats() const{ + return sats.size(); + } + GwSatInfo *getAt(int idx){ + if (idx > 0 && idx < sats.size()) return &sats.at(idx); + return NULL; + } +}; + +bool convertToJson(const GwSatInfoList &si,JsonVariant &variant); +class GwBoatDataSatList : public GwBoatItem +{ +public: + GwBoatDataSatList(String name, String formatInfo, unsigned long invalidTime = INVALID_TIME, GwBoatItemMap *map = NULL) : GwBoatItem(name, formatInfo, invalidTime, map) {} + bool update(GwSatInfo info, int source) + { + unsigned long now = millis(); + if (isValid(now)) + { + //priority handling + //sources with lower ids will win + //and we will not overwrite their value + if (lastUpdateSource < source) + { + return false; + } + } + lastUpdateSource = source; + uls(now); + data.update(info); + return true; + } + +}; #define GWBOATDATA(type,name,time,fmt) \ GwBoatItem *name=new GwBoatItem(F(#name),GwBoatItemBase::fmt,time,&values) ; +#define GWSPECBOATDATA(clazz,name,time,fmt) \ + clazz *name=new clazz(F(#name),GwBoatItemBase::fmt,time,&values) ; class GwBoatData{ private: GwLog *logger; @@ -131,6 +202,7 @@ class GwBoatData{ GWBOATDATA(double,Deviation,4000,formatCourse) GWBOATDATA(double,HDOP,4000,formatDop) GWBOATDATA(double,PDOP,4000,formatDop) + GWBOATDATA(double,VDOP,4000,formatDop) GWBOATDATA(double,RudderPosition,4000,formatCourse) GWBOATDATA(double,Latitude,4000,formatLatitude) GWBOATDATA(double,Longitude,4000,formatLongitude) @@ -148,6 +220,7 @@ class GwBoatData{ GWBOATDATA(uint32_t,TripLog,0,mtr2nm) GWBOATDATA(uint32_t,DaysSince1970,4000,formatFixed0) GWBOATDATA(int16_t,Timezone,8000,formatFixed0) + GWSPECBOATDATA(GwBoatDataSatList,SatInfo,GwSatInfoList::lifeTime,formatFixed0); public: GwBoatData(GwLog *logger); ~GwBoatData(); diff --git a/lib/nmea0183ton2k/NMEA0183DataToN2K.cpp b/lib/nmea0183ton2k/NMEA0183DataToN2K.cpp index cb6af00..1ef60ef 100644 --- a/lib/nmea0183ton2k/NMEA0183DataToN2K.cpp +++ b/lib/nmea0183ton2k/NMEA0183DataToN2K.cpp @@ -612,6 +612,32 @@ private: DGPSAge); send(n2kMsg); } + void convertGSA(const SNMEA0183Msg &msg){ + if (msg.FieldCount() < 17) + { + LOG_DEBUG(GwLog::DEBUG, "failed to parse GSA %s", msg.line); + return; + } + int fixMode=atoi(msg.Field(1)); + + tN2kMsg n2kMsg; + tN2kGNSSDOPmode mode=N2kGNSSdm_Unavailable; + if (fixMode >= 0 && fixMode <=3) mode=(tN2kGNSSDOPmode)fixMode; + tN2kGNSSDOPmode rmode=mode; + if (msg.Field(0)[0] == 'A') rmode=N2kGNSSdm_Auto; + double HDOP=N2kDoubleNA; + double VDOP=N2kDoubleNA; + if (msg.FieldLen(15)> 0){ + HDOP=atof(msg.Field(15)); + if (!updateDouble(boatData->HDOP,HDOP,msg.sourceId)) return; + } + if (msg.FieldLen(16)> 0){ + VDOP=atof(msg.Field(16)); + if (!updateDouble(boatData->VDOP,VDOP,msg.sourceId)) return; + } + SetN2kGNSSDOPData(n2kMsg,1,rmode,mode,HDOP,VDOP,N2kDoubleNA); + send(n2kMsg); + } //shortcut for lambda converters #define CVL [](const SNMEA0183Msg &msg, NMEA0183DataToN2KFunctions *p) -> void @@ -666,7 +692,10 @@ private: String(F("ZDA")), &NMEA0183DataToN2KFunctions::convertZDA); converters.registerConverter( 129029UL, - String(F("GGA")), &NMEA0183DataToN2KFunctions::convertGGA); + String(F("GGA")), &NMEA0183DataToN2KFunctions::convertGGA); + converters.registerConverter( + 129539UL, + String(F("GSA")), &NMEA0183DataToN2KFunctions::convertGSA); 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 996dc9a..bb616d3 100644 --- a/lib/nmea2kto0183/N2kDataToNMEA0183.cpp +++ b/lib/nmea2kto0183/N2kDataToNMEA0183.cpp @@ -323,6 +323,37 @@ private: } } } + void HandleDop(const tN2kMsg &msg){ + double HDOP=N2kDoubleNA; + double VDOP=N2kDoubleNA; + double TDOP; + tN2kGNSSDOPmode DesiredMode; + tN2kGNSSDOPmode ActualMode; + unsigned char SID; + if (ParseN2kGNSSDOPData(msg,SID, DesiredMode, ActualMode, + HDOP, VDOP, TDOP)){ + updateDouble(boatData->HDOP,HDOP); + updateDouble(boatData->VDOP,VDOP); + } + } + 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;iSatInfo->update(satInfo,sourceId)) return; + } + } + } + } //***************************************************************************** void HandleWind(const tN2kMsg &N2kMsg) @@ -948,6 +979,8 @@ private: converters.registerConverter(130310UL, &N2kToNMEA0183Functions::HandleWaterTemp); converters.registerConverter(126992UL, &N2kToNMEA0183Functions::HandleSystemTime); converters.registerConverter(129033UL, &N2kToNMEA0183Functions::HandleTimeOffset); + converters.registerConverter(129539UL, &N2kToNMEA0183Functions::HandleDop); + converters.registerConverter(129540UL, &N2kToNMEA0183Functions::HandleSats); #define HANDLE_AIS #ifdef HANDLE_AIS converters.registerConverter(129038UL, &N2kToNMEA0183Functions::HandleAISClassAPosReport); // AIS Class A Position Report, Message Type 1