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