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