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