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