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