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