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