FlightGear next
nmea.cxx
Go to the documentation of this file.
1/*
2 * SPDX-FileName: nmea.cxx
3 * SPDX-FileComment: NMEA protocol class
4 * SPDX-FileCopyrightText: Copyright (C) 1999 Curtis L. Olson - http://www.flightgear.org/~curt
5 * SPDX-License-Identifier: GPL-2.0-or-later
6 */
7
8#ifdef HAVE_CONFIG_H
9# include "config.h"
10#endif
11
12#include <cstdlib>
13#include <cstring>
14#include <cstdio>
15
16#include <simgear/debug/logstream.hxx>
17#include <simgear/math/sg_geodesy.hxx>
18#include <simgear/io/iochannel.hxx>
19#include <simgear/timing/sg_time.hxx>
20
21#include <FDM/flightProperties.hxx>
22#include <Main/fg_props.hxx>
23#include <Main/globals.hxx>
24
25#include "nmea.hxx"
26
27FGNMEA::FGNMEA() :
28 mLength(0),
29 mNmeaMessages(NMEA::SET),
30 // by default, expect 2 messages per iteration (input)
31 mMaxReceiveLines(2),
32 mBiDirectionalSupport(false), // protocol normally only supports input _or_ output
33 mLineFeed("\n")
34{
35}
36
37
40
41
42// calculate the NMEA check sum
43void FGNMEA::add_with_checksum(char *sentence, unsigned int buf_size) {
44 unsigned int i;
45 unsigned char sum = 0;
46
47 for (i = 1; sentence[i] != 0; i++ ) {
48 sum ^= sentence[i];
49 }
50
51 if (i + 6 < buf_size)
52 snprintf( &sentence[i], 6, "*%02X%s", sum, mLineFeed);
53
54 SG_LOG( SG_IO, SG_DEBUG, sentence );
55
56 mNmeaSentence += sentence;
57}
58
59
60// generate NMEA message
61bool FGNMEA::gen_message()
62{
63 char dir;
64 int deg;
65 double min;
66 char nmea[256];
67
68 SGTime *t = globals->get_time_params();
69
70 char utc[10];
71 snprintf( utc, sizeof(utc), "%02d%02d%02d",
72 t->getGmt()->tm_hour, t->getGmt()->tm_min, t->getGmt()->tm_sec );
73
74 char lat[20];
75 {
76 double latd = mFdm.get_Latitude() * SGD_RADIANS_TO_DEGREES;
77 if ( latd < 0.0 ) {
78 latd = -latd;
79 dir = 'S';
80 } else {
81 dir = 'N';
82 }
83 deg = (int)(latd);
84 min = (latd - (double)deg) * 60.0;
85 snprintf( lat, sizeof(lat), "%02d%07.4f,%c", abs(deg), min, dir);
86 }
87
88 char lon[20];
89 {
90 double lond = mFdm.get_Longitude() * SGD_RADIANS_TO_DEGREES;
91 if ( lond < 0.0 ) {
92 lond = -lond;
93 dir = 'W';
94 } else {
95 dir = 'E';
96 }
97 deg = (int)(lond);
98 min = (lond - (double)deg) * 60.0;
99 snprintf( lon, sizeof(lon), "%03d%07.4f,%c", abs(deg), min, dir);
100 }
101
102 double vn = fgGetDouble( "/velocities/speed-north-fps" );
103 double ve = fgGetDouble( "/velocities/speed-east-fps" );
104
105 char speed[10];
106 {
107 double fps = sqrt( vn*vn + ve*ve );
108 double mps = fps * SG_FEET_TO_METER;
109 double kts = mps * SG_METER_TO_NM * 3600;
110 snprintf( speed, sizeof(speed), "%.1f", kts );
111 }
112
113 char heading[10];
114 {
115 double hdg_true = atan2( ve, vn ) * SGD_RADIANS_TO_DEGREES;
116 if ( hdg_true < 0 ) {
117 hdg_true += 360.0;
118 }
119 snprintf( heading, sizeof(heading), "%.1f", hdg_true );
120 }
121
122 double altitude_ft = mFdm.get_Altitude();
123
124 char date[16];
125 {
126 unsigned short tm_mday = t->getGmt()->tm_mday;
127 unsigned short tm_mon = t->getGmt()->tm_mon + 1;
128 unsigned short tm_year = t->getGmt()->tm_year % 100;
129 snprintf(date, sizeof(date), "%02u%02u%02u", tm_mday, tm_mon, tm_year);
130 }
131
132 char magvar[10];
133 {
134 float magdeg = fgGetDouble( "/environment/magnetic-variation-deg" );
135 if ( magdeg < 0.0 ) {
136 magdeg = -magdeg;
137 dir = 'W';
138 } else {
139 dir = 'E';
140 }
141 snprintf( magvar, sizeof(magvar), "%.1f,%c", magdeg, dir );
142 }
143
144 // RMC sentence
145 if (mNmeaMessages & NMEA::GPRMC)
146 {
147 // $GPRMC,HHMMSS,A,DDMM.MMMM,N,DDDMM.MMMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E,A*XX
148 snprintf( nmea, sizeof(nmea), "$GPRMC,%s,A,%s,%s,%s,%s,%s,%s,A",
149 utc, lat, lon, speed, heading, date, magvar );
150 add_with_checksum(nmea, 256);
151 }
152
153 // GGA sentence
154 if (mNmeaMessages & NMEA::GPGGA)
155 {
156 // $GPGGA,HHMMSS,DDMM.MMMM,N,DDDMM.MMMM,W,1,NN,H.H,AAAA.A,M,GG.G,M,,*XX
157 snprintf( nmea, sizeof(nmea), "$GPGGA,%s,%s,%s,1,08,0.9,%.1f,M,0.0,M,,",
158 utc, lat, lon, altitude_ft * SG_FEET_TO_METER );
159 add_with_checksum(nmea, 256);
160 }
161
162 // GSA sentence (totally faked)
163 if (mNmeaMessages & NMEA::GPGSA)
164 {
165 snprintf( nmea, sizeof(nmea), "%s%s",
166 "$GPGSA,A,3,01,02,03,,05,,07,,09,,11,12,0.9,0.9,2.0*38", mLineFeed );
167 SG_LOG( SG_IO, SG_DEBUG, nmea );
168
169 mNmeaSentence += nmea;
170 }
171
172 return true;
173}
174
175// parse NMEA message. messages will look something like the
176// following:
177//
178// $GPRMC,163227,A,3321.173,N,11039.855,W,000.1,270.0,171199,0.000,E*61
179// $GPGGA,163227,3321.173,N,11039.855,W,1,,,3333,F,,,,*0F
180
181void FGNMEA::parse_line() {
182 SG_LOG( SG_IO, SG_DEBUG, "parse nmea message" );
183
184 if (mLength > FG_MAX_MSG_SIZE-1)
185 mLength = FG_MAX_MSG_SIZE-1;
186
187 SG_LOG( SG_IO, SG_DEBUG, "entire message = " << mBuf );
188
189 // test leading character
190 if (mBuf[0] != '$')
191 {
192 SG_LOG( SG_IO, SG_DEBUG, " invalid NMEA start character = " << mBuf[0]);
193 return;
194 }
195
196 // get rid of checksum and "*" delimiter
197 while ((mLength > 0)&&(mBuf[mLength]!='*'))
198 {
199 mLength--;
200 }
201 mBuf[mLength] = 0;
202
203 // split string to tokens
204 std::vector<std::string> tokens;
205 for (unsigned int pos=1;pos < mLength;pos++)
206 {
207 const char* pCurrent = &mBuf[pos];
208 while ((mBuf[pos]!=',')&&(pos<mLength))
209 pos++;
210 if (mBuf[pos]==',')
211 mBuf[pos] = 0;
212 tokens.push_back(pCurrent);
213 }
214
215 if (tokens.empty())
216 return;
217
218 if (tokens.size()>1)
219 {
220 for (unsigned int i=0;i<tokens.size();i++)
221 {
222 SG_LOG( SG_IO, SG_DEBUG, " NMEA token # " << i << ": " << tokens[i]);
223 }
224 parse_message(tokens);
225 }
226}
227
228void FGNMEA::parse_message(const std::vector<std::string>& tokens)
229{
230 double lon_deg, lon_min, lat_deg, lat_min;
231 double lon, lat;
232 // std::string::size_type begin = 0, end;
233
234 if (tokens[0] == "GPRMC" ) {
235 // $GPRMC,HHMMSS,A,DDMM.MMMM,N,DDDMM.MMMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E,A*XX
236 if ( tokens.size()<9)
237 return;
238
239 // #1: time
240 const std::string& utc = tokens[1];
241 SG_LOG( SG_IO, SG_DEBUG, " utc = " << utc );
242
243 // #2: junk
244 SG_LOG( SG_IO, SG_DEBUG, " junk = " << tokens[2] );
245
246 // #3: lat val
247 lat_deg = std::stod(tokens[3].substr(0, 2));
248 lat_min = std::stod(tokens[3].substr(2));
249 lat = lat_deg + ( lat_min / 60.0 );
250
251 // #4: lat dir
252 if ( tokens[4] == "S" )
253 lat *= -1;
254
255 mFdm.set_Latitude( lat * SGD_DEGREES_TO_RADIANS );
256
257 // #5: lon val
258 lon_deg = std::stod(tokens[5].substr(0, 3));
259 lon_min = std::stod(tokens[5].substr(3));
260 lon = lon_deg + ( lon_min / 60.0 );
261
262 // #6: lon dir
263 if ( tokens[6] == "W" )
264 lon *= -1;
265
266 mFdm.set_Longitude( lon * SGD_DEGREES_TO_RADIANS );
267 SG_LOG( SG_IO, SG_DEBUG, " lat = " << lat << ", lon = " << lon );
268
269#if 0
270 double sl_radius, lat_geoc;
271 sgGeodToGeoc( mFdm.get_Latitude(),
272 mFdm.get_Altitude(),
273 &sl_radius, &lat_geoc );
274 mFdm.set_Geocentric_Position( lat_geoc,
275 mFdm.get_Longitude(),
276 sl_radius + mFdm.get_Altitude() );
277#endif
278
279 // #7: speed
280 double speed = std::stod(tokens[7]);
281 mFdm.set_V_calibrated_kts( speed );
282 // mFdm.set_V_ground_speed( speed );
283 SG_LOG( SG_IO, SG_DEBUG, " speed = " << speed );
284
285 // #8: heading
286 double heading = std::stod(tokens[8]);
287 mFdm.set_Euler_Angles( mFdm.get_Phi(),
288 mFdm.get_Theta(),
289 heading * SGD_DEGREES_TO_RADIANS );
290 SG_LOG( SG_IO, SG_DEBUG, " heading = " << heading );
291 } else
292 if (tokens[0] == "GPGGA" ) {
293 if ( tokens.size()<11)
294 return;
295
296 // #1: time
297 const std::string& utc = tokens[1];
298 SG_LOG( SG_IO, SG_DEBUG, " utc = " << utc );
299
300 // #2: lat val
301 lat_deg = std::stod(tokens[2].substr(0, 2));
302 lat_min = std::stod(tokens[2].substr(2));
303 lat = lat_deg + ( lat_min / 60.0 );
304
305 // #3: lat dir
306 if ( tokens[4] == "S" )
307 lat *= -1;
308
309 mFdm.set_Latitude( lat * SGD_DEGREES_TO_RADIANS );
310
311 // #4: lon val
312 lon_deg = std::stod(tokens[4].substr(0, 3));
313 lon_min = std::stod(tokens[4].substr(3));
314 lon = lon_deg + ( lon_min / 60.0 );
315
316 // #5: lon dir
317 if ( tokens[5] == "W" )
318 lon *= -1;
319
320 mFdm.set_Longitude( lon * SGD_DEGREES_TO_RADIANS );
321 SG_LOG( SG_IO, SG_DEBUG, " lat = " << lat << ", lon = " << lon );
322
323 // #6: junk
324 SG_LOG( SG_IO, SG_DEBUG, " junk = " << tokens[6] );
325
326 // #7: junk
327 SG_LOG( SG_IO, SG_DEBUG, " junk = " << tokens[7] );
328
329 // #8: junk
330 SG_LOG( SG_IO, SG_DEBUG, " junk = " << tokens[8] );
331
332 // #9: altitude
333 double altitude = std::stod(tokens[9]);
334
335 // #10: altitude unit
336 const std::string& alt_units = tokens[10];
337
338 if ( alt_units != "F" && alt_units != "f" ) {
339 altitude *= SG_METER_TO_FEET;
340 }
341
342 mFdm.set_Altitude( altitude );
343
344 SG_LOG( SG_IO, SG_DEBUG, " altitude = " << altitude );
345 }
346}
347
348
349// open hailing frequencies
350bool FGNMEA::open() {
351 if ( is_enabled() ) {
352 SG_LOG( SG_IO, SG_ALERT, "This shouldn't happen, but the channel "
353 << "is already in use, ignoring" );
354 return false;
355 }
356
357 // bidirectional support does not make sense for NMEA (and Garmin) protocols
358 if ((get_direction() == SG_IO_BI)&&
359 (!mBiDirectionalSupport))
360 {
361 SG_LOG( SG_IO, SG_ALERT, "NMEA protocol does not support bidirectional communication. "
362 "Use 'in' or 'out' instead of 'bi'.");
363 return false;
364 }
365
366 SGIOChannel *io = get_io_channel();
367
368 if ( ! io->open( get_direction() ) ) {
369 SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." );
370 return false;
371 }
372
373 set_enabled( true );
374
375 return true;
376}
377
378
379// process work for this port
380bool FGNMEA::process() {
381 SGIOChannel *io = get_io_channel();
382
383 if (( get_direction() == SG_IO_OUT )||
384 ( get_direction() == SG_IO_BI))
385 {
386 // process output
387 gen_message();
388 if ((!mNmeaSentence.empty())&&
389 (!io->write( mNmeaSentence.c_str(), mNmeaSentence.length() )))
390 {
391 SG_LOG( SG_IO, SG_WARN, "Error writing data." );
392 }
393 mNmeaSentence = "";
394 }
395
396 if (( get_direction() == SG_IO_IN )||
397 ( get_direction() == SG_IO_BI))
398 {
399 // process input lines (normally expecting 2 messages per cycle)
400 for (unsigned int i=0;i<mMaxReceiveLines;i++)
401 {
402 if ( (mLength = io->readline( mBuf, FG_MAX_MSG_SIZE )) > 0 ) {
403 parse_line();
404 } else {
405 SG_LOG( SG_IO, SG_WARN, "Error reading data." );
406 }
407 }
408 }
409
410 return true; // return value is unused
411}
412
413
414// close the channel
415bool FGNMEA::close() {
416 SGIOChannel *io = get_io_channel();
417
418 set_enabled( false );
419
420 return io->close();
421}
altitude
double altitude
Definition ADA.cxx:46
lat_geoc
double lat_geoc
Definition ADA.cxx:44
min
#define min(X, Y)
Definition FGOutputFG.cpp:60
i
#define i(x)
FGNMEA::mBuf
char mBuf[16384]
Definition nmea.hxx:46
FGNMEA::parse_line
void parse_line()
Definition nmea.cxx:181
FGNMEA::gen_message
virtual bool gen_message()
Definition nmea.cxx:61
FGNMEA::mLength
unsigned int mLength
Definition nmea.hxx:47
FGNMEA::mMaxReceiveLines
unsigned int mMaxReceiveLines
Definition nmea.hxx:49
FGNMEA::mFdm
FlightProperties mFdm
Definition nmea.hxx:51
FGNMEA::add_with_checksum
void add_with_checksum(char *sentence, unsigned int buf_size)
Definition nmea.cxx:43
FGNMEA::mNmeaSentence
std::string mNmeaSentence
Definition nmea.hxx:53
FGNMEA::~FGNMEA
~FGNMEA()
Definition nmea.cxx:38
FGNMEA::mBiDirectionalSupport
bool mBiDirectionalSupport
Definition nmea.hxx:50
FGNMEA::FGNMEA
FGNMEA()
Definition nmea.cxx:27
FGNMEA::open
virtual bool open()
Definition nmea.cxx:350
FGNMEA::mLineFeed
const char * mLineFeed
Definition nmea.hxx:52
FGNMEA::close
virtual bool close()
Definition nmea.cxx:415
FGNMEA::mNmeaMessages
unsigned int mNmeaMessages
Definition nmea.hxx:48
FGNMEA::process
virtual bool process()
Definition nmea.cxx:380
FGProtocol::get_direction
SGProtocolDir get_direction() const
Definition protocol.hxx:65
FGProtocol::get_io_channel
SGIOChannel * get_io_channel() const
Definition protocol.hxx:90
FGProtocol::set_enabled
void set_enabled(const bool b)
Definition protocol.hxx:88
FGProtocol::parse_message
virtual bool parse_message()
Definition protocol.cxx:105
FGProtocol::is_enabled
bool is_enabled() const
Definition protocol.hxx:87
fg_props.hxx
globals
FGGlobals * globals
Definition globals.cxx:142
NMEA
Definition flarm.hxx:29
NMEA::GPGSA
const unsigned int GPGSA
Definition nmea.hxx:38
NMEA::GPGGA
const unsigned int GPGGA
Definition nmea.hxx:37
NMEA::GPRMC
const unsigned int GPRMC
Definition nmea.hxx:36
fgGetDouble
double fgGetDouble(const char *name, double defaultValue)
Get a double value for a property.
Definition proptest.cpp:30
FG_MAX_MSG_SIZE
#define FG_MAX_MSG_SIZE
Definition protocol.hxx:33