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 - http://www.flightgear.org/~curt
   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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  20 //
  21 // $Id$
  22
  23 #ifdef HAVE_CONFIG_H
  24 #  include "config.h"
  25 #endif
  26
  27 #include <simgear/debug/logstream.hxx>
  28 #include <simgear/math/sg_geodesy.hxx>
  29 #include <simgear/io/iochannel.hxx>
  30 #include <simgear/timing/sg_time.hxx>
  31
  32 #include <FDM/flight.hxx>
  33 #include <Main/fg_props.hxx>
  34 #include <Main/globals.hxx>
  35
  36 #include "nmea.hxx"
  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%07.4f,%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%07.4f,%c", abs(deg), min, dir);
 105     sprintf( rmc_lon, "%03d%07.4f,%c", abs(deg), min, dir);
 106
 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;
 112     char speed[10];
 113     sprintf( speed, "%.1f", kts );
 114
 115     double hdg_true = atan2( ve, vn ) * SGD_RADIANS_TO_DEGREES;
 116     if ( hdg_true < 0 ) {
 117       hdg_true += 360.0;
 118     }
 119     char heading[10];
 120     sprintf( heading, "%.1f", hdg_true );
 121
 122     char altitude_m[10];
 123     sprintf( altitude_m, "%.1f",
 124              cur_fdm_state->get_Altitude() * SG_FEET_TO_METER );
 125
 126     char date[10];
 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 );
 131
 132     char magvar[10];
 133     float magdeg = fgGetDouble( "/environment/magnetic-variation-deg" );
 134     if ( magdeg < 0.0 ) {
 135         magdeg = -magdeg;
 136         dir = 'W';
 137     } else {
 138         dir = 'E';
 139     }
 140     sprintf( magvar, "%.1f,%c", magdeg, dir );
 141
 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) );
 146
 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) );
 151     sprintf( gsa, "%s",
 152              "$GPGSA,A,3,01,02,03,,05,,07,,09,,11,12,0.9,0.9,2.0*38" );
 153
 154     SG_LOG( SG_IO, SG_DEBUG, rmc );
 155     SG_LOG( SG_IO, SG_DEBUG, gga );
 156     SG_LOG( SG_IO, SG_DEBUG, gsa );
 157
 158     string nmea_sentence;
 159
 160     // RMC sentence
 161     nmea_sentence = "$";
 162     nmea_sentence += rmc;
 163     nmea_sentence += "*";
 164     nmea_sentence += rmc_sum;
 165     nmea_sentence += "\n";
 166
 167     // GGA sentence
 168     nmea_sentence += "$";
 169     nmea_sentence += gga;
 170     nmea_sentence += "*";
 171     nmea_sentence += gga_sum;
 172     nmea_sentence += "\n";
 173
 174     // GSA sentence (totally faked)
 175     nmea_sentence += gsa;
 176     nmea_sentence += "\n";
 177
 178     // cout << nmea_sentence;
 179
 180     length = nmea_sentence.length();
 181     strncpy( buf, nmea_sentence.c_str(), length );
 182
 183     return true;
 184 }
 185
 186
 187 // parse NMEA message.  messages will look something like the
 188 // following:
 189 //
 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
 192
 193 bool FGNMEA::parse_message() {
 194     SG_LOG( SG_IO, SG_INFO, "parse nmea message" );
 195
 196     string msg = buf;
 197     msg = msg.substr( 0, length );
 198     SG_LOG( SG_IO, SG_INFO, "entire message = " << msg );
 199
 200     string::size_type begin_line, end_line, begin, end;
 201     begin_line = begin = 0;
 202
 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 );
 209
 210         // leading character
 211         string start = msg.substr(begin, 1);
 212         ++begin;
 213         SG_LOG( SG_IO, SG_INFO, "  start = " << start );
 214
 215         // sentence
 216         end = msg.find(",", begin);
 217         if ( end == string::npos ) {
 218             return false;
 219         }
 220
 221         string sentence = msg.substr(begin, end - begin);
 222         begin = end + 1;
 223         SG_LOG( SG_IO, SG_INFO, "  sentence = " << sentence );
 224
 225         double lon_deg, lon_min, lat_deg, lat_min;
 226         double lon, lat, speed, heading, altitude;
 227
 228         if ( sentence == "GPRMC" ) {
 229             // time
 230             end = msg.find(",", begin);
 231             if ( end == string::npos ) {
 232                 return false;
 233             }
 234
 235             string utc = msg.substr(begin, end - begin);
 236             begin = end + 1;
 237             SG_LOG( SG_IO, SG_INFO, "  utc = " << utc );
 238
 239             // junk
 240             end = msg.find(",", begin);
 241             if ( end == string::npos ) {
 242                 return false;
 243             }
 244
 245             string junk = msg.substr(begin, end - begin);
 246             begin = end + 1;
 247             SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
 248
 249             // lat val
 250             end = msg.find(",", begin);
 251             if ( end == string::npos ) {
 252                 return false;
 253             }
 254
 255             string lat_str = msg.substr(begin, end - begin);
 256             begin = end + 1;
 257
 258             lat_deg = atof( lat_str.substr(0, 2).c_str() );
 259             lat_min = atof( lat_str.substr(2).c_str() );
 260
 261             // lat dir
 262             end = msg.find(",", begin);
 263             if ( end == string::npos ) {
 264                 return false;
 265             }
 266
 267             string lat_dir = msg.substr(begin, end - begin);
 268             begin = end + 1;
 269
 270             lat = lat_deg + ( lat_min / 60.0 );
 271             if ( lat_dir == "S" ) {
 272                 lat *= -1;
 273             }
 274
 275             cur_fdm_state->set_Latitude( lat * SGD_DEGREES_TO_RADIANS );
 276             SG_LOG( SG_IO, SG_INFO, "  lat = " << lat );
 277
 278             // lon val
 279             end = msg.find(",", begin);
 280             if ( end == string::npos ) {
 281                 return false;
 282             }
 283
 284             string lon_str = msg.substr(begin, end - begin);
 285             begin = end + 1;
 286
 287             lon_deg = atof( lon_str.substr(0, 3).c_str() );
 288             lon_min = atof( lon_str.substr(3).c_str() );
 289
 290             // lon dir
 291             end = msg.find(",", begin);
 292             if ( end == string::npos ) {
 293                 return false;
 294             }
 295
 296             string lon_dir = msg.substr(begin, end - begin);
 297             begin = end + 1;
 298
 299             lon = lon_deg + ( lon_min / 60.0 );
 300             if ( lon_dir == "W" ) {
 301                 lon *= -1;
 302             }
 303
 304             cur_fdm_state->set_Longitude( lon * SGD_DEGREES_TO_RADIANS );
 305             SG_LOG( SG_IO, SG_INFO, "  lon = " << lon );
 306
 307 #if 0
 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() );
 315 #endif
 316
 317             // speed
 318             end = msg.find(",", begin);
 319             if ( end == string::npos ) {
 320                 return false;
 321             }
 322
 323             string speed_str = msg.substr(begin, end - begin);
 324             begin = end + 1;
 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 );
 329
 330             // heading
 331             end = msg.find(",", begin);
 332             if ( end == string::npos ) {
 333                 return false;
 334             }
 335
 336             string hdg_str = msg.substr(begin, end - begin);
 337             begin = end + 1;
 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" ) {
 344             // time
 345             end = msg.find(",", begin);
 346             if ( end == string::npos ) {
 347                 return false;
 348             }
 349
 350             string utc = msg.substr(begin, end - begin);
 351             begin = end + 1;
 352             SG_LOG( SG_IO, SG_INFO, "  utc = " << utc );
 353
 354             // lat val
 355             end = msg.find(",", begin);
 356             if ( end == string::npos ) {
 357                 return false;
 358             }
 359
 360             string lat_str = msg.substr(begin, end - begin);
 361             begin = end + 1;
 362
 363             lat_deg = atof( lat_str.substr(0, 2).c_str() );
 364             lat_min = atof( lat_str.substr(2).c_str() );
 365
 366             // lat dir
 367             end = msg.find(",", begin);
 368             if ( end == string::npos ) {
 369                 return false;
 370             }
 371
 372             string lat_dir = msg.substr(begin, end - begin);
 373             begin = end + 1;
 374
 375             lat = lat_deg + ( lat_min / 60.0 );
 376             if ( lat_dir == "S" ) {
 377                 lat *= -1;
 378             }
 379
 380             // cur_fdm_state->set_Latitude( lat * SGD_DEGREES_TO_RADIANS );
 381             SG_LOG( SG_IO, SG_INFO, "  lat = " << lat );
 382
 383             // lon val
 384             end = msg.find(",", begin);
 385             if ( end == string::npos ) {
 386                 return false;
 387             }
 388
 389             string lon_str = msg.substr(begin, end - begin);
 390             begin = end + 1;
 391
 392             lon_deg = atof( lon_str.substr(0, 3).c_str() );
 393             lon_min = atof( lon_str.substr(3).c_str() );
 394
 395             // lon dir
 396             end = msg.find(",", begin);
 397             if ( end == string::npos ) {
 398                 return false;
 399             }
 400
 401             string lon_dir = msg.substr(begin, end - begin);
 402             begin = end + 1;
 403
 404             lon = lon_deg + ( lon_min / 60.0 );
 405             if ( lon_dir == "W" ) {
 406                 lon *= -1;
 407             }
 408
 409             // cur_fdm_state->set_Longitude( lon * SGD_DEGREES_TO_RADIANS );
 410             SG_LOG( SG_IO, SG_INFO, "  lon = " << lon );
 411
 412             // junk
 413             end = msg.find(",", begin);
 414             if ( end == string::npos ) {
 415                 return false;
 416             }
 417
 418             string 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             // junk
 433             end = msg.find(",", begin);
 434             if ( end == string::npos ) {
 435                 return false;
 436             }
 437
 438             junk = msg.substr(begin, end - begin);
 439             begin = end + 1;
 440             SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
 441
 442             // altitude
 443             end = msg.find(",", begin);
 444             if ( end == string::npos ) {
 445                 return false;
 446             }
 447
 448             string alt_str = msg.substr(begin, end - begin);
 449             altitude = atof( alt_str.c_str() );
 450             begin = end + 1;
 451
 452             // altitude units
 453             end = msg.find(",", begin);
 454             if ( end == string::npos ) {
 455                 return false;
 456             }
 457
 458             string alt_units = msg.substr(begin, end - begin);
 459             begin = end + 1;
 460
 461             if ( alt_units != "F" ) {
 462                 altitude *= SG_METER_TO_FEET;
 463             }
 464
 465             cur_fdm_state->set_Altitude( altitude );
 466
 467             SG_LOG( SG_IO, SG_INFO, " altitude  = " << altitude );
 468
 469         }
 470
 471         // printf("%.8f %.8f\n", lon, lat);
 472
 473         begin = begin_line;
 474         end_line = msg.find("\n", begin_line);
 475     }
 476
 477     return true;
 478 }
 479
 480
 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" );
 486         return false;
 487     }
 488
 489     SGIOChannel *io = get_io_channel();
 490
 491     if ( ! io->open( get_direction() ) ) {
 492         SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." );
 493         return false;
 494     }
 495
 496     set_enabled( true );
 497
 498     return true;
 499 }
 500
 501
 502 // process work for this port
 503 bool FGNMEA::process() {
 504     SGIOChannel *io = get_io_channel();
 505
 506     if ( get_direction() == SG_IO_OUT ) {
 507         gen_message();
 508         if ( ! io->write( buf, length ) ) {
 509             SG_LOG( SG_IO, SG_WARN, "Error writing data." );
 510             return false;
 511         }
 512     } else if ( get_direction() == SG_IO_IN ) {
 513         if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
 514             parse_message();
 515         } else {
 516             SG_LOG( SG_IO, SG_WARN, "Error reading data." );
 517             return false;
 518         }
 519         if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
 520             parse_message();
 521         } else {
 522             SG_LOG( SG_IO, SG_WARN, "Error reading data." );
 523             return false;
 524         }
 525     }
 526
 527     return true;
 528 }
 529
 530
 531 // close the channel
 532 bool FGNMEA::close() {
 533     SGIOChannel *io = get_io_channel();
 534
 535     set_enabled( false );
 536
 537     if ( ! io->close() ) {
 538         return false;
 539     }
 540
 541     return true;
 542 }