src/Network/AV400WSim.cxx

   1 // AV400WSim.cxx -- Garmin 400 series protocal class.  This AV400WSim
   2 // protocol generates the set of "simulator" commands a garmin 400 WAAS
   3 // series gps would expect as input in simulator mode.  The AV400W
   4 // protocol parses the set of commands that a garmin 400W series gps
   5 // would emit.
   6 //
   7 // The Garmin WAAS GPS uses 2 serial channels to communicate with the
   8 // simulator.  These 2 channels are represented by the FGAV400WSimA and
   9 // the FGAV400WSimB classes.  The "A" channel is similar to the previous
  10 // AVSim400 protocol. The "B" channel is considered the "GPS" channel and
  11 // uses a different protocol than the "A" channel. The GPS unit expects
  12 // input on the "B" channel at two different frequencies (1hz and 5hz,
  13 // normally).  The "B" channel also expects responses to certain output
  14 // messages.
  15 //
  16 // Original AV400Sim code Written by Curtis Olson, started Janauary 2009.
  17 // This AV400W code written by Bruce Hellstrom, March 2011.
  18 //
  19 // Copyright (C) 2009  Curtis L. Olson - http://www.flightgear.org/~curt
  20 // Copyright (c) 2011  Bruce Hellstrom - http://www.celebritycc.com
  21 //
  22 // This program is free software; you can redistribute it and/or
  23 // modify it under the terms of the GNU General Public License as
  24 // published by the Free Software Foundation; either version 2 of the
  25 // License, or (at your option) any later version.
  26 //
  27 // This program is distributed in the hope that it will be useful, but
  28 // WITHOUT ANY WARRANTY; without even the implied warranty of
  29 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  30 // General Public License for more details.
  31 //
  32 // You should have received a copy of the GNU General Public License
  33 // along with this program; if not, write to the Free Software
  34 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  35 //
  36 //
  37 // $Id$
  38
  39
  40 #ifdef HAVE_CONFIG_H
  41 #  include "config.h"
  42 #endif
  43
  44 #include <cstdio>
  45
  46 #include <simgear/debug/logstream.hxx>
  47 #include <simgear/math/sg_geodesy.hxx>
  48 #include <simgear/io/iochannel.hxx>
  49 #include <simgear/timing/sg_time.hxx>
  50
  51 #include <FDM/flightProperties.hxx>
  52 #include <Main/fg_props.hxx>
  53 #include <Main/globals.hxx>
  54
  55 #include "AV400WSim.hxx"
  56
  57 FGAV400WSimA::FGAV400WSimA() {
  58 }
  59
  60 FGAV400WSimA::~FGAV400WSimA() {
  61 }
  62
  63
  64 // generate AV400WSimA message
  65 bool FGAV400WSimA::gen_message() {
  66     // cout << "generating garmin message" << endl;
  67
  68     char msg_h[32], msg_i[32], msg_j[32], msg_k[32], msg_l[32];
  69     //char msg_type2[256];
  70
  71     double alt;
  72
  73     // create msg_h
  74     double obs = fgGetDouble( "/instrumentation/nav[0]/radials/selected-deg" );
  75     sprintf( msg_h, "h%04d\r\n", (int)(obs*10) );
  76
  77     // create msg_i
  78     double fuel = fgGetDouble( "/consumables/fuel/total-fuel-gals" );
  79     if ( fuel > 999.9 ) { fuel = 999.9; }
  80     sprintf( msg_i, "i%04.0f\r\n", fuel*10.0 );
  81
  82     // create msg_j
  83     double gph = fgGetDouble( "/engines/engine[0]/fuel-flow-gph" );
  84     gph += fgGetDouble( "/engines/engine[1]/fuel-flow-gph" );
  85     gph += fgGetDouble( "/engines/engine[2]/fuel-flow-gph" );
  86     gph += fgGetDouble( "/engines/engine[3]/fuel-flow-gph" );
  87     if ( gph > 999.9 ) { gph = 999.9; }
  88     sprintf( msg_j, "j%04.0f\r\n", gph*10.0 );
  89
  90     // create msg_k
  91     sprintf( msg_k, "k%04d%02d%02d%02d%02d%02d\r\n",
  92              fgGetInt( "/sim/time/utc/year"),
  93              fgGetInt( "/sim/time/utc/month"),
  94              fgGetInt( "/sim/time/utc/day"),
  95              fgGetInt( "/sim/time/utc/hour"),
  96              fgGetInt( "/sim/time/utc/minute"),
  97              fgGetInt( "/sim/time/utc/second") );
  98
  99     // create msg_l
 100     alt = fgGetDouble( "/instrumentation/pressure-alt-ft" );
 101     if ( alt > 99999.0 ) { alt = 99999.0; }
 102     sprintf( msg_l, "l%05.0f\r\n", alt );
 103
 104     // sentence type 2
 105     //sprintf( msg_type2, "w01%c\r\n", (char)65 );
 106
 107     // assemble message
 108     string sentence;
 109     sentence += '\002';         // STX
 110     sentence += msg_h;          // obs heading in deg (*10)
 111     sentence += msg_i;          // total fuel in gal (*10)
 112     sentence += msg_j;          // fuel flow gph (*10)
 113     sentence += msg_k;          // date/time (UTC)
 114     sentence += msg_l;          // pressure altitude
 115     //sentence += msg_type2;      // type2 message
 116     sentence += '\003';         // ETX
 117
 118     // cout << sentence;
 119     length = sentence.length();
 120     // cout << endl << "length = " << length << endl;
 121     strncpy( buf, sentence.c_str(), length );
 122
 123     return true;
 124 }
 125
 126
 127 // parse AV400SimA message
 128 bool FGAV400WSimA::parse_message() {
 129     SG_LOG( SG_IO, SG_INFO, "parse AV400WSimA message" );
 130
 131     string msg = buf;
 132     msg = msg.substr( 0, length );
 133     SG_LOG( SG_IO, SG_INFO, "entire message = " << msg );
 134
 135     string ident = msg.substr(0, 1);
 136     if ( ident == "i" ) {
 137         string side = msg.substr(1,1);
 138         string num = msg.substr(2,3);
 139         if ( side == "-" ) {
 140             fgSetDouble("/instrumentation/av400w/cdi-deflection", 0.0);
 141         }
 142         else {
 143             int pos = atoi(num.c_str());
 144             if ( side == "L" ) {
 145                 pos *= -1;
 146             }
 147             fgSetDouble("/instrumentation/av400w/cdi-deflection",
 148                         (double)pos / 10.0);
 149             //printf( "i, %s%s, %f\n", side.c_str(), num.c_str(), (double)(pos / 10.0) );
 150         }
 151     }
 152     else if ( ident == "j" ) {
 153         string side = msg.substr(1,1);
 154         string num = msg.substr(2,3);
 155         if ( side == "-" ) {
 156             fgSetDouble("/instrumentation/av400w/gs-deflection", 0.0);
 157         }
 158         else {
 159             int pos = atoi(num.c_str());
 160             if ( side == "B" ) {
 161                 pos *= -1;
 162             }
 163             // convert glideslope to -3.5 to 3.5
 164             fgSetDouble("/instrumentation/av400w/gs-deflection",
 165                         (double)pos / 28.57);
 166             //printf( "j, %s%s, %f\n", side.c_str(), num.c_str(), (double)(pos / 28.57) );
 167         }
 168     }
 169     else if ( ident == "k" ) {
 170         string ind = msg.substr(1,1);
 171         if ( ind == "T" ) {
 172             fgSetBool("/instrumentation/av400w/to-flag", true);
 173             fgSetBool("/instrumentation/av400w/from-flag", false);
 174             //printf( "set to-flag\n" );
 175         } else if ( ind == "F" ) {
 176             fgSetBool("/instrumentation/av400w/to-flag", false);
 177             fgSetBool("/instrumentation/av400w/from-flag", true);
 178             //printf( "set from flag\n" );
 179         } else {
 180             fgSetBool("/instrumentation/av400w/to-flag", false);
 181             fgSetBool("/instrumentation/av400w/from-flag", false);
 182             //printf( "set t/f both false\n" );
 183         }
 184         //printf( "k, %s\n", ind.c_str() );
 185     }
 186     else if ( ident == "S" ) {
 187         string ind = msg.substr(1,5);
 188         //printf( "S - %s\n", ind.c_str() );
 189     }
 190     else {
 191         // SG_LOG( SG_IO, SG_ALERT, "unknown AV400Sim message = " << msg );
 192     }
 193
 194     return true;
 195 }
 196
 197
 198 // open hailing frequencies
 199 bool FGAV400WSimA::open() {
 200     if ( is_enabled() ) {
 201         SG_LOG( SG_IO, SG_ALERT, "This shouldn't happen, but the channel "
 202                 << "is already in use, ignoring" );
 203         return false;
 204     }
 205
 206     SGIOChannel *io = get_io_channel();
 207
 208     if ( ! io->open( get_direction() ) ) {
 209         SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." );
 210         return false;
 211     }
 212
 213     set_enabled( true );
 214
 215     return true;
 216 }
 217
 218
 219 // process work for this port
 220 bool FGAV400WSimA::process() {
 221     SGIOChannel *io = get_io_channel();
 222
 223     // until we have parsers/generators for the reverse direction,
 224     // this is hardwired to expect that the physical GPS is slaving
 225     // from FlightGear.
 226
 227     // Send FlightGear data to the external device
 228     gen_message();
 229     if ( ! io->write( buf, length ) ) {
 230         SG_LOG( SG_IO, SG_WARN, "Error writing data." );
 231         return false;
 232     }
 233
 234     // read the device messages back
 235     while ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
 236         // SG_LOG( SG_IO, SG_ALERT, "Success reading data." );
 237         if ( parse_message() ) {
 238             // SG_LOG( SG_IO, SG_ALERT, "Success parsing data." );
 239         } else {
 240             // SG_LOG( SG_IO, SG_ALERT, "Error parsing data." );
 241         }
 242     }
 243
 244     return true;
 245 }
 246
 247
 248 // close the channel
 249 bool FGAV400WSimA::close() {
 250     SGIOChannel *io = get_io_channel();
 251
 252     set_enabled( false );
 253
 254     if ( ! io->close() ) {
 255         return false;
 256     }
 257
 258     return true;
 259 }
 260
 261 // Start of FGAV400WSimB class methods
 262 FGAV400WSimB::FGAV400WSimB() :
 263 hz2(0.0),
 264 hz2count(0),
 265 hz2cycles(0),
 266 flight_phase(0xFF),
 267 req_hostid(true),
 268 req_sbas(false)
 269 {
 270     hal.clear();
 271     val.clear();
 272     hal.append( "\0\0", 2 );
 273     val.append( "\0\0", 2 );
 274     outputctr = 0;
 275     sbas_sel.append( "\0\x01", 2 );
 276     fdm = new FlightProperties;
 277 }
 278
 279 FGAV400WSimB::~FGAV400WSimB() {
 280     delete fdm;
 281 }
 282
 283
 284 bool FGAV400WSimB::gen_hostid_message() {
 285     char chksum = 0;
 286     string data = "Cj\r\n";
 287     data += "COPYRIGHT 2008 GARMIN LTD.       \r\n";
 288     data += "SFTW P/N #    006-B0339-0A\r\n";
 289     data += "SOFTWARE VER #           3\r\n";
 290     data += "SOFTWARE REV #           2\r\n";
 291     data += "SOFTWARE DATE   11/03/2008\r\n";
 292     data += "SW CRC   8F5E7DD1 AE5D4563\r\n";
 293     data += "HDWR P/N # 012-00857-01   \r\n";
 294     data += "SERIAL #   085701214976140\r\n";
 295     data += "MANUFACTUR DATE 02/26/2007\r\n";
 296     data += "OPTIONS LIST    iiiiiiiiii";
 297
 298     // calculate the checksum
 299     for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
 300         chksum ^= *cli;
 301     }
 302
 303     string sentence( "@@" );
 304     sentence += data;
 305     sentence.push_back( chksum );
 306     sentence += "\x0D\n";
 307
 308     length = sentence.length();
 309     char *bufptr = buf;
 310     for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
 311         *bufptr++ = *cli;
 312     }
 313
 314     return true;
 315 }
 316
 317 bool FGAV400WSimB::gen_sbas_message() {
 318     char chksum = 0;
 319     string data = "WA";
 320     data.push_back( '\0' );
 321     data += sbas_sel;
 322
 323     // calculate the checksum
 324     for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
 325         chksum ^= *cli;
 326     }
 327
 328     string sentence( "@@" );
 329     sentence += data;
 330     sentence.push_back( chksum );
 331     sentence += "\x0D\n";
 332
 333     length = sentence.length();
 334     char *bufptr = buf;
 335     for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
 336         *bufptr++ = *cli;
 337     }
 338
 339     return true;
 340 }
 341
 342 // Wh - Visible SBAS Satellites (hz2)
 343 bool FGAV400WSimB::gen_Wh_message() {
 344     char chksum = 0;
 345
 346     // generate the Wh message
 347     string data = "Wh";
 348     data.push_back( '\x0F' );
 349     data.append( "\x3f\x00\x00\x20\x00\x20", 6 );
 350     data.append( "\x4f\x00\x00\x28\x00\x30", 6 );
 351     data.append( "\x2d\x00\x00\x48\x01\x05", 6 );
 352     data.append( "\x1d\x00\x00\x10\x01\x10", 6 );
 353     data.append( "\x50\x00\x00\x33\x00\x50", 6 );
 354     data.append( "\x22\x00\x00\x16\x00\x90", 6 );
 355     data.append( "\x40\x00\x00\x20\x00\x20", 6 );
 356     data.append( "\x50\x00\x00\x28\x00\x30", 6 );
 357     data.append( "\x2e\x00\x00\x48\x01\x05", 6 );
 358     data.append( "\x1e\x00\x00\x10\x01\x10", 6 );
 359     data.append( "\x51\x00\x00\x33\x00\x50", 6 );
 360     data.append( "\x23\x00\x00\x16\x00\x90", 6 );
 361     data.append( "\x1f\x00\x00\x10\x01\x10", 6 );
 362     data.append( "\x52\x00\x00\x33\x00\x50", 6 );
 363     data.append( "\x24\x00\x00\x16\x00\x90", 6 );
 364     data.push_back( '0' );
 365
 366     // calculate the checksum
 367     for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
 368         chksum ^= *cli;
 369     }
 370
 371     string sentence( "@@" );
 372     sentence += data;
 373     sentence.push_back( chksum );
 374     sentence += "\x0D\n";
 375
 376     length = sentence.length();
 377     char *bufptr = buf;
 378     for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
 379         *bufptr++ = *cli;
 380     }
 381
 382     return true;
 383 }
 384
 385
 386 // Wx - Channel Status Message (hz2)
 387 bool FGAV400WSimB::gen_Wx_message() {
 388     char chksum = 0;
 389
 390     // Now process the Wx message
 391     string data = "Wx";
 392     data.push_back( (char)( fgGetInt( "/sim/time/utc/month") & 0xFF ) );
 393     data.push_back( (char)( fgGetInt( "/sim/time/utc/day") & 0xFF ) );
 394     data.push_back( (char)( (fgGetInt( "/sim/time/utc/year") >> 8 ) & 0xFF ) );
 395     data.push_back( (char)( fgGetInt( "/sim/time/utc/year") & 0xFF ) );
 396     data.push_back( (char)( fgGetInt( "/sim/time/utc/hour") & 0xFF ) );
 397     data.push_back( (char)( fgGetInt( "/sim/time/utc/minute") & 0xFF ) );
 398     data.push_back( (char)( fgGetInt( "/sim/time/utc/second") & 0xFF ) );
 399     data.append( "\x00\x00\x00\x00", 4 );
 400
 401     for ( int xctr = 0; xctr < 15; xctr++ ) {
 402         data.append( "\x00\x00\x00\x00\x00\x00\x00\x00", 8 );
 403     }
 404     data.push_back( '\0' );
 405
 406     // calculate the checksum
 407     for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
 408         chksum ^= *cli;
 409     }
 410
 411     string sentence( "@@" );
 412     sentence += data;
 413     sentence.push_back( chksum );
 414     sentence += "\x0D\n";
 415
 416     // cout << sentence;
 417     length = sentence.length();
 418     char *bufptr = buf;
 419     for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
 420         *bufptr++ = *cli;
 421     }
 422
 423     return true;
 424 }
 425
 426
 427 // Wt - Position and Navigation status
 428 bool FGAV400WSimB::gen_Wt_message() {
 429     char chksum = 0;
 430
 431     // generate the Wt message
 432     string data = "Wt";
 433     data.push_back( (char)( fgGetInt( "/sim/time/utc/month") & 0xFF ) );
 434     data.push_back( (char)( fgGetInt( "/sim/time/utc/day") & 0xFF ) );
 435     data.push_back( (char)( (fgGetInt( "/sim/time/utc/year") >> 8 ) & 0xFF ) );
 436     data.push_back( (char)( fgGetInt( "/sim/time/utc/year") & 0xFF ) );
 437     data.push_back( (char)( fgGetInt( "/sim/time/utc/hour") & 0xFF ) );
 438     data.push_back( (char)( fgGetInt( "/sim/time/utc/minute") & 0xFF ) );
 439     data.push_back( (char)( fgGetInt( "/sim/time/utc/second") & 0xFF ) );
 440     data.append( "\x00\x00\x00\x00", 4 );
 441
 442     // get latitude in milliarcseconds
 443     double latd = fdm->get_Latitude() * SGD_RADIANS_TO_DEGREES;
 444     latd *= DEG_TO_MILLIARCSECS;
 445     int latitude = (int)latd;
 446     data.push_back( (char)( ( latitude >> 24 ) & 0xFF ) );
 447     data.push_back( (char)( ( latitude >> 16 ) & 0xFF ) );
 448     data.push_back( (char)( ( latitude >> 8 ) & 0xFF ) );
 449     data.push_back( (char)( latitude & 0xFF ) );
 450
 451     // get longitude in milliarcseconds
 452     double lond = fdm->get_Longitude() * SGD_RADIANS_TO_DEGREES;
 453     lond *= DEG_TO_MILLIARCSECS;
 454     int longitude = (int)lond;
 455     data.push_back( (char)( ( longitude >> 24 ) & 0xFF ) );
 456     data.push_back( (char)( ( longitude >> 16 ) & 0xFF ) );
 457     data.push_back( (char)( ( longitude >> 8 ) & 0xFF ) );
 458     data.push_back( (char)( longitude & 0xFF ) );
 459
 460
 461     // Altitude settings
 462     double alt = fdm->get_Altitude();
 463     if ( alt > 99999.0 ) { alt = 99999.0; }
 464
 465     // send the  WGS-84 ellipsoid height om /-1, (just use regular altitude)
 466     alt *= SG_FEET_TO_METER;
 467     int altm = (int)( alt * 100.0f );
 468     data.push_back( (char)( ( altm >> 24 ) & 0xFF ) );
 469     data.push_back( (char)( ( altm >> 16 ) & 0xFF ) );
 470     data.push_back( (char)( ( altm >> 8 ) & 0xFF ) );
 471     data.push_back( (char)( altm & 0xFF ) );
 472
 473     // put in the geoid height in 0.1 meters
 474     data.push_back( (char)( ( altm >> 24 ) & 0xFF ) );
 475     data.push_back( (char)( ( altm >> 16 ) & 0xFF ) );
 476     data.push_back( (char)( ( altm >> 8 ) & 0xFF ) );
 477     data.push_back( (char)( altm & 0xFF ) );
 478
 479     // get ground speed
 480     double gskt = fgGetDouble( "/velocities/groundspeed-kt" );
 481     gskt *= SG_KT_TO_MPS;
 482     int gsm = (int)( gskt * 100.0f );
 483     data.push_back( (char)( ( gsm >> 8 ) & 0xFF ) );
 484     data.push_back( (char)( gsm & 0xFF ) );
 485
 486     // ground track
 487     double trkdeg = fgGetDouble("/orientation/heading-deg");
 488     int hdg = (int)(trkdeg * 10.0f);
 489     data.push_back( (char)( ( hdg >> 8 ) & 0xFF ) );
 490     data.push_back( (char)( hdg & 0xFF ) );
 491
 492     // vertical velocity
 493     double climb_fpm = fgGetDouble( "/velocities/vertical-speed-fps" );
 494     climb_fpm *= SG_FEET_TO_METER;
 495     int vvm = (int)( climb_fpm * 50.0f );
 496     data.push_back( (char)( ( vvm >> 8 ) & 0xFF ) );
 497     data.push_back( (char)( vvm & 0xFF ) );
 498
 499     // navigation solution status
 500     data.push_back( '\0' );
 501
 502     // HFOM/VFOM
 503     data.append( "\0\x09\0\x09", 4 );
 504
 505     // ARINC 748 Mode
 506     data.push_back( '\x0D' );
 507
 508     // Channel Tracking
 509     data += "\x7F\xFF";
 510
 511     // calculate the checksum
 512     for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
 513         chksum ^= *cli;
 514     }
 515
 516     string sentence( "@@" );
 517     sentence += data;
 518     sentence.push_back( chksum );
 519     sentence += "\x0D\n";
 520
 521     length = sentence.length();
 522     char *bufptr = buf;
 523     for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
 524         *bufptr++ = *cli;
 525     }
 526
 527     return true;
 528 }
 529
 530
 531 // Wm - Data integrity status
 532 bool FGAV400WSimB::gen_Wm_message() {
 533     char chksum = 0;
 534
 535     // generate the Wt message
 536     string data = "Wm";
 537
 538     // flight phase
 539     data.push_back( flight_phase );
 540
 541     // HAL and VAL
 542     if ( hal.empty() ) {
 543         data.append( "\0\0", 2 );
 544     }
 545     else {
 546         data += hal;
 547     }
 548
 549     if ( val.empty() ) {
 550         data.append( "\0\0", 2 );
 551     }
 552     else {
 553         data += val;
 554     }
 555
 556     // Integrity status
 557     data.append( "\x00\x00\x00", 3 );
 558     data.append( "\x00\x01\x00\x01\x00\x01\x00\x01", 8 );
 559     data.append( "\x00\x0F\x00\x0F\x00\x0F", 6 );
 560
 561     // calculate the checksum
 562     for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
 563         chksum ^= *cli;
 564     }
 565
 566     string sentence( "@@" );
 567     sentence += data;
 568     sentence.push_back( chksum );
 569     sentence += "\x0D\n";
 570
 571     length = sentence.length();
 572     char *bufptr = buf;
 573     for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
 574         *bufptr++ = *cli;
 575     }
 576
 577     return true;
 578 }
 579
 580 // Wv - 3d velocity
 581 bool FGAV400WSimB::gen_Wv_message() {
 582     char chksum = 0;
 583
 584     // generate the Wt message
 585     string data = "Wv";
 586
 587     // data is valid
 588     data += "1";
 589
 590     // N velocity in .01 m/s
 591     double vn_mps = fgGetDouble( "/velocities/speed-north-fps" ) * SG_FEET_TO_METER;
 592     int vnm = (int)( vn_mps * 100 );
 593     data.push_back( (char)( ( vnm >> 24 ) & 0xFF ) );
 594     data.push_back( (char)( ( vnm >> 16 ) & 0xFF ) );
 595     data.push_back( (char)( ( vnm >> 8 ) & 0xFF ) );
 596     data.push_back( (char)( vnm & 0xFF ) );
 597
 598     // E velocity in .01 m/s
 599     double ve_mps = fgGetDouble( "/velocities/speed-east-fps" ) * SG_FEET_TO_METER;
 600     int vne = (int)( ve_mps * 100 );
 601     data.push_back( (char)( ( vne >> 24 ) & 0xFF ) );
 602     data.push_back( (char)( ( vne >> 16 ) & 0xFF ) );
 603     data.push_back( (char)( ( vne >> 8 ) & 0xFF ) );
 604     data.push_back( (char)( vne & 0xFF ) );
 605
 606     // Up velocity in .01 m/s
 607     double climb_mps = fgGetDouble( "/velocities/vertical-speed-fps" ) * SG_FEET_TO_METER;
 608     int vnup = (int)( climb_mps * 100 );
 609     data.push_back( (char)( ( vnup >> 24 ) & 0xFF ) );
 610     data.push_back( (char)( ( vnup >> 16 ) & 0xFF ) );
 611     data.push_back( (char)( ( vnup >> 8 ) & 0xFF ) );
 612     data.push_back( (char)( vnup & 0xFF ) );
 613
 614     // calculate the checksum
 615     for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
 616         chksum ^= *cli;
 617     }
 618
 619     string sentence( "@@" );
 620     sentence += data;
 621     sentence.push_back( chksum );
 622     sentence += "\x0D\n";
 623
 624     // cout << sentence;
 625     length = sentence.length();
 626     char *bufptr = buf;
 627     for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
 628         *bufptr++ = *cli;
 629     }
 630
 631     return true;
 632 }
 633
 634
 635 bool FGAV400WSimB::verify_checksum( string message, int datachars ) {
 636     bool bRet = false;
 637     string dataseg = message.substr(SOM_SIZE, datachars);
 638     char chksum = 0;
 639     char cs = message[SOM_SIZE + datachars];
 640     for ( string::const_iterator cli = dataseg.begin();
 641           cli != dataseg.end(); cli++ ) {
 642         chksum ^= *cli;
 643     }
 644
 645     if ( chksum == cs ) {
 646         bRet = true;
 647     }
 648     else {
 649         SG_LOG( SG_IO, SG_INFO, "bad input checksum: " << message );
 650         //string msgid = asciitize_message( message );
 651         //printf( "FGAV400SimB::verify_checksum bad input checksum:\n%s\n", msgid.c_str() );
 652     }
 653
 654     return( bRet );
 655 }
 656
 657
 658 string FGAV400WSimB::asciitize_message( string message ) {
 659     string asciimsg;
 660
 661     for ( string::const_iterator cli = message.begin();
 662           cli != message.end(); cli++ )
 663     {
 664         unsigned char uc = static_cast<unsigned char>(*cli);
 665         if ( uc >= 32 && uc <= 127 ) {
 666             asciimsg += *cli;
 667         }
 668         else {
 669             char tempbuf[20];
 670             sprintf( tempbuf, "\\x%02X", uc );
 671             asciimsg += tempbuf;
 672         }
 673     }
 674
 675     return( asciimsg );
 676 }
 677
 678 string FGAV400WSimB::buffer_to_string() {
 679     string message;
 680     char *bufctr = buf;
 681
 682     for ( int xctr = 0; xctr < length; xctr++ ) {
 683         message.push_back( *bufctr++ );
 684     }
 685     return( message );
 686 }
 687
 688
 689 // parse AV400Sim message
 690 bool FGAV400WSimB::parse_message() {
 691     SG_LOG( SG_IO, SG_INFO, "parse AV400WSimB message" );
 692
 693     string msg = buffer_to_string();
 694
 695     string som = msg.substr(0, 2);
 696     if ( som != "@@" ) {
 697         SG_LOG( SG_IO, SG_INFO, "bad start message" );
 698         return false;
 699     }
 700
 701     string ident = msg.substr(2,2);
 702
 703     if ( ident == "AH" ) { // Flight Phase
 704         if ( verify_checksum( msg, 3 ) ) {
 705             flight_phase = msg[4];
 706             //string ascmsg = asciitize_message( msg );
 707             //printf( "%10d received AH %s\n", outputctr, ascmsg.c_str() );
 708             switch( flight_phase ) {
 709                 case FGAV400WSimB::PHASE_OCEANIC: // Oceanic
 710                     if ( hal.empty() ) {
 711                         hal = "\x39\xE0";
 712                     }
 713                     if ( val.empty() ) {
 714                         val = "\x00\x00";
 715                     }
 716                     fgSetBool( "/instrumentation/av400w/has-gs", false );
 717                     break;
 718
 719                 case PHASE_ENROUTE: // Enroute
 720                     if ( hal.empty() ) {
 721                         hal = "\x1C\xF0";
 722                     }
 723                     if ( val.empty() ) {
 724                         val = "\x00\x00";
 725                     }
 726                     fgSetBool( "/instrumentation/av400w/has-gs", false );
 727                     break;
 728
 729                 case PHASE_TERM: // Terminal
 730                     if ( hal.empty() ) {
 731                         hal = "\x0E\x78";
 732                     }
 733                     if ( val.empty() ) {
 734                         val = "\x00\x00";
 735                     }
 736                     fgSetBool( "/instrumentation/av400w/has-gs", false );
 737                     break;
 738
 739                 case PHASE_NONPREC: // Non Precision Approach
 740                     if ( hal.empty() ) {
 741                         hal = "\x04\x57";
 742                     }
 743                     if ( val.empty() ) {
 744                         val = "\x00\x00";
 745                     }
 746                     fgSetBool( "/instrumentation/av400w/has-gs", false );
 747                     break;
 748
 749                 case PHASE_LNAVVNAV: // LNAV/VNAV
 750                     if ( hal.empty() ) {
 751                         hal = "\x04\x57";
 752                     }
 753                     if ( val.empty() ) {
 754                         val = "\x00\x64";
 755                     }
 756                     fgSetBool( "/instrumentation/av400w/has-gs", true );
 757                     break;
 758
 759                 case PHASE_LPVLP: // LPV/LP
 760                     if ( hal.empty() ) {
 761                         hal = "\x00\x00";
 762                     }
 763                     if ( val.empty() ) {
 764                         val = "\x00\x00";
 765                     }
 766                     fgSetBool( "/instrumentation/av400w/has-gs", true );
 767                     break;
 768
 769                 default:
 770                     if ( hal.empty() ) {
 771                         hal = "\x00\x00";
 772                     }
 773                     if ( val.empty() ) {
 774                         val = "\x00\x00";
 775                     }
 776                     fgSetBool( "/instrumentation/av400w/has-gs", false );
 777                     break;
 778             }
 779             //printf( "AH flight status: %c\n", flight_phase + '0' );
 780         }
 781     }
 782     else if ( ident == "AI" ) { // HAL
 783         if ( verify_checksum( msg, 4 ) ) {
 784             hal = msg.substr(4,2);
 785             //printf( "%10d received AI\n", outputctr );
 786         }
 787     }
 788     else if ( ident == "Cj" ) { // Host ID
 789         if ( verify_checksum( msg, 2 ) ) {
 790             req_hostid = true;
 791             //printf( "%10d received Cj\n", outputctr );
 792         }
 793     }
 794     else if ( ident == "WA" ) { // SBAS selection
 795         if ( verify_checksum( msg, 5 ) ) {
 796             sbas_sel = msg.substr( 5, 2 );
 797             req_sbas = true;
 798             //printf( "%10d received WA\n", outputctr );
 799         }
 800     }
 801     else if ( ident == "Wd" ) { // VAL
 802         if ( verify_checksum( msg, 4 ) ) {
 803             val = msg.substr( 4, 2 );
 804             //printf( "%10d received Wd\n", outputctr );
 805         }
 806     }
 807     else if ( ident == "WY" ) { // ???? Not listed in protocol document
 808         // Do nothing until we know what it does
 809     }
 810     else {
 811         string unkmsg = msg.substr( 0, 4 );
 812         printf( "parse_message unknown: %s\n", unkmsg.c_str() );
 813     }
 814
 815     return true;
 816 }
 817
 818
 819 // open hailing frequencies
 820 bool FGAV400WSimB::open() {
 821     if ( is_enabled() ) {
 822         SG_LOG( SG_IO, SG_ALERT, "This shouldn't happen, but the channel "
 823                 << "is already in use, ignoring" );
 824         return false;
 825     }
 826
 827     SGIOChannel *io = get_io_channel();
 828
 829     if ( ! io->open( get_direction() ) ) {
 830         SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." );
 831         return false;
 832     }
 833
 834     set_enabled( true );
 835
 836     return true;
 837 }
 838
 839
 840 // process work for this port
 841 bool FGAV400WSimB::process() {
 842     SGIOChannel *io = get_io_channel();
 843
 844     // read the device messages back
 845     // Because the protocol allows for binary data, we can't just read
 846     // ascii lines.
 847     char readbuf[10];
 848     char *bufptr = buf;
 849     int templen;
 850     bool gotCr = false;
 851     bool gotLf = false;
 852     bool som1 = false;
 853     bool som2 = false;
 854     length = 0;
 855
 856     while ( ( templen = io->read( readbuf, 1 ) ) == 1 ) {
 857         if ( !som1 && !som2 ) {
 858             if ( *readbuf == '@' ) {
 859                 som1 = true;
 860             }
 861             else {
 862                 continue;
 863             }
 864         }
 865         else if ( !som2 ) {
 866             if ( *readbuf == '@' ) {
 867                 som2 = true;
 868             }
 869             else {
 870                 som1 = false;
 871                 continue;
 872             }
 873         }
 874         else if ( som1 && som2 ) {
 875             if ( *readbuf == '\n' && !gotCr ) {  // check for a carriage return
 876                 gotCr = true;
 877             }
 878             else if ( *readbuf == '\n' && gotCr ) { // see if we got a cr/lf
 879                 gotLf = true;
 880             }
 881             else if ( gotCr ) { // we had a cr but the next char was not a lf, so just must be data
 882                 gotCr = false;
 883             }
 884         }
 885
 886         *bufptr++ = *readbuf;
 887         length++;
 888
 889         if ( gotCr && gotLf ) { // message done
 890             if ( parse_message() ) {
 891                 // SG_LOG( SG_IO, SG_ALERT, "Success parsing data." );
 892             } else {
 893                 // SG_LOG( SG_IO, SG_ALERT, "Error parsing data." );
 894             }
 895             length = 0;
 896             break;
 897         }
 898     }
 899
 900
 901     // Check for polled messages
 902     if ( req_hostid ) {
 903         gen_hostid_message();
 904         if ( ! io->write( buf, length ) ) {
 905             SG_LOG( SG_IO, SG_WARN, "Error writing data." );
 906             printf( "Error sending HostID\n" );
 907             return false;
 908         }
 909         //printf( "Sent HostID, %d bytes\n", length );
 910         req_hostid = false;
 911     }
 912     else if ( req_sbas ) {
 913         gen_sbas_message();
 914         if ( ! io->write( buf, length ) ) {
 915             SG_LOG( SG_IO, SG_WARN, "Error writing data." );
 916             printf( "Error sending SBAS\n" );
 917             return false;
 918         }
 919         //printf( "Sent SBAS, %d bytes\n", length );
 920         req_sbas = false;
 921     }
 922
 923     // Send the 5Hz messages
 924     gen_Wt_message();
 925     if ( ! io->write( buf, length ) ) {
 926         SG_LOG( SG_IO, SG_WARN, "Error writing data." );
 927         printf( "Error writing hz message\n" );
 928         return false;
 929     }
 930     //printf( "Sent Wt, %d bytes\n", length );
 931
 932     gen_Wm_message();
 933     if ( ! io->write( buf, length ) ) {
 934         SG_LOG( SG_IO, SG_WARN, "Error writing data." );
 935         printf( "Error writing hz message\n" );
 936         return false;
 937     }
 938     //printf( "Sent Wm, %d bytes\n", length );
 939
 940     gen_Wv_message();
 941     if ( ! io->write( buf, length ) ) {
 942         SG_LOG( SG_IO, SG_WARN, "Error writing data." );
 943         printf( "Error writing hz message\n" );
 944         return false;
 945     }
 946     //printf( "Sent Wv, %d bytes\n", length );
 947
 948     hz2count++;
 949     if ( hz2 > 0 && ( hz2count % hz2cycles == 0 ) ) {
 950         // Send the 1Hz messages
 951         gen_Wh_message();
 952         if ( ! io->write( buf, length ) ) {
 953             SG_LOG( SG_IO, SG_WARN, "Error writing data." );
 954             printf( "Error writing hz2 message\n" );
 955             return false;
 956         }
 957         //printf( "Sent Wh, %d bytes\n", length );
 958
 959         gen_Wx_message();
 960         if ( ! io->write( buf, length ) ) {
 961             SG_LOG( SG_IO, SG_WARN, "Error writing data." );
 962             printf( "Error writing hz2 message\n" );
 963             return false;
 964         }
 965         //printf( "Sent Wx, %d bytes\n", length );
 966     }
 967
 968     // read the device messages back again to make sure we don't miss anything
 969     bufptr = buf;
 970     templen = 0;
 971     gotCr = false;
 972     gotLf = false;
 973     som1 = false;
 974     som2 = false;
 975     length = 0;
 976
 977     while ( ( templen = io->read( readbuf, 1 ) ) == 1 ) {
 978         if ( !som1 && !som2 ) {
 979             if ( *readbuf == '@' ) {
 980                 som1 = true;
 981             }
 982             else {
 983                 continue;
 984             }
 985         }
 986         else if ( !som2 ) {
 987             if ( *readbuf == '@' ) {
 988                 som2 = true;
 989             }
 990             else {
 991                 som1 = false;
 992                 continue;
 993             }
 994         }
 995         else if ( som1 && som2 ) {
 996             if ( *readbuf == '\n' && !gotCr ) {  // check for a carriage return
 997                 gotCr = true;
 998             }
 999             else if ( *readbuf == '\n' && gotCr ) { // see if we got a cr/lf
1000                 gotLf = true;
1001             }
1002             else if ( gotCr ) { // we had a cr but the next char was not a lf, so just must be data
1003                 gotCr = false;
1004             }
1005         }
1006
1007         *bufptr++ = *readbuf;
1008         length++;
1009
1010         if ( gotCr && gotLf ) { // message done
1011             //string msg = buffer_to_string();
1012             //string ascmsg = asciitize_message( msg );
1013             //printf( "Received message\n" );
1014             //printf( "%s\n", ascmsg.c_str() );
1015             //printf( "got message\n" );
1016             if ( parse_message() ) {
1017                 // SG_LOG( SG_IO, SG_ALERT, "Success parsing data." );
1018             } else {
1019                 // SG_LOG( SG_IO, SG_ALERT, "Error parsing data." );
1020             }
1021             length = 0;
1022             break;
1023         }
1024     }
1025
1026
1027     outputctr++;
1028     if ( outputctr % 10 == 0 ) {
1029         //printf( "AV400WSimB::process finished\n" );
1030     }
1031
1032     return true;
1033 }
1034
1035
1036 // close the channel
1037 bool FGAV400WSimB::close() {
1038     SGIOChannel *io = get_io_channel();
1039
1040     set_enabled( false );
1041
1042     if ( ! io->close() ) {
1043         return false;
1044     }
1045
1046     return true;
1047 }
1048
1049