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