1 // nmea.cxx -- NMEA protocal class
3 // Written by Curtis Olson, started November 1999.
5 // Copyright (C) 1999 Curtis L. Olson - curt@flightgear.org
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Public License for more details.
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
24 #include <simgear/debug/logstream.hxx>
25 #include <simgear/math/sg_geodesy.hxx>
26 #include <simgear/io/iochannel.hxx>
27 #include <simgear/timing/sg_time.hxx>
29 #include <FDM/flight.hxx>
30 #include <Main/fg_props.hxx>
31 #include <Main/globals.hxx>
35 SG_USING_NAMESPACE(std);
45 // calculate the nmea check sum
46 static char calc_nmea_cksum(char *sentence) {
47 unsigned char sum = 0;
50 // cout << sentence << endl;
52 len = strlen(sentence);
54 for ( i = 1; i < len; i++ ) {
55 // cout << sentence[i];
60 // printf("sum = %02x\n", sum);
65 // generate NMEA message
66 bool FGNMEA::gen_message() {
67 // cout << "generating nmea message" << endl;
69 char rmc[256], gga[256], gsa[256];
70 char rmc_sum[10], gga_sum[10];
75 SGTime *t = globals->get_time_params();
78 sprintf( utc, "%02d%02d%02d",
79 t->getGmt()->tm_hour, t->getGmt()->tm_min, t->getGmt()->tm_sec );
81 char gga_lat[20], rmc_lat[20];
82 double latd = cur_fdm_state->get_Latitude() * SGD_RADIANS_TO_DEGREES;
90 min = (latd - (double)deg) * 60.0;
91 sprintf( gga_lat, "%02d%07.4f,%c", abs(deg), min, dir);
92 sprintf( rmc_lat, "%02d%07.4f,%c", abs(deg), min, dir);
94 char gga_lon[20], rmc_lon[20];
95 double lond = cur_fdm_state->get_Longitude() * SGD_RADIANS_TO_DEGREES;
103 min = (lond - (double)deg) * 60.0;
104 sprintf( gga_lon, "%03d%07.4f,%c", abs(deg), min, dir);
105 sprintf( rmc_lon, "%03d%07.4f,%c", abs(deg), min, dir);
107 double vn = fgGetDouble( "/velocities/speed-north-fps" );
108 double ve = fgGetDouble( "/velocities/speed-east-fps" );
109 double fps = sqrt( vn*vn + ve*ve );
110 double mps = fps * SG_FEET_TO_METER;
111 double kts = mps * SG_METER_TO_NM * 3600;
113 sprintf( speed, "%.1f", kts );
115 double hdg_true = atan2( ve, vn ) * SGD_RADIANS_TO_DEGREES;
116 if ( hdg_true < 0 ) {
120 sprintf( heading, "%.1f", hdg_true );
123 sprintf( altitude_m, "%.1f",
124 cur_fdm_state->get_Altitude() * SG_FEET_TO_METER );
127 int year = t->getGmt()->tm_year;
128 while ( year >= 100 ) { year -= 100; }
129 sprintf( date, "%02d%02d%02d", t->getGmt()->tm_mday,
130 t->getGmt()->tm_mon+1, year );
133 float magdeg = fgGetDouble( "/environment/magnetic-variation-deg" );
134 if ( magdeg < 0.0 ) {
140 sprintf( magvar, "%.1f,%c", magdeg, dir );
142 // $GPRMC,HHMMSS,A,DDMM.MMMM,N,DDDMM.MMMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E,A*XX
143 sprintf( rmc, "GPRMC,%s,A,%s,%s,%s,%s,%s,%s,A",
144 utc, rmc_lat, rmc_lon, speed, heading, date, magvar );
145 sprintf( rmc_sum, "%02X", calc_nmea_cksum(rmc) );
147 // $GPGGA,HHMMSS,DDMM.MMMM,N,DDDMM.MMMM,W,1,NN,H.H,AAAA.A,M,GG.G,M,,*XX
148 sprintf( gga, "GPGGA,%s,%s,%s,1,08,0.9,%s,M,0.0,M,,",
149 utc, gga_lat, gga_lon, altitude_m );
150 sprintf( gga_sum, "%02X", calc_nmea_cksum(gga) );
152 "$GPGSA,A,3,01,02,03,,05,,07,,09,,11,12,0.9,0.9,2.0*38" );
154 SG_LOG( SG_IO, SG_DEBUG, rmc );
155 SG_LOG( SG_IO, SG_DEBUG, gga );
156 SG_LOG( SG_IO, SG_DEBUG, gsa );
158 string nmea_sentence;
162 nmea_sentence += rmc;
163 nmea_sentence += "*";
164 nmea_sentence += rmc_sum;
165 nmea_sentence += "\n";
168 nmea_sentence += "$";
169 nmea_sentence += gga;
170 nmea_sentence += "*";
171 nmea_sentence += gga_sum;
172 nmea_sentence += "\n";
174 // GSA sentence (totally faked)
175 nmea_sentence += gsa;
176 nmea_sentence += "\n";
178 // cout << nmea_sentence;
180 length = nmea_sentence.length();
181 strncpy( buf, nmea_sentence.c_str(), length );
187 // parse NMEA message. messages will look something like the
190 // $GPRMC,163227,A,3321.173,N,11039.855,W,000.1,270.0,171199,0.000,E*61
191 // $GPGGA,163227,3321.173,N,11039.855,W,1,,,3333,F,,,,*0F
193 bool FGNMEA::parse_message() {
194 SG_LOG( SG_IO, SG_INFO, "parse nmea message" );
197 msg = msg.substr( 0, length );
198 SG_LOG( SG_IO, SG_INFO, "entire message = " << msg );
200 string::size_type begin_line, end_line, begin, end;
201 begin_line = begin = 0;
203 // extract out each line
204 end_line = msg.find("\n", begin_line);
205 while ( end_line != string::npos ) {
206 string line = msg.substr(begin_line, end_line - begin_line);
207 begin_line = end_line + 1;
208 SG_LOG( SG_IO, SG_INFO, " input line = " << line );
211 string start = msg.substr(begin, 1);
213 SG_LOG( SG_IO, SG_INFO, " start = " << start );
216 end = msg.find(",", begin);
217 if ( end == string::npos ) {
221 string sentence = msg.substr(begin, end - begin);
223 SG_LOG( SG_IO, SG_INFO, " sentence = " << sentence );
225 double lon_deg, lon_min, lat_deg, lat_min;
226 double lon, lat, speed, heading, altitude;
228 if ( sentence == "GPRMC" ) {
230 end = msg.find(",", begin);
231 if ( end == string::npos ) {
235 string utc = msg.substr(begin, end - begin);
237 SG_LOG( SG_IO, SG_INFO, " utc = " << utc );
240 end = msg.find(",", begin);
241 if ( end == string::npos ) {
245 string junk = msg.substr(begin, end - begin);
247 SG_LOG( SG_IO, SG_INFO, " junk = " << junk );
250 end = msg.find(",", begin);
251 if ( end == string::npos ) {
255 string lat_str = msg.substr(begin, end - begin);
258 lat_deg = atof( lat_str.substr(0, 2).c_str() );
259 lat_min = atof( lat_str.substr(2).c_str() );
262 end = msg.find(",", begin);
263 if ( end == string::npos ) {
267 string lat_dir = msg.substr(begin, end - begin);
270 lat = lat_deg + ( lat_min / 60.0 );
271 if ( lat_dir == "S" ) {
275 cur_fdm_state->set_Latitude( lat * SGD_DEGREES_TO_RADIANS );
276 SG_LOG( SG_IO, SG_INFO, " lat = " << lat );
279 end = msg.find(",", begin);
280 if ( end == string::npos ) {
284 string lon_str = msg.substr(begin, end - begin);
287 lon_deg = atof( lon_str.substr(0, 3).c_str() );
288 lon_min = atof( lon_str.substr(3).c_str() );
291 end = msg.find(",", begin);
292 if ( end == string::npos ) {
296 string lon_dir = msg.substr(begin, end - begin);
299 lon = lon_deg + ( lon_min / 60.0 );
300 if ( lon_dir == "W" ) {
304 cur_fdm_state->set_Longitude( lon * SGD_DEGREES_TO_RADIANS );
305 SG_LOG( SG_IO, SG_INFO, " lon = " << lon );
308 double sl_radius, lat_geoc;
309 sgGeodToGeoc( cur_fdm_state->get_Latitude(),
310 cur_fdm_state->get_Altitude(),
311 &sl_radius, &lat_geoc );
312 cur_fdm_state->set_Geocentric_Position( lat_geoc,
313 cur_fdm_state->get_Longitude(),
314 sl_radius + cur_fdm_state->get_Altitude() );
318 end = msg.find(",", begin);
319 if ( end == string::npos ) {
323 string speed_str = msg.substr(begin, end - begin);
325 speed = atof( speed_str.c_str() );
326 cur_fdm_state->set_V_calibrated_kts( speed );
327 // cur_fdm_state->set_V_ground_speed( speed );
328 SG_LOG( SG_IO, SG_INFO, " speed = " << speed );
331 end = msg.find(",", begin);
332 if ( end == string::npos ) {
336 string hdg_str = msg.substr(begin, end - begin);
338 heading = atof( hdg_str.c_str() );
339 cur_fdm_state->set_Euler_Angles( cur_fdm_state->get_Phi(),
340 cur_fdm_state->get_Theta(),
341 heading * SGD_DEGREES_TO_RADIANS );
342 SG_LOG( SG_IO, SG_INFO, " heading = " << heading );
343 } else if ( sentence == "GPGGA" ) {
345 end = msg.find(",", begin);
346 if ( end == string::npos ) {
350 string utc = msg.substr(begin, end - begin);
352 SG_LOG( SG_IO, SG_INFO, " utc = " << utc );
355 end = msg.find(",", begin);
356 if ( end == string::npos ) {
360 string lat_str = msg.substr(begin, end - begin);
363 lat_deg = atof( lat_str.substr(0, 2).c_str() );
364 lat_min = atof( lat_str.substr(2).c_str() );
367 end = msg.find(",", begin);
368 if ( end == string::npos ) {
372 string lat_dir = msg.substr(begin, end - begin);
375 lat = lat_deg + ( lat_min / 60.0 );
376 if ( lat_dir == "S" ) {
380 // cur_fdm_state->set_Latitude( lat * SGD_DEGREES_TO_RADIANS );
381 SG_LOG( SG_IO, SG_INFO, " lat = " << lat );
384 end = msg.find(",", begin);
385 if ( end == string::npos ) {
389 string lon_str = msg.substr(begin, end - begin);
392 lon_deg = atof( lon_str.substr(0, 3).c_str() );
393 lon_min = atof( lon_str.substr(3).c_str() );
396 end = msg.find(",", begin);
397 if ( end == string::npos ) {
401 string lon_dir = msg.substr(begin, end - begin);
404 lon = lon_deg + ( lon_min / 60.0 );
405 if ( lon_dir == "W" ) {
409 // cur_fdm_state->set_Longitude( lon * SGD_DEGREES_TO_RADIANS );
410 SG_LOG( SG_IO, SG_INFO, " lon = " << lon );
413 end = msg.find(",", begin);
414 if ( end == string::npos ) {
418 string junk = msg.substr(begin, end - begin);
420 SG_LOG( SG_IO, SG_INFO, " junk = " << junk );
423 end = msg.find(",", begin);
424 if ( end == string::npos ) {
428 junk = msg.substr(begin, end - begin);
430 SG_LOG( SG_IO, SG_INFO, " junk = " << junk );
433 end = msg.find(",", begin);
434 if ( end == string::npos ) {
438 junk = msg.substr(begin, end - begin);
440 SG_LOG( SG_IO, SG_INFO, " junk = " << junk );
443 end = msg.find(",", begin);
444 if ( end == string::npos ) {
448 string alt_str = msg.substr(begin, end - begin);
449 altitude = atof( alt_str.c_str() );
453 end = msg.find(",", begin);
454 if ( end == string::npos ) {
458 string alt_units = msg.substr(begin, end - begin);
461 if ( alt_units != "F" ) {
462 altitude *= SG_METER_TO_FEET;
465 cur_fdm_state->set_Altitude( altitude );
467 SG_LOG( SG_IO, SG_INFO, " altitude = " << altitude );
471 // printf("%.8f %.8f\n", lon, lat);
474 end_line = msg.find("\n", begin_line);
481 // open hailing frequencies
482 bool FGNMEA::open() {
483 if ( is_enabled() ) {
484 SG_LOG( SG_IO, SG_ALERT, "This shouldn't happen, but the channel "
485 << "is already in use, ignoring" );
489 SGIOChannel *io = get_io_channel();
491 if ( ! io->open( get_direction() ) ) {
492 SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." );
502 // process work for this port
503 bool FGNMEA::process() {
504 SGIOChannel *io = get_io_channel();
506 if ( get_direction() == SG_IO_OUT ) {
508 if ( ! io->write( buf, length ) ) {
509 SG_LOG( SG_IO, SG_WARN, "Error writing data." );
512 } else if ( get_direction() == SG_IO_IN ) {
513 if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
516 SG_LOG( SG_IO, SG_WARN, "Error reading data." );
519 if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
522 SG_LOG( SG_IO, SG_WARN, "Error reading data." );
532 bool FGNMEA::close() {
533 SGIOChannel *io = get_io_channel();
535 set_enabled( false );
537 if ( ! io->close() ) {