EMANE  1.2.1
agent.cc
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013-2014,2016 - Adjacent Link LLC, Bridgewater, New
3  * Jersey
4  * Copyright (c) 2008-2009 - DRS CenGen, LLC, Columbia, Maryland
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above copyright
14  * notice, this list of conditions and the following disclaimer in
15  * the documentation and/or other materials provided with the
16  * distribution.
17  * * Neither the name of DRS CenGen, LLC nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #include "agent.h"
36 #include "emane/constants.h"
39 #include "emane/startexception.h"
40 
41 #include <sstream>
42 
43 #include <pty.h>
44 #include <termios.h>
45 #include <unistd.h>
46 #include <climits>
47 #include <fstream>
48 #include <cstring>
49 
51  PlatformServiceProvider *pPlatformService):
52  EventAgent{nemId, pPlatformService},
53  nemId_{nemId},
54  masterPTY_{},
55  slavePTY_{},
56  bHaveInitialPosition_{false},
57  bHaveInitialVelocity_{false},
58  dLatitudeDegrees_{},
59  dLongitudeDegrees_{},
60  dAltitudeMeters_{},
61  dAzimuthDegrees_{},
62  dMagnitudeMetersPerSecond_{},
63  timerId_{}{}
64 
66 
68 {
69  auto & configRegistrar = registrar.configurationRegistrar();
70 
71  configRegistrar.registerNonNumeric<std::string>("pseudoterminalfile",
73  {"/tmp/gps.pty"},
74  "File to write the name of the created pseudo term.");
75 
76  auto & eventRegistrar = registrar.eventRegistrar();
77 
79 }
80 
82 {
83  for(const auto & item : update)
84  {
85  if(item.first == "pseudoterminalfile")
86  {
87  sPseudoTerminalFile_ = item.second[0].asString();
88  }
89  else
90  {
91  throw makeException<ConfigureException>("GPSDLocation::Agent: "
92  "Unexpected configuration item %s",
93  item.first.c_str());
94  }
95  }
96 }
97 
98 
100 {
101  char pzName[PATH_MAX]{};
102 
103  if(openpty(&masterPTY_, &slavePTY_, nullptr,nullptr,nullptr) == -1)
104  {
105  throw StartException("Unable to open pseudo terminal for gpsd");
106  }
107 
108  if(ttyname_r(slavePTY_,&pzName[1],sizeof(pzName)-1) == -1)
109  {
110  throw StartException("Unable to determin pseudo terminal name");
111  }
112 
113  struct termios term;
114 
115  tcgetattr(slavePTY_,&term);
116 
117  term.c_iflag = 0;
118  term.c_oflag = 0;
119  term.c_lflag = 0;
120 
121  term.c_cflag &=~ (PARENB | PARODD | CRTSCTS | CSIZE |
122  CSTOPB | PARENB | PARODD | CS8);
123 
124  term.c_cflag |= CREAD | CLOCAL;
125 
126  term.c_iflag &=~ (PARMRK | INPCK);
127 
128  term.c_cc[VMIN] = 1;
129 
130  cfsetispeed(&term, B4800);
131  cfsetospeed(&term, B4800);
132 
133  tcsetattr(slavePTY_,TCSANOW,&term);
134 
135  close(slavePTY_);
136 
137  // open up the file containing the name of the pseudo ternimal
138  std::ofstream ofs{sPseudoTerminalFile_.c_str(),std::ios::trunc};
139 
140  ofs<<&pzName[1]<<std::endl;
141 
142  ofs.close();
143 
144  std::chrono::seconds interval{1};
145 
146  timerId_ = pPlatformService_->timerService().scheduleTimedEvent(Clock::now() + interval,
147  nullptr,
148  interval);
149 
151 }
152 
154 {
155  // remove the pseudo terminal filename file
157  sPseudoTerminalFile_.c_str());
158 
159  unlink(sPseudoTerminalFile_.c_str());
160 
162 }
163 
165  throw()
166 {}
167 
169  const Serialization & serialization)
170 {
172  DEBUG_LEVEL,
173  "GPSDLocation::Agent NEM: %hu processEvent Called",
174  nemId_);
175 
176  if(eventId == Events::LocationEvent::IDENTIFIER)
177  {
178  Events::LocationEvent event{serialization};
179 
180  const auto & locations = event.getLocations();
181 
182  auto iter = std::find_if(locations.begin(),
183  locations.end(),
184  [this](const Events::Location & p)
185  {
186  return p.getNEMId() == nemId_;
187  });
188 
189  if(iter != locations.end())
190  {
191  const auto & position = iter->getPosition();
192  dLatitudeDegrees_ = position.getLatitudeDegrees();
193  dLongitudeDegrees_ = position.getLongitudeDegrees();
194  dAltitudeMeters_ = position.getAltitudeMeters();
195  bHaveInitialPosition_ = true;
196 
197  auto velocity = iter->getVelocity();
198 
199  if(velocity.second)
200  {
201  dAzimuthDegrees_ = velocity.first.getAzimuthDegrees();
202  dMagnitudeMetersPerSecond_ = velocity.first.getMagnitudeMetersPerSecond();
203  bHaveInitialVelocity_ = true;
204  }
205  }
206  }
207 }
208 
210  const TimePoint &,
211  const TimePoint &,
212  const TimePoint &,
213  const void *)
214 {
215  if(bHaveInitialPosition_)
216  {
217  sendSpoofedNMEA(dLatitudeDegrees_,dLongitudeDegrees_,dAltitudeMeters_);
218 
220  DEBUG_LEVEL,
221  "gpsdlocationaganet NEM: %hu lat: %lf lon: %lf alt:%lf",
222  nemId_,
223  dLatitudeDegrees_,
224  dLongitudeDegrees_,
225  dAltitudeMeters_);
226 
227  if(bHaveInitialVelocity_)
228  {
229  sendSpoofedGPVTG(dAzimuthDegrees_,dMagnitudeMetersPerSecond_);
230 
232  DEBUG_LEVEL,
233  "gpsdlocationagent NEM: %hu azm: %lf mag: %lf",
234  nemId_,
235  dAzimuthDegrees_,
236  dMagnitudeMetersPerSecond_);
237  }
238  }
239 }
240 
241 void EMANE::Agents::GPSDLocation::Agent::sendSpoofedNMEA(double dLatitude, double dLongitude, double dAltitude)
242 {
243  time_t t;
244  tm tmval;
245  char buf[1024];
246 
247  const int NUM_GSV_STRINGS = 2;
248  const int NUM_SV_PER_STRING = 4;
249 
250  int elv[NUM_GSV_STRINGS * NUM_SV_PER_STRING] = {41, 9, 70, 35, 10, 53, 2, 48 };
251  int azm[NUM_GSV_STRINGS * NUM_SV_PER_STRING] = {104, 84, 30, 185, 297, 311, 29, 64 };
252  int snr[NUM_GSV_STRINGS * NUM_SV_PER_STRING] = {41, 51, 39, 25, 25, 21, 29, 32 };
253 
254  char cLatitudeHemisphere = (dLatitude > 0) ? 'N' : 'S';
255  char cLongitudeHemisphere = (dLongitude > 0) ? 'E' : 'W';
256 
257  if(dLatitude < 0)
258  {
259  dLatitude *= -1;
260  }
261 
262  if(dLongitude < 0)
263  {
264  dLongitude *= -1;
265  }
266 
267  int iLongitudeDegrees = static_cast<int>(dLongitude);
268  int iLatitudeDegrees = static_cast<int>(dLatitude);
269 
270  double dLongitudeMinutes = (dLongitude - iLongitudeDegrees) * 60.0;
271  double dLatitudeMinutes = (dLatitude - iLatitudeDegrees) * 60.0;
272 
273  int iLongitudeMinutes = static_cast<int>(dLongitudeMinutes);
274  int iLatitudeMinutes = static_cast<int>(dLatitudeMinutes);
275 
276  int iGPSquality = 2;
277  int iNumSatellitesUsed = NUM_GSV_STRINGS * NUM_SV_PER_STRING;
278 
279  double dDOP = 1.8;
280  double dHorizontalDOP = 1.1;
281  double dVerticalDOP = 1.3;
282 
283  double dGeoidalHeight = -34.0;
284 
285  dLongitudeMinutes -= iLongitudeMinutes;
286  dLatitudeMinutes -= iLatitudeMinutes;
287 
288  dLongitudeMinutes *= 10000;
289  dLatitudeMinutes *= 10000;
290 
291  time(&t);
292  gmtime_r(&t,&tmval);
293 
294  /* NMEA GGA */
295  snprintf(buf,sizeof(buf),"$GPGGA,%02d%02d%02d,%02d%02d.%04d,%c,%03d%02d.%04d,%c,%d,%02d,%.1f,%.1f,M,%.1f,M,,",
296  tmval.tm_hour,
297  tmval.tm_min,
298  tmval.tm_sec,
299  iLatitudeDegrees,
300  iLatitudeMinutes,
301  static_cast<int>(dLatitudeMinutes),
302  cLatitudeHemisphere,
303  iLongitudeDegrees,
304  iLongitudeMinutes,
305  static_cast<int>(dLongitudeMinutes),
306  cLongitudeHemisphere,
307  iGPSquality,
308  iNumSatellitesUsed,
309  dHorizontalDOP,
310  dAltitude,
311  dGeoidalHeight
312  );
313 
314  doCheckSumNMEA(buf,sizeof(buf));
315  write(masterPTY_,buf,strlen(buf));
316 
317 
318  /* NMEA RMC */
319  snprintf(buf,sizeof(buf),"$GPRMC,%02d%02d%02d,A,%02d%02d.%04d,%c,%03d%02d.%04d,%c,000.0,000.0,%02d%02d%02d,000.0,%c",
320  tmval.tm_hour,
321  tmval.tm_min,
322  tmval.tm_sec,
323  iLatitudeDegrees,
324  iLatitudeMinutes,
325  static_cast<int>(dLatitudeMinutes),
326  cLatitudeHemisphere,
327  iLongitudeDegrees,
328  iLongitudeMinutes,
329  static_cast<int>(dLongitudeMinutes),
330  cLongitudeHemisphere,
331  tmval.tm_mday,
332  tmval.tm_mon + 1,
333  (tmval.tm_year + 1900) % 100,
334  cLongitudeHemisphere
335  );
336 
337  doCheckSumNMEA(buf,sizeof(buf));
338  write(masterPTY_,buf,strlen(buf));
339 
340 
341  /* NMEA GSA */
342  snprintf(buf,sizeof(buf), "$GPGSA,A,3,01,02,03,04,05,06,07,08,,,,,%2.1f,%2.1f,%2.1f",
343  dDOP, dHorizontalDOP, dVerticalDOP);
344 
345  doCheckSumNMEA(buf,sizeof(buf));
346  write(masterPTY_,buf,strlen(buf));
347 
348 
349  for(int i = 0; i < NUM_GSV_STRINGS; ++i)
350  {
351  const int a = (NUM_SV_PER_STRING * i) + 0;
352  const int b = (NUM_SV_PER_STRING * i) + 1;
353  const int c = (NUM_SV_PER_STRING * i) + 2;
354  const int d = (NUM_SV_PER_STRING * i) + 3;
355 
356  /* NMEA GSV */
357  snprintf(buf,sizeof(buf), "$GPGSV,%d,%d,%02d,%02d,%02d,%03d,%02d,%02d,%02d,%03d,%02d,%02d,%02d,%03d,%02d,%02d,%02d,%03d,%02d",
358  NUM_GSV_STRINGS,
359  i + 1,
360  iNumSatellitesUsed,
361  a + 1, elv[a], azm[a], snr[a],
362  b + 2, elv[b], azm[b], snr[b],
363  c + 3, elv[c], azm[c], snr[c],
364  d + 4, elv[d], azm[d], snr[d]);
365 
366  doCheckSumNMEA(buf,sizeof(buf));
367  write(masterPTY_,buf,strlen(buf));
368  }
369 }
370 
371 void EMANE::Agents::GPSDLocation::Agent::sendSpoofedGPVTG(double dAzimuth, double dMagnitude)
372 {
373  char buf[1024];
374 
375  double dMagnitudeKnots = dMagnitude * 1.94384;
376  double dMagnitudeKilometerPerHour = (dMagnitude * 3600.0) / 1000.0;
377 
378  /* NMEA GPVTG */
379  snprintf(buf,sizeof(buf),"$GPVTG,%.1f,%C,%.1f,%C,%.1f,%C,%.1f,%C",
380  dAzimuth,
381  'T',
382  dAzimuth,
383  'M',
384  dMagnitudeKnots,
385  'N',
386  dMagnitudeKilometerPerHour,
387  'K'
388  );
389 
390  doCheckSumNMEA(buf,sizeof(buf));
391  write(masterPTY_,buf,strlen(buf));
392 }
393 
394 void EMANE::Agents::GPSDLocation::Agent::doCheckSumNMEA(char *buf, size_t len)
395 {
396  char foo[6] = {0};
397 
398  char chksum = buf[1];
399 
400  for(unsigned int i = 2; i < strlen(buf); ++i)
401  {
402  chksum ^= buf[i];
403  }
404 
405  snprintf(foo, sizeof(foo), "*%02X\r\n", static_cast<unsigned int>(chksum));
406 
407  strncat(buf, foo, len - strlen(buf) - 1);
408 }
409 
std::string Serialization
Definition: serializable.h:42
The Registrar interface provides access to all of the emulator registrars.
Definition: registrar.h:50
virtual ConfigurationRegistrar & configurationRegistrar()=0
void initialize(Registrar &registrar) override
Definition: agent.cc:67
virtual TimerEventId scheduleTimedEvent(const TimePoint &timePoint, const void *arg=nullptr, const Duration &interval=Duration::zero())=0
#define LOGGER_VERBOSE_LOGGING(logger, level, fmt, args...)
void processTimedEvent(TimerEventId eventId, const TimePoint &expireTime, const TimePoint &scheduleTime, const TimePoint &fireTime, const void *arg) override
Definition: agent.cc:209
void registerNonNumeric(const std::string &sName, const ConfigurationProperties &properties=ConfigurationProperties::NONE, const std::initializer_list< T > &values={}, const std::string &sUsage="", std::size_t minOccurs=1, std::size_t maxOccurs=1, const std::string &sRegexPattern={})
DECLARE_EVENT_AGENT(EMANE::Agents::GPSDLocation::Agent)
A location event is usd to set the position, orientation and velocity information for one or more NEM...
Definition: locationevent.h:52
std::uint16_t EventId
Definition: types.h:53
The PlatformServiceProvider interface provides access to emulator services.
virtual EventRegistrar & eventRegistrar()=0
A location entry holds an NEM Id, that NEM&#39;s position information and optional orientation and veloci...
Definition: location.h:61
Agent(NEMId nemId, PlatformServiceProvider *pPlatformService)
Definition: agent.cc:50
void processEvent(const EventId &, const Serialization &) override
Definition: agent.cc:168
Base class for all event agents.
Definition: eventagent.h:52
std::uint16_t NEMId
Definition: types.h:52
PlatformServiceProvider * pPlatformService_
void configure(const ConfigurationUpdate &update) override
Definition: agent.cc:81
virtual LogServiceProvider & logService()=0
Component start exception is used to indicate an exception during transition to the start state...
std::vector< ConfigurationNameAnyValues > ConfigurationUpdate
Agent translating location events to NMEA gpsd messages.
Definition: agent.h:54
virtual void registerEvent(EventId eventId)=0
std::size_t TimerEventId
Definition: types.h:54
Clock::time_point TimePoint
Definition: types.h:50
virtual bool cancelTimedEvent(TimerEventId eventId)=0
#define LOGGER_STANDARD_LOGGING(logger, level, fmt, args...)
const Locations & getLocations() const
virtual TimerServiceProvider & timerService()=0