feature/vtgnmea adds Vector Track and Speed over Ground (VTG) NMEA sentence to gpsdlocationagent

[sgalgano]

Added Vector Track and Speed over Ground (VTG) NMEA sentence support to
gdpsdlocatonagent. Velocity location events are used to generate VTG
sentences.

Submitted-by: Leonid Veytser
Developed-by: James Kirsch

Patched submitted via email. Slight modifications made to better conform with module style.

diff --git a/src/agents/gpsdlocation/agent.cc b/src/agents/gpsdlocation/agent.cc
index ef5f2b1..34d4e00 100644
--- a/src/agents/gpsdlocation/agent.cc
+++ b/src/agents/gpsdlocation/agent.cc
@@ -3,6 +3,7 @@
  * Copyright (c) 2008-2009 - DRS CenGen, LLC, Columbia, Maryland
  * All rights reserved.
  *
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
  * are met:
@@ -60,9 +61,12 @@ EMANE::Agents::GPSDLocation::Agent::Agent(NEMId nemId,
   masterPTY_{},
   slavePTY_{},
   bHaveInitialPosition_{false},
+  bHaveInitialVelocity_{false},
   dLatitudeDegrees_{},
   dLongitudeDegrees_{},
   dAltitudeMeters_{},
+  dAzimuthDegrees_{},
+  dMagnitudeMetersPerSecond_{},
   timerId_{}{}

 EMANE::Agents::GPSDLocation::Agent::~Agent(){}
@@ -286,6 +290,13 @@ void EMANE::Agents::GPSDLocation::Agent::processEvent(const EventId & eventId,
           dLatitudeDegrees_ = position.getLatitudeDegrees();
           dLongitudeDegrees_ = position.getLongitudeDegrees();
           dAltitudeMeters_ = position.getAltitudeMeters();
+          auto velocity = iter->getVelocity();
+     if (velocity.second == true) // valid
+            {
+         dAzimuthDegrees_ = velocity.first.getAzimuthDegrees();
+         dMagnitudeMetersPerSecond_ = velocity.first.getMagnitudeMetersPerSecond();
+              bHaveInitialVelocity_ = true;
+            }
           bHaveInitialPosition_ = true;
         }
     }      
@@ -308,6 +319,18 @@ void  EMANE::Agents::GPSDLocation::Agent::processTimedEvent(TimerEventId,
                               dLatitudeDegrees_,
                               dLongitudeDegrees_,
                               dAltitudeMeters_);
+
+      if(bHaveInitialVelocity_)
+        {
+          sendSpoofedGPVTG(dAzimuthDegrees_, dMagnitudeMetersPerSecond_);
+
+          LOGGER_STANDARD_LOGGING(pPlatformService_->logService(),
+                                 DEBUG_LEVEL,
+                                 "gpsdlocationagent NEM: %hu azm: %lf mag: %lf",
+                                 nemId_,
+                                 dAzimuthDegrees_,
+                                 dMagnitudeMetersPerSecond_);
+        }
     }
 }

@@ -441,6 +464,28 @@ void  EMANE::Agents::GPSDLocation::Agent::sendSpoofedNMEA(double dLatitude, doub
     }
 }

+void  EMANE::Agents::GPSDLocation::Agent::sendSpoofedGPVTG(double dAzimuth, double dMagnitude)
+{
+  char buf[1024];
+
+  double dMagnitudeKnots = dMagnitude * 1.94384;
+  double dMagnitudeKilometerPerHour = (dMagnitude * 3600.0) / 1000.0;
+
+  /* NMEA GPVTG */
+  snprintf(buf,sizeof(buf),"$GPVTG,%.1f,%C,%.1f,%C,%.1f,%C,%.1f,%C",
+           dAzimuth,
+           'T', 
+           dAzimuth,
+           'M',
+           dMagnitudeKnots,
+           'N',
+           dMagnitudeKilometerPerHour,
+           'K'
+           );
+  
+  doCheckSumNMEA(buf,sizeof(buf));
+  write(masterPTY_,buf,strlen(buf));
+}

 void  EMANE::Agents::GPSDLocation::Agent::doCheckSumNMEA(char *buf, size_t len)
 {
diff --git a/src/agents/gpsdlocation/agent.h b/src/agents/gpsdlocation/agent.h
index 714086f..9ad4f55 100644
--- a/src/agents/gpsdlocation/agent.h
+++ b/src/agents/gpsdlocation/agent.h
@@ -92,9 +92,12 @@ namespace EMANE
         bool bGPSDConnectionEnabled_;
         ACE_FILE_IO pseudoTerminalNameFile_;
         bool bHaveInitialPosition_;
+        bool bHaveInitialVelocity_;
         double dLatitudeDegrees_;
         double dLongitudeDegrees_;
         double dAltitudeMeters_;
+   double dAzimuthDegrees_;
+   double dMagnitudeMetersPerSecond_; 
         TimerEventId timerId_;

         /**
@@ -106,6 +109,15 @@ namespace EMANE
          *
          */
         void sendSpoofedNMEA(double dLatitude, double dLongitude, double dAltitude);
+
+        /**
+         * Convert attitude in to NMEA strings and send to gpsd via pseudo  terminal
+         *
+         * @param dAzimuth   Azimuth in degrees
+         * @param dMagnitude Magnitude in meters per second
+         *
+         */
+   void sendSpoofedGPVTG(double dAzimuth, double dMagnitude);

         void doCheckSumNMEA(char *buf, size_t len);
       };

[sgalgano]

Verified existing functionality still works. Will merge to develop for follow on testing in the release preparation branch.