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