src/Network/atlas.cxx

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