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