src/Network/nmea.cxx

   1 // nmea.cxx -- NMEA protocal class
   2 //
   3 // Written by Curtis Olson, started November 1999.
   4 //
   5 // Copyright (C) 1999  Curtis L. Olson - curt@flightgear.org
   6 //
   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.
  11 //
  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.
  16 //
  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.
  20 //
  21 // $Id$
  22
  23
  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>
  28
  29 #include <FDM/flight.hxx>
  30 #include <Main/globals.hxx>
  31
  32 #include "nmea.hxx"
  33
  34 SG_USING_NAMESPACE(std);
  35
  36
  37 FGNMEA::FGNMEA() {
  38 }
  39
  40 FGNMEA::~FGNMEA() {
  41 }
  42
  43
  44 // calculate the nmea check sum
  45 static char calc_nmea_cksum(char *sentence) {
  46     unsigned char sum = 0;
  47     int i, len;
  48
  49     // cout << sentence << endl;
  50
  51     len = strlen(sentence);
  52     sum = sentence[0];
  53     for ( i = 1; i < len; i++ ) {
  54         // cout << sentence[i];
  55         sum ^= sentence[i];
  56     }
  57     // cout << endl;
  58
  59     // printf("sum = %02x\n", sum);
  60     return sum;
  61 }
  62
  63
  64 // generate NMEA message
  65 bool FGNMEA::gen_message() {
  66     // cout << "generating nmea message" << endl;
  67
  68     char rmc[256], gga[256];
  69     char rmc_sum[10], gga_sum[10];
  70     char dir;
  71     int deg;
  72     double min;
  73
  74     SGTime *t = globals->get_time_params();
  75
  76     char utc[10];
  77     sprintf( utc, "%02d%02d%02d",
  78              t->getGmt()->tm_hour, t->getGmt()->tm_min, t->getGmt()->tm_sec );
  79
  80     char lat[20];
  81     double latd = cur_fdm_state->get_Latitude() * SGD_RADIANS_TO_DEGREES;
  82     if ( latd < 0.0 ) {
  83         latd *= -1.0;
  84         dir = 'S';
  85     } else {
  86         dir = 'N';
  87     }
  88     deg = (int)(latd);
  89     min = (latd - (double)deg) * 60.0;
  90     sprintf( lat, "%02d%06.3f,%c", abs(deg), min, dir);
  91
  92     char lon[20];
  93     double lond = cur_fdm_state->get_Longitude() * SGD_RADIANS_TO_DEGREES;
  94     if ( lond < 0.0 ) {
  95         lond *= -1.0;
  96         dir = 'W';
  97     } else {
  98         dir = 'E';
  99     }
 100     deg = (int)(lond);
 101     min = (lond - (double)deg) * 60.0;
 102     sprintf( lon, "%03d%06.3f,%c", abs(deg), min, dir);
 103
 104     char speed[10];
 105     sprintf( speed, "%05.1f", cur_fdm_state->get_V_equiv_kts() );
 106
 107     char heading[10];
 108     sprintf( heading, "%05.1f", cur_fdm_state->get_Psi() * SGD_RADIANS_TO_DEGREES );
 109
 110     char altitude_m[10];
 111     sprintf( altitude_m, "%02d",
 112              (int)(cur_fdm_state->get_Altitude() * SG_FEET_TO_METER) );
 113
 114     char altitude_ft[10];
 115     sprintf( altitude_ft, "%02d", (int)cur_fdm_state->get_Altitude() );
 116
 117     char date[10];
 118     sprintf( date, "%02d%02d%02d", t->getGmt()->tm_mday,
 119              t->getGmt()->tm_mon+1, t->getGmt()->tm_year );
 120
 121     // $GPRMC,HHMMSS,A,DDMM.MMM,N,DDDMM.MMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E*XX
 122     sprintf( rmc, "GPRMC,%s,A,%s,%s,%s,%s,%s,0.000,E",
 123              utc, lat, lon, speed, heading, date );
 124     sprintf( rmc_sum, "%02X", calc_nmea_cksum(rmc) );
 125
 126     sprintf( gga, "GPGGA,%s,%s,%s,1,,,%s,F,,,,",
 127              utc, lat, lon, altitude_ft );
 128     sprintf( gga_sum, "%02X", calc_nmea_cksum(gga) );
 129
 130
 131     SG_LOG( SG_IO, SG_DEBUG, rmc );
 132     SG_LOG( SG_IO, SG_DEBUG, gga );
 133
 134     string nmea_sentence;
 135
 136     // RMC sentence
 137     nmea_sentence = "$";
 138     nmea_sentence += rmc;
 139     nmea_sentence += "*";
 140     nmea_sentence += rmc_sum;
 141     nmea_sentence += "\n";
 142
 143     // GGA sentence
 144     nmea_sentence += "$";
 145     nmea_sentence += gga;
 146     nmea_sentence += "*";
 147     nmea_sentence += gga_sum;
 148     nmea_sentence += "\n";
 149
 150     cout << nmea_sentence;
 151
 152     length = nmea_sentence.length();
 153     strncpy( buf, nmea_sentence.c_str(), length );
 154
 155     return true;
 156 }
 157
 158
 159 // parse NMEA message.  messages will look something like the
 160 // following:
 161 //
 162 // $GPRMC,163227,A,3321.173,N,11039.855,W,000.1,270.0,171199,0.000,E*61
 163 // $GPGGA,163227,3321.173,N,11039.855,W,1,,,3333,F,,,,*0F
 164
 165 bool FGNMEA::parse_message() {
 166     SG_LOG( SG_IO, SG_INFO, "parse nmea message" );
 167
 168     string msg = buf;
 169     msg = msg.substr( 0, length );
 170     SG_LOG( SG_IO, SG_INFO, "entire message = " << msg );
 171
 172     string::size_type begin_line, end_line, begin, end;
 173     begin_line = begin = 0;
 174
 175     // extract out each line
 176     end_line = msg.find("\n", begin_line);
 177     while ( end_line != string::npos ) {
 178         string line = msg.substr(begin_line, end_line - begin_line);
 179         begin_line = end_line + 1;
 180         SG_LOG( SG_IO, SG_INFO, "  input line = " << line );
 181
 182         // leading character
 183         string start = msg.substr(begin, 1);
 184         ++begin;
 185         SG_LOG( SG_IO, SG_INFO, "  start = " << start );
 186
 187         // sentence
 188         end = msg.find(",", begin);
 189         if ( end == string::npos ) {
 190             return false;
 191         }
 192
 193         string sentence = msg.substr(begin, end - begin);
 194         begin = end + 1;
 195         SG_LOG( SG_IO, SG_INFO, "  sentence = " << sentence );
 196
 197         double lon_deg, lon_min, lat_deg, lat_min;
 198         double lon, lat, speed, heading, altitude;
 199
 200         if ( sentence == "GPRMC" ) {
 201             // time
 202             end = msg.find(",", begin);
 203             if ( end == string::npos ) {
 204                 return false;
 205             }
 206
 207             string utc = msg.substr(begin, end - begin);
 208             begin = end + 1;
 209             SG_LOG( SG_IO, SG_INFO, "  utc = " << utc );
 210
 211             // junk
 212             end = msg.find(",", begin);
 213             if ( end == string::npos ) {
 214                 return false;
 215             }
 216
 217             string junk = msg.substr(begin, end - begin);
 218             begin = end + 1;
 219             SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
 220
 221             // lat val
 222             end = msg.find(",", begin);
 223             if ( end == string::npos ) {
 224                 return false;
 225             }
 226
 227             string lat_str = msg.substr(begin, end - begin);
 228             begin = end + 1;
 229
 230             lat_deg = atof( lat_str.substr(0, 2).c_str() );
 231             lat_min = atof( lat_str.substr(2).c_str() );
 232
 233             // lat dir
 234             end = msg.find(",", begin);
 235             if ( end == string::npos ) {
 236                 return false;
 237             }
 238
 239             string lat_dir = msg.substr(begin, end - begin);
 240             begin = end + 1;
 241
 242             lat = lat_deg + ( lat_min / 60.0 );
 243             if ( lat_dir == "S" ) {
 244                 lat *= -1;
 245             }
 246
 247             cur_fdm_state->set_Latitude( lat * SGD_DEGREES_TO_RADIANS );
 248             SG_LOG( SG_IO, SG_INFO, "  lat = " << lat );
 249
 250             // lon val
 251             end = msg.find(",", begin);
 252             if ( end == string::npos ) {
 253                 return false;
 254             }
 255
 256             string lon_str = msg.substr(begin, end - begin);
 257             begin = end + 1;
 258
 259             lon_deg = atof( lon_str.substr(0, 3).c_str() );
 260             lon_min = atof( lon_str.substr(3).c_str() );
 261
 262             // lon dir
 263             end = msg.find(",", begin);
 264             if ( end == string::npos ) {
 265                 return false;
 266             }
 267
 268             string lon_dir = msg.substr(begin, end - begin);
 269             begin = end + 1;
 270
 271             lon = lon_deg + ( lon_min / 60.0 );
 272             if ( lon_dir == "W" ) {
 273                 lon *= -1;
 274             }
 275
 276             cur_fdm_state->set_Longitude( lon * SGD_DEGREES_TO_RADIANS );
 277             SG_LOG( SG_IO, SG_INFO, "  lon = " << lon );
 278
 279 #if 0
 280             double sl_radius, lat_geoc;
 281             sgGeodToGeoc( cur_fdm_state->get_Latitude(),
 282                           cur_fdm_state->get_Altitude(),
 283                           &sl_radius, &lat_geoc );
 284             cur_fdm_state->set_Geocentric_Position( lat_geoc,
 285                            cur_fdm_state->get_Longitude(),
 286                            sl_radius + cur_fdm_state->get_Altitude() );
 287 #endif
 288
 289             // speed
 290             end = msg.find(",", begin);
 291             if ( end == string::npos ) {
 292                 return false;
 293             }
 294
 295             string speed_str = msg.substr(begin, end - begin);
 296             begin = end + 1;
 297             speed = atof( speed_str.c_str() );
 298             cur_fdm_state->set_V_calibrated_kts( speed );
 299             // cur_fdm_state->set_V_ground_speed( speed );
 300             SG_LOG( SG_IO, SG_INFO, "  speed = " << speed );
 301
 302             // heading
 303             end = msg.find(",", begin);
 304             if ( end == string::npos ) {
 305                 return false;
 306             }
 307
 308             string hdg_str = msg.substr(begin, end - begin);
 309             begin = end + 1;
 310             heading = atof( hdg_str.c_str() );
 311             cur_fdm_state->set_Euler_Angles( cur_fdm_state->get_Phi(),
 312                                              cur_fdm_state->get_Theta(),
 313                                              heading * SGD_DEGREES_TO_RADIANS );
 314             SG_LOG( SG_IO, SG_INFO, "  heading = " << heading );
 315         } else if ( sentence == "GPGGA" ) {
 316             // time
 317             end = msg.find(",", begin);
 318             if ( end == string::npos ) {
 319                 return false;
 320             }
 321
 322             string utc = msg.substr(begin, end - begin);
 323             begin = end + 1;
 324             SG_LOG( SG_IO, SG_INFO, "  utc = " << utc );
 325
 326             // lat val
 327             end = msg.find(",", begin);
 328             if ( end == string::npos ) {
 329                 return false;
 330             }
 331
 332             string lat_str = msg.substr(begin, end - begin);
 333             begin = end + 1;
 334
 335             lat_deg = atof( lat_str.substr(0, 2).c_str() );
 336             lat_min = atof( lat_str.substr(2).c_str() );
 337
 338             // lat dir
 339             end = msg.find(",", begin);
 340             if ( end == string::npos ) {
 341                 return false;
 342             }
 343
 344             string lat_dir = msg.substr(begin, end - begin);
 345             begin = end + 1;
 346
 347             lat = lat_deg + ( lat_min / 60.0 );
 348             if ( lat_dir == "S" ) {
 349                 lat *= -1;
 350             }
 351
 352             // cur_fdm_state->set_Latitude( lat * SGD_DEGREES_TO_RADIANS );
 353             SG_LOG( SG_IO, SG_INFO, "  lat = " << lat );
 354
 355             // lon val
 356             end = msg.find(",", begin);
 357             if ( end == string::npos ) {
 358                 return false;
 359             }
 360
 361             string lon_str = msg.substr(begin, end - begin);
 362             begin = end + 1;
 363
 364             lon_deg = atof( lon_str.substr(0, 3).c_str() );
 365             lon_min = atof( lon_str.substr(3).c_str() );
 366
 367             // lon dir
 368             end = msg.find(",", begin);
 369             if ( end == string::npos ) {
 370                 return false;
 371             }
 372
 373             string lon_dir = msg.substr(begin, end - begin);
 374             begin = end + 1;
 375
 376             lon = lon_deg + ( lon_min / 60.0 );
 377             if ( lon_dir == "W" ) {
 378                 lon *= -1;
 379             }
 380
 381             // cur_fdm_state->set_Longitude( lon * SGD_DEGREES_TO_RADIANS );
 382             SG_LOG( SG_IO, SG_INFO, "  lon = " << lon );
 383
 384             // junk
 385             end = msg.find(",", begin);
 386             if ( end == string::npos ) {
 387                 return false;
 388             }
 389
 390             string junk = msg.substr(begin, end - begin);
 391             begin = end + 1;
 392             SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
 393
 394             // junk
 395             end = msg.find(",", begin);
 396             if ( end == string::npos ) {
 397                 return false;
 398             }
 399
 400             junk = msg.substr(begin, end - begin);
 401             begin = end + 1;
 402             SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
 403
 404             // junk
 405             end = msg.find(",", begin);
 406             if ( end == string::npos ) {
 407                 return false;
 408             }
 409
 410             junk = msg.substr(begin, end - begin);
 411             begin = end + 1;
 412             SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
 413
 414             // altitude
 415             end = msg.find(",", begin);
 416             if ( end == string::npos ) {
 417                 return false;
 418             }
 419
 420             string alt_str = msg.substr(begin, end - begin);
 421             altitude = atof( alt_str.c_str() );
 422             begin = end + 1;
 423
 424             // altitude units
 425             end = msg.find(",", begin);
 426             if ( end == string::npos ) {
 427                 return false;
 428             }
 429
 430             string alt_units = msg.substr(begin, end - begin);
 431             begin = end + 1;
 432
 433             if ( alt_units != (string)"F" ) {
 434                 altitude *= SG_METER_TO_FEET;
 435             }
 436
 437             cur_fdm_state->set_Altitude( altitude );
 438
 439             SG_LOG( SG_IO, SG_INFO, " altitude  = " << altitude );
 440
 441         }
 442
 443         // printf("%.8f %.8f\n", lon, lat);
 444
 445         begin = begin_line;
 446         end_line = msg.find("\n", begin_line);
 447     }
 448
 449     return true;
 450 }
 451
 452
 453 // open hailing frequencies
 454 bool FGNMEA::open() {
 455     if ( is_enabled() ) {
 456         SG_LOG( SG_IO, SG_ALERT, "This shouldn't happen, but the channel "
 457                 << "is already in use, ignoring" );
 458         return false;
 459     }
 460
 461     SGIOChannel *io = get_io_channel();
 462
 463     if ( ! io->open( get_direction() ) ) {
 464         SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." );
 465         return false;
 466     }
 467
 468     set_enabled( true );
 469
 470     return true;
 471 }
 472
 473
 474 // process work for this port
 475 bool FGNMEA::process() {
 476     SGIOChannel *io = get_io_channel();
 477
 478     if ( get_direction() == SG_IO_OUT ) {
 479         gen_message();
 480         if ( ! io->write( buf, length ) ) {
 481             SG_LOG( SG_IO, SG_ALERT, "Error writing data." );
 482             return false;
 483         }
 484     } else if ( get_direction() == SG_IO_IN ) {
 485         if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
 486             parse_message();
 487         } else {
 488             SG_LOG( SG_IO, SG_ALERT, "Error reading data." );
 489             return false;
 490         }
 491         if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
 492             parse_message();
 493         } else {
 494             SG_LOG( SG_IO, SG_ALERT, "Error reading data." );
 495             return false;
 496         }
 497     }
 498
 499     return true;
 500 }
 501
 502
 503 // close the channel
 504 bool FGNMEA::close() {
 505     SGIOChannel *io = get_io_channel();
 506
 507     set_enabled( false );
 508
 509     if ( ! io->close() ) {
 510         return false;
 511     }
 512
 513     return true;
 514 }