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 <cstdlib>
  28 #include <cstring>
  29
  30 #include <simgear/debug/logstream.hxx>
  31 #include <simgear/math/sg_geodesy.hxx>
  32 #include <simgear/io/iochannel.hxx>
  33 #include <simgear/timing/sg_time.hxx>
  34
  35 #include <FDM/flightProperties.hxx>
  36 #include <Main/globals.hxx>
  37 #include <Main/fg_props.hxx>
  38 #include <Main/fg_init.hxx>
  39
  40 #include "atlas.hxx"
  41
  42
  43 FGAtlas::FGAtlas() :
  44   length(0),
  45   fdm(new FlightProperties)
  46 {
  47   _adf_freq        = fgGetNode("/instrumentation/adf/frequencies/selected-khz", true);
  48   _nav1_freq       = fgGetNode("/instrumentation/nav/frequencies/selected-mhz", true);
  49   _nav1_sel_radial = fgGetNode("/instrumentation/nav/radials/selected-deg", true);
  50   _nav2_freq       = fgGetNode("/instrumentation/nav[1]/frequencies/selected-mhz", true);
  51   _nav2_sel_radial = fgGetNode("/instrumentation/nav[1]/radials/selected-deg", true);
  52 }
  53
  54 FGAtlas::~FGAtlas() {
  55   delete fdm;
  56 }
  57
  58
  59 // calculate the atlas check sum
  60 static char calc_atlas_cksum(char *sentence) {
  61     unsigned char sum = 0;
  62     int i, len;
  63
  64     // cout << sentence << endl;
  65
  66     len = strlen(sentence);
  67     sum = sentence[0];
  68     for ( i = 1; i < len; i++ ) {
  69         // cout << sentence[i];
  70         sum ^= sentence[i];
  71     }
  72     // cout << endl;
  73
  74     // printf("sum = %02x\n", sum);
  75     return sum;
  76 }
  77
  78 // generate Atlas message
  79 bool FGAtlas::gen_message() {
  80     // cout << "generating atlas message" << endl;
  81     char rmc[256], gga[256], patla[256];
  82     char rmc_sum[10], gga_sum[10], patla_sum[10];
  83     char dir;
  84     int deg;
  85     double min;
  86
  87     SGTime *t = globals->get_time_params();
  88
  89     char utc[10];
  90     sprintf( utc, "%02d%02d%02d",
  91              t->getGmt()->tm_hour, t->getGmt()->tm_min, t->getGmt()->tm_sec );
  92
  93     char lat[20];
  94     double latd = fdm->get_Latitude() * SGD_RADIANS_TO_DEGREES;
  95     if ( latd < 0.0 ) {
  96         latd *= -1.0;
  97         dir = 'S';
  98     } else {
  99         dir = 'N';
 100     }
 101     deg = (int)(latd);
 102     min = (latd - (double)deg) * 60.0;
 103     sprintf( lat, "%02d%06.3f,%c", abs(deg), min, dir);
 104
 105     char lon[20];
 106     double lond = fdm->get_Longitude() * SGD_RADIANS_TO_DEGREES;
 107     if ( lond < 0.0 ) {
 108         lond *= -1.0;
 109         dir = 'W';
 110     } else {
 111         dir = 'E';
 112     }
 113     deg = (int)(lond);
 114     min = (lond - (double)deg) * 60.0;
 115     sprintf( lon, "%03d%06.3f,%c", abs(deg), min, dir);
 116
 117     char speed[10];
 118     sprintf( speed, "%05.1f", fdm->get_V_equiv_kts() );
 119
 120     char heading[10];
 121     sprintf( heading, "%05.1f", fdm->get_Psi() * SGD_RADIANS_TO_DEGREES );
 122
 123     char altitude_m[10];
 124     sprintf( altitude_m, "%02d",
 125              (int)(fdm->get_Altitude() * SG_FEET_TO_METER) );
 126
 127     char altitude_ft[10];
 128     sprintf( altitude_ft, "%02d", (int)fdm->get_Altitude() );
 129
 130     char date[10];
 131     sprintf( date, "%02d%02d%02d", t->getGmt()->tm_mday,
 132              t->getGmt()->tm_mon+1, t->getGmt()->tm_year );
 133
 134     // $GPRMC,HHMMSS,A,DDMM.MMM,N,DDDMM.MMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E*XX
 135     sprintf( rmc, "GPRMC,%s,A,%s,%s,%s,%s,%s,0.000,E",
 136              utc, lat, lon, speed, heading, date );
 137     sprintf( rmc_sum, "%02X", calc_atlas_cksum(rmc) );
 138
 139     sprintf( gga, "GPGGA,%s,%s,%s,1,,,%s,F,,,,",
 140              utc, lat, lon, altitude_ft );
 141     sprintf( gga_sum, "%02X", calc_atlas_cksum(gga) );
 142
 143     sprintf( patla, "PATLA,%.2f,%.1f,%.2f,%.1f,%.0f",
 144              _nav1_freq->getDoubleValue(),
 145              _nav1_sel_radial->getDoubleValue(),
 146              _nav2_freq->getDoubleValue(),
 147              _nav2_sel_radial->getDoubleValue(),
 148              _adf_freq->getDoubleValue() );
 149     sprintf( patla_sum, "%02X", calc_atlas_cksum(patla) );
 150
 151     SG_LOG( SG_IO, SG_DEBUG, rmc );
 152     SG_LOG( SG_IO, SG_DEBUG, gga );
 153     SG_LOG( SG_IO, SG_DEBUG, patla );
 154
 155     string atlas_sentence;
 156
 157     // RMC sentence
 158     atlas_sentence = "$";
 159     atlas_sentence += rmc;
 160     atlas_sentence += "*";
 161     atlas_sentence += rmc_sum;
 162     atlas_sentence += "\n";
 163
 164     // GGA sentence
 165     atlas_sentence += "$";
 166     atlas_sentence += gga;
 167     atlas_sentence += "*";
 168     atlas_sentence += gga_sum;
 169     atlas_sentence += "\n";
 170
 171     // PATLA sentence
 172     atlas_sentence += "$";
 173     atlas_sentence += patla;
 174     atlas_sentence += "*";
 175     atlas_sentence += patla_sum;
 176     atlas_sentence += "\n";
 177
 178     //    cout << atlas_sentence;
 179
 180     length = atlas_sentence.length();
 181     strncpy( buf, atlas_sentence.c_str(), length );
 182
 183     return true;
 184 }
 185
 186
 187 // parse Atlas 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 FGAtlas::parse_message() {
 194     SG_LOG( SG_IO, SG_INFO, "parse atlas 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             fdm->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             fdm->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( fdm->get_Latitude(),
 310                           fdm->get_Altitude(),
 311                           &sl_radius, &lat_geoc );
 312             fdm->set_Geocentric_Position( lat_geoc,
 313                            fdm->get_Longitude(),
 314                            sl_radius + fdm->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             fdm->set_V_calibrated_kts( speed );
 327             // fdm->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             fdm->set_Euler_Angles( fdm->get_Phi(),
 340                                              fdm->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             // fdm->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             // fdm->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             fdm->set_Altitude( altitude );
 466
 467             SG_LOG( SG_IO, SG_INFO, " altitude  = " << altitude );
 468
 469         } else if ( sentence == "PATLA" ) {
 470             // nav1 freq
 471             end = msg.find(",", begin);
 472             if ( end == string::npos ) {
 473                 return false;
 474             }
 475
 476             string nav1_freq = msg.substr(begin, end - begin);
 477             begin = end + 1;
 478             SG_LOG( SG_IO, SG_INFO, "  nav1_freq = " << nav1_freq );
 479
 480             // nav1 selected radial
 481             end = msg.find(",", begin);
 482             if ( end == string::npos ) {
 483                 return false;
 484             }
 485
 486             string nav1_rad = msg.substr(begin, end - begin);
 487             begin = end + 1;
 488             SG_LOG( SG_IO, SG_INFO, "  nav1_rad = " << nav1_rad );
 489
 490             // nav2 freq
 491             end = msg.find(",", begin);
 492             if ( end == string::npos ) {
 493                 return false;
 494             }
 495
 496             string nav2_freq = msg.substr(begin, end - begin);
 497             begin = end + 1;
 498             SG_LOG( SG_IO, SG_INFO, "  nav2_freq = " << nav2_freq );
 499
 500             // nav2 selected radial
 501             end = msg.find(",", begin);
 502             if ( end == string::npos ) {
 503                 return false;
 504             }
 505
 506             string nav2_rad = msg.substr(begin, end - begin);
 507             begin = end + 1;
 508             SG_LOG( SG_IO, SG_INFO, "  nav2_rad = " << nav2_rad );
 509
 510             // adf freq
 511             end = msg.find("*", begin);
 512             if ( end == string::npos ) {
 513                 return false;
 514             }
 515
 516             string adf_freq = msg.substr(begin, end - begin);
 517             begin = end + 1;
 518             SG_LOG( SG_IO, SG_INFO, "  adf_freq = " << adf_freq );
 519         }
 520
 521         // printf("%.8f %.8f\n", lon, lat);
 522
 523         begin = begin_line;
 524         end_line = msg.find("\n", begin_line);
 525     }
 526
 527     return true;
 528 }
 529
 530
 531 // open hailing frequencies
 532 bool FGAtlas::open() {
 533     if ( is_enabled() ) {
 534         SG_LOG( SG_IO, SG_ALERT, "This shouldn't happen, but the channel "
 535                 << "is already in use, ignoring" );
 536         return false;
 537     }
 538
 539     SGIOChannel *io = get_io_channel();
 540
 541     if ( ! io->open( get_direction() ) ) {
 542         SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." );
 543         return false;
 544     }
 545
 546     set_enabled( true );
 547
 548     return true;
 549 }
 550
 551
 552 // process work for this port
 553 bool FGAtlas::process() {
 554     SGIOChannel *io = get_io_channel();
 555
 556     if ( get_direction() == SG_IO_OUT ) {
 557         gen_message();
 558         if ( ! io->write( buf, length ) ) {
 559             SG_LOG( SG_IO, SG_WARN, "Error writing data." );
 560             return false;
 561         }
 562     } else if ( get_direction() == SG_IO_IN ) {
 563         if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
 564             parse_message();
 565         } else {
 566             SG_LOG( SG_IO, SG_WARN, "Error reading data." );
 567             return false;
 568         }
 569         if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
 570             parse_message();
 571         } else {
 572             SG_LOG( SG_IO, SG_WARN, "Error reading data." );
 573             return false;
 574         }
 575     }
 576
 577     return true;
 578 }
 579
 580
 581 // close the channel
 582 bool FGAtlas::close() {
 583     SG_LOG( SG_IO, SG_INFO, "closing FGAtlas" );
 584     SGIOChannel *io = get_io_channel();
 585
 586     set_enabled( false );
 587
 588     if ( ! io->close() ) {
 589         return false;
 590     }
 591
 592     return true;
 593 }