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