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_raimap(false),
 269 req_sbas(false)
 270 {
 271     hal.clear();
 272     val.clear();
 273     hal.append( "\0\0", 2 );
 274     val.append( "\0\0", 2 );
 275     outputctr = 0;
 276     sbas_sel.append( "\0\x01", 2 );
 277     fdm = new FlightProperties;
 278 }
 279
 280 FGAV400WSimB::~FGAV400WSimB() {
 281     delete fdm;
 282 }
 283
 284
 285 bool FGAV400WSimB::gen_hostid_message() {
 286     char chksum = 0;
 287     string data = "Cj\r\n";
 288     data += "COPYRIGHT 2008 GARMIN LTD.       \r\n";
 289     data += "SFTW P/N #    006-B0339-0A\r\n";
 290     data += "SOFTWARE VER #           3\r\n";
 291     data += "SOFTWARE REV #           2\r\n";
 292     data += "SOFTWARE DATE   11/03/2008\r\n";
 293     data += "SW CRC   8F5E7DD1 AE5D4563\r\n";
 294     data += "HDWR P/N # 012-00857-01   \r\n";
 295     data += "SERIAL #   085701214976140\r\n";
 296     data += "MANUFACTUR DATE 02/26/2007\r\n";
 297     data += "OPTIONS LIST    iiiiiiiiii";
 298
 299     // calculate the checksum
 300     for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
 301         chksum ^= *cli;
 302     }
 303
 304     string sentence( "@@" );
 305     sentence += data;
 306     sentence.push_back( chksum );
 307     sentence += "\x0D\n";
 308
 309     length = sentence.length();
 310     char *bufptr = buf;
 311     for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
 312         *bufptr++ = *cli;
 313     }
 314
 315     return true;
 316 }
 317
 318 bool FGAV400WSimB::gen_sbas_message() {
 319     char chksum = 0;
 320     string data = "WA";
 321     data.push_back( '\0' );
 322     data += sbas_sel;
 323
 324     // calculate the checksum
 325     for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
 326         chksum ^= *cli;
 327     }
 328
 329     string sentence( "@@" );
 330     sentence += data;
 331     sentence.push_back( chksum );
 332     sentence += "\x0D\n";
 333
 334     length = sentence.length();
 335     char *bufptr = buf;
 336     for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
 337         *bufptr++ = *cli;
 338     }
 339
 340     return true;
 341 }
 342
 343 // Wh - Visible SBAS Satellites (hz2)
 344 bool FGAV400WSimB::gen_Wh_message() {
 345     char chksum = 0;
 346
 347     // generate the Wh message
 348     string data = "Wh";
 349     data.push_back( '\x0F' );
 350     data.append( "\x3f\x00\x00\x20\x00\x20", 6 );
 351     data.append( "\x4f\x00\x00\x28\x00\x30", 6 );
 352     data.append( "\x2d\x00\x00\x48\x01\x05", 6 );
 353     data.append( "\x1d\x00\x00\x10\x01\x10", 6 );
 354     data.append( "\x50\x00\x00\x33\x00\x50", 6 );
 355     data.append( "\x22\x00\x00\x16\x00\x90", 6 );
 356     data.append( "\x40\x00\x00\x20\x00\x20", 6 );
 357     data.append( "\x50\x00\x00\x28\x00\x30", 6 );
 358     data.append( "\x2e\x00\x00\x48\x01\x05", 6 );
 359     data.append( "\x1e\x00\x00\x10\x01\x10", 6 );
 360     data.append( "\x51\x00\x00\x33\x00\x50", 6 );
 361     data.append( "\x23\x00\x00\x16\x00\x90", 6 );
 362     data.append( "\x1f\x00\x00\x10\x01\x10", 6 );
 363     data.append( "\x52\x00\x00\x33\x00\x50", 6 );
 364     data.append( "\x24\x00\x00\x16\x00\x90", 6 );
 365     data.push_back( '0' );
 366
 367     // calculate the checksum
 368     for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
 369         chksum ^= *cli;
 370     }
 371
 372     string sentence( "@@" );
 373     sentence += data;
 374     sentence.push_back( chksum );
 375     sentence += "\x0D\n";
 376
 377     length = sentence.length();
 378     char *bufptr = buf;
 379     for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
 380         *bufptr++ = *cli;
 381     }
 382
 383     return true;
 384 }
 385
 386
 387 // Wx - Channel Status Message (hz2)
 388 bool FGAV400WSimB::gen_Wx_message() {
 389     char chksum = 0;
 390
 391     // Now process the Wx message
 392     string data = "Wx";
 393     data.push_back( (char)( fgGetInt( "/sim/time/utc/month") & 0xFF ) );
 394     data.push_back( (char)( fgGetInt( "/sim/time/utc/day") & 0xFF ) );
 395     data.push_back( (char)( (fgGetInt( "/sim/time/utc/year") >> 8 ) & 0xFF ) );
 396     data.push_back( (char)( fgGetInt( "/sim/time/utc/year") & 0xFF ) );
 397     data.push_back( (char)( fgGetInt( "/sim/time/utc/hour") & 0xFF ) );
 398     data.push_back( (char)( fgGetInt( "/sim/time/utc/minute") & 0xFF ) );
 399     data.push_back( (char)( fgGetInt( "/sim/time/utc/second") & 0xFF ) );
 400     data.append( "\x00\x00\x00\x00", 4 );
 401
 402     for ( int xctr = 0; xctr < 15; xctr++ ) {
 403         data.append( "\x00\x00\x00\x00\x00\x00\x00\x00", 8 );
 404     }
 405     data.push_back( '\0' );
 406
 407     // calculate the checksum
 408     for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
 409         chksum ^= *cli;
 410     }
 411
 412     string sentence( "@@" );
 413     sentence += data;
 414     sentence.push_back( chksum );
 415     sentence += "\x0D\n";
 416
 417     // cout << sentence;
 418     length = sentence.length();
 419     char *bufptr = buf;
 420     for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
 421         *bufptr++ = *cli;
 422     }
 423
 424     return true;
 425 }
 426
 427
 428 // Wt - Position and Navigation status
 429 bool FGAV400WSimB::gen_Wt_message() {
 430     char chksum = 0;
 431
 432     // generate the Wt message
 433     string data = "Wt";
 434     data.push_back( (char)( fgGetInt( "/sim/time/utc/month") & 0xFF ) );
 435     data.push_back( (char)( fgGetInt( "/sim/time/utc/day") & 0xFF ) );
 436     data.push_back( (char)( (fgGetInt( "/sim/time/utc/year") >> 8 ) & 0xFF ) );
 437     data.push_back( (char)( fgGetInt( "/sim/time/utc/year") & 0xFF ) );
 438     data.push_back( (char)( fgGetInt( "/sim/time/utc/hour") & 0xFF ) );
 439     data.push_back( (char)( fgGetInt( "/sim/time/utc/minute") & 0xFF ) );
 440     data.push_back( (char)( fgGetInt( "/sim/time/utc/second") & 0xFF ) );
 441     data.append( "\x00\x00\x00\x00", 4 );
 442
 443     // get latitude in milliarcseconds
 444     double latd = fdm->get_Latitude() * SGD_RADIANS_TO_DEGREES;
 445     latd *= DEG_TO_MILLIARCSECS;
 446     int latitude = (int)latd;
 447     data.push_back( (char)( ( latitude >> 24 ) & 0xFF ) );
 448     data.push_back( (char)( ( latitude >> 16 ) & 0xFF ) );
 449     data.push_back( (char)( ( latitude >> 8 ) & 0xFF ) );
 450     data.push_back( (char)( latitude & 0xFF ) );
 451
 452     // get longitude in milliarcseconds
 453     double lond = fdm->get_Longitude() * SGD_RADIANS_TO_DEGREES;
 454     lond *= DEG_TO_MILLIARCSECS;
 455     int longitude = (int)lond;
 456     data.push_back( (char)( ( longitude >> 24 ) & 0xFF ) );
 457     data.push_back( (char)( ( longitude >> 16 ) & 0xFF ) );
 458     data.push_back( (char)( ( longitude >> 8 ) & 0xFF ) );
 459     data.push_back( (char)( longitude & 0xFF ) );
 460
 461
 462     // Altitude settings
 463     double alt = fdm->get_Altitude();
 464     if ( alt > 99999.0 ) { alt = 99999.0; }
 465
 466     // send the  WGS-84 ellipsoid height om /-1, (just use regular altitude)
 467     alt *= SG_FEET_TO_METER;
 468     int altm = (int)( alt * 100.0f );
 469     data.push_back( (char)( ( altm >> 24 ) & 0xFF ) );
 470     data.push_back( (char)( ( altm >> 16 ) & 0xFF ) );
 471     data.push_back( (char)( ( altm >> 8 ) & 0xFF ) );
 472     data.push_back( (char)( altm & 0xFF ) );
 473
 474     // put in the geoid height in 0.1 meters
 475     data.push_back( (char)( ( altm >> 24 ) & 0xFF ) );
 476     data.push_back( (char)( ( altm >> 16 ) & 0xFF ) );
 477     data.push_back( (char)( ( altm >> 8 ) & 0xFF ) );
 478     data.push_back( (char)( altm & 0xFF ) );
 479
 480     // get ground speed
 481     double gskt = fgGetDouble( "/velocities/groundspeed-kt" );
 482     gskt *= SG_KT_TO_MPS;
 483     int gsm = (int)( gskt * 100.0f );
 484     data.push_back( (char)( ( gsm >> 8 ) & 0xFF ) );
 485     data.push_back( (char)( gsm & 0xFF ) );
 486
 487     // ground track
 488     double trkdeg = fgGetDouble("/orientation/heading-deg");
 489     int hdg = (int)(trkdeg * 10.0f);
 490     data.push_back( (char)( ( hdg >> 8 ) & 0xFF ) );
 491     data.push_back( (char)( hdg & 0xFF ) );
 492
 493     // vertical velocity
 494     double climb_fpm = fgGetDouble( "/velocities/vertical-speed-fps" );
 495     climb_fpm *= SG_FEET_TO_METER;
 496     int vvm = (int)( climb_fpm * 50.0f );
 497     data.push_back( (char)( ( vvm >> 8 ) & 0xFF ) );
 498     data.push_back( (char)( vvm & 0xFF ) );
 499
 500     // navigation solution status
 501     data.push_back( '\0' );
 502
 503     // HFOM/VFOM
 504     data.append( "\0\x09\0\x09", 4 );
 505
 506     // ARINC 748 Mode
 507     data.push_back( '\x0D' );
 508
 509     // Channel Tracking
 510     data += "\x7F\xFF";
 511
 512     // calculate the checksum
 513     for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
 514         chksum ^= *cli;
 515     }
 516
 517     string sentence( "@@" );
 518     sentence += data;
 519     sentence.push_back( chksum );
 520     sentence += "\x0D\n";
 521
 522     length = sentence.length();
 523     char *bufptr = buf;
 524     for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
 525         *bufptr++ = *cli;
 526     }
 527
 528     return true;
 529 }
 530
 531
 532 // Wm - Data integrity status
 533 bool FGAV400WSimB::gen_Wm_message() {
 534     char chksum = 0;
 535
 536     // generate the Wt message
 537     string data = "Wm";
 538
 539     // flight phase
 540     data.push_back( flight_phase );
 541
 542     // HAL and VAL
 543     if ( hal.empty() ) {
 544         data.append( "\0\0", 2 );
 545     }
 546     else {
 547         data += hal;
 548     }
 549
 550     if ( val.empty() ) {
 551         data.append( "\0\0", 2 );
 552     }
 553     else {
 554         data += val;
 555     }
 556
 557     // Integrity status
 558     data.append( "\x00\x00\x00", 3 );
 559     data.append( "\x00\x01\x00\x01\x00\x01\x00\x01", 8 );
 560     data.append( "\x00\x0F\x00\x0F\x00\x0F", 6 );
 561
 562     // calculate the checksum
 563     for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
 564         chksum ^= *cli;
 565     }
 566
 567     string sentence( "@@" );
 568     sentence += data;
 569     sentence.push_back( chksum );
 570     sentence += "\x0D\n";
 571
 572     length = sentence.length();
 573     char *bufptr = buf;
 574     for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
 575         *bufptr++ = *cli;
 576     }
 577
 578     return true;
 579 }
 580
 581 // Wv - 3d velocity
 582 bool FGAV400WSimB::gen_Wv_message() {
 583     char chksum = 0;
 584
 585     // generate the Wt message
 586     string data = "Wv";
 587
 588     // data is valid
 589     data += "1";
 590
 591     // N velocity in .01 m/s
 592     double vn_mps = fgGetDouble( "/velocities/speed-north-fps" ) * SG_FEET_TO_METER;
 593     int vnm = (int)( vn_mps * 100 );
 594     data.push_back( (char)( ( vnm >> 24 ) & 0xFF ) );
 595     data.push_back( (char)( ( vnm >> 16 ) & 0xFF ) );
 596     data.push_back( (char)( ( vnm >> 8 ) & 0xFF ) );
 597     data.push_back( (char)( vnm & 0xFF ) );
 598
 599     // E velocity in .01 m/s
 600     double ve_mps = fgGetDouble( "/velocities/speed-east-fps" ) * SG_FEET_TO_METER;
 601     int vne = (int)( ve_mps * 100 );
 602     data.push_back( (char)( ( vne >> 24 ) & 0xFF ) );
 603     data.push_back( (char)( ( vne >> 16 ) & 0xFF ) );
 604     data.push_back( (char)( ( vne >> 8 ) & 0xFF ) );
 605     data.push_back( (char)( vne & 0xFF ) );
 606
 607     // Up velocity in .01 m/s
 608     double climb_mps = fgGetDouble( "/velocities/vertical-speed-fps" ) * SG_FEET_TO_METER;
 609     int vnup = (int)( climb_mps * 100 );
 610     data.push_back( (char)( ( vnup >> 24 ) & 0xFF ) );
 611     data.push_back( (char)( ( vnup >> 16 ) & 0xFF ) );
 612     data.push_back( (char)( ( vnup >> 8 ) & 0xFF ) );
 613     data.push_back( (char)( vnup & 0xFF ) );
 614
 615     // calculate the checksum
 616     for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
 617         chksum ^= *cli;
 618     }
 619
 620     string sentence( "@@" );
 621     sentence += data;
 622     sentence.push_back( chksum );
 623     sentence += "\x0D\n";
 624
 625     // cout << sentence;
 626     length = sentence.length();
 627     char *bufptr = buf;
 628     for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
 629         *bufptr++ = *cli;
 630     }
 631
 632     return true;
 633 }
 634
 635
 636 bool FGAV400WSimB::verify_checksum( string message, int datachars ) {
 637     bool bRet = false;
 638     string dataseg = message.substr(SOM_SIZE, datachars);
 639     char chksum = 0;
 640     char cs = message[SOM_SIZE + datachars];
 641     for ( string::const_iterator cli = dataseg.begin();
 642           cli != dataseg.end(); cli++ ) {
 643         chksum ^= *cli;
 644     }
 645
 646     if ( chksum == cs ) {
 647         bRet = true;
 648     }
 649     else {
 650         SG_LOG( SG_IO, SG_INFO, "bad input checksum: " << message );
 651         //string msgid = asciitize_message( message );
 652         //printf( "FGAV400SimB::verify_checksum bad input checksum:\n%s\n", msgid.c_str() );
 653     }
 654
 655     return( bRet );
 656 }
 657
 658
 659 string FGAV400WSimB::asciitize_message( string message ) {
 660     string asciimsg;
 661
 662     for ( string::const_iterator cli = message.begin();
 663           cli != message.end(); cli++ )
 664     {
 665         unsigned char uc = static_cast<unsigned char>(*cli);
 666         if ( uc >= 32 && uc <= 127 ) {
 667             asciimsg += *cli;
 668         }
 669         else {
 670             char tempbuf[20];
 671             sprintf( tempbuf, "\\x%02X", uc );
 672             asciimsg += tempbuf;
 673         }
 674     }
 675
 676     return( asciimsg );
 677 }
 678
 679 string FGAV400WSimB::buffer_to_string() {
 680     string message;
 681     char *bufctr = buf;
 682
 683     for ( int xctr = 0; xctr < length; xctr++ ) {
 684         message.push_back( *bufctr++ );
 685     }
 686     return( message );
 687 }
 688
 689
 690 // parse AV400Sim message
 691 bool FGAV400WSimB::parse_message() {
 692     SG_LOG( SG_IO, SG_INFO, "parse AV400WSimB message" );
 693
 694     string msg = buffer_to_string();
 695
 696     string som = msg.substr(0, 2);
 697     if ( som != "@@" ) {
 698         SG_LOG( SG_IO, SG_INFO, "bad start message" );
 699         return false;
 700     }
 701
 702     string ident = msg.substr(2,2);
 703
 704     if ( ident == "AH" ) { // Flight Phase
 705         if ( verify_checksum( msg, 3 ) ) {
 706             flight_phase = msg[4];
 707             //string ascmsg = asciitize_message( msg );
 708             //printf( "%10d received AH %s\n", outputctr, ascmsg.c_str() );
 709             switch( flight_phase ) {
 710                 case FGAV400WSimB::PHASE_OCEANIC: // Oceanic
 711                     if ( hal.empty() ) {
 712                         hal = "\x39\xE0";
 713                     }
 714                     if ( val.empty() ) {
 715                         val = "\x00\x00";
 716                     }
 717                     fgSetBool( "/instrumentation/av400w/has-gs", false );
 718                     break;
 719
 720                 case PHASE_ENROUTE: // Enroute
 721                     if ( hal.empty() ) {
 722                         hal = "\x1C\xF0";
 723                     }
 724                     if ( val.empty() ) {
 725                         val = "\x00\x00";
 726                     }
 727                     fgSetBool( "/instrumentation/av400w/has-gs", false );
 728                     break;
 729
 730                 case PHASE_TERM: // Terminal
 731                     if ( hal.empty() ) {
 732                         hal = "\x0E\x78";
 733                     }
 734                     if ( val.empty() ) {
 735                         val = "\x00\x00";
 736                     }
 737                     fgSetBool( "/instrumentation/av400w/has-gs", false );
 738                     break;
 739
 740                 case PHASE_NONPREC: // Non Precision Approach
 741                     if ( hal.empty() ) {
 742                         hal = "\x04\x57";
 743                     }
 744                     if ( val.empty() ) {
 745                         val = "\x00\x00";
 746                     }
 747                     fgSetBool( "/instrumentation/av400w/has-gs", false );
 748                     break;
 749
 750                 case PHASE_LNAVVNAV: // LNAV/VNAV
 751                     if ( hal.empty() ) {
 752                         hal = "\x04\x57";
 753                     }
 754                     if ( val.empty() ) {
 755                         val = "\x00\x64";
 756                     }
 757                     fgSetBool( "/instrumentation/av400w/has-gs", true );
 758                     break;
 759
 760                 case PHASE_LPVLP: // LPV/LP
 761                     if ( hal.empty() ) {
 762                         hal = "\x00\x00";
 763                     }
 764                     if ( val.empty() ) {
 765                         val = "\x00\x00";
 766                     }
 767                     fgSetBool( "/instrumentation/av400w/has-gs", true );
 768                     break;
 769
 770                 default:
 771                     if ( hal.empty() ) {
 772                         hal = "\x00\x00";
 773                     }
 774                     if ( val.empty() ) {
 775                         val = "\x00\x00";
 776                     }
 777                     fgSetBool( "/instrumentation/av400w/has-gs", false );
 778                     break;
 779             }
 780             //printf( "AH flight status: %c\n", flight_phase + '0' );
 781         }
 782     }
 783     else if ( ident == "AI" ) { // HAL
 784         if ( verify_checksum( msg, 4 ) ) {
 785             hal = msg.substr(4,2);
 786             //printf( "%10d received AI\n", outputctr );
 787         }
 788     }
 789     else if ( ident == "Cj" ) { // Host ID
 790         if ( verify_checksum( msg, 2 ) ) {
 791             req_hostid = true;
 792             //printf( "%10d received Cj\n", outputctr );
 793         }
 794     }
 795     else if ( ident == "WA" ) { // SBAS selection
 796         if ( verify_checksum( msg, 5 ) ) {
 797             sbas_sel = msg.substr( 5, 2 );
 798             req_sbas = true;
 799             //printf( "%10d received WA\n", outputctr );
 800         }
 801     }
 802     else if ( ident == "Wd" ) { // VAL
 803         if ( verify_checksum( msg, 4 ) ) {
 804             val = msg.substr( 4, 2 );
 805             //printf( "%10d received Wd\n", outputctr );
 806         }
 807     }
 808     else if ( ident == "WY" ) { // ???? Not listed in protocol document
 809         // Do nothing until we know what it does
 810     }
 811     else {
 812         string unkmsg = msg.substr( 0, 4 );
 813         printf( "parse_message unknown: %s\n", unkmsg.c_str() );
 814     }
 815
 816     return true;
 817 }
 818
 819
 820 // open hailing frequencies
 821 bool FGAV400WSimB::open() {
 822     if ( is_enabled() ) {
 823         SG_LOG( SG_IO, SG_ALERT, "This shouldn't happen, but the channel "
 824                 << "is already in use, ignoring" );
 825         return false;
 826     }
 827
 828     SGIOChannel *io = get_io_channel();
 829
 830     if ( ! io->open( get_direction() ) ) {
 831         SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." );
 832         return false;
 833     }
 834
 835     set_enabled( true );
 836
 837     return true;
 838 }
 839
 840
 841 // process work for this port
 842 bool FGAV400WSimB::process() {
 843     SGIOChannel *io = get_io_channel();
 844
 845     // read the device messages back
 846     // Because the protocol allows for binary data, we can't just read
 847     // ascii lines.
 848     char readbuf[10];
 849     char *bufptr = buf;
 850     int templen;
 851     bool gotCr = false;
 852     bool gotLf = false;
 853     bool som1 = false;
 854     bool som2 = false;
 855     length = 0;
 856
 857     while ( ( templen = io->read( readbuf, 1 ) ) == 1 ) {
 858         if ( !som1 && !som2 ) {
 859             if ( *readbuf == '@' ) {
 860                 som1 = true;
 861             }
 862             else {
 863                 continue;
 864             }
 865         }
 866         else if ( !som2 ) {
 867             if ( *readbuf == '@' ) {
 868                 som2 = true;
 869             }
 870             else {
 871                 som1 = false;
 872                 continue;
 873             }
 874         }
 875         else if ( som1 && som2 ) {
 876             if ( *readbuf == '\n' && !gotCr ) {  // check for a carriage return
 877                 gotCr = true;
 878             }
 879             else if ( *readbuf == '\n' && gotCr ) { // see if we got a cr/lf
 880                 gotLf = true;
 881             }
 882             else if ( gotCr ) { // we had a cr but the next char was not a lf, so just must be data
 883                 gotCr = false;
 884             }
 885         }
 886
 887         *bufptr++ = *readbuf;
 888         length++;
 889
 890         if ( gotCr && gotLf ) { // message done
 891             if ( parse_message() ) {
 892                 // SG_LOG( SG_IO, SG_ALERT, "Success parsing data." );
 893             } else {
 894                 // SG_LOG( SG_IO, SG_ALERT, "Error parsing data." );
 895             }
 896             length = 0;
 897             break;
 898         }
 899     }
 900
 901
 902     // Check for polled messages
 903     if ( req_hostid ) {
 904         gen_hostid_message();
 905         if ( ! io->write( buf, length ) ) {
 906             SG_LOG( SG_IO, SG_WARN, "Error writing data." );
 907             printf( "Error sending HostID\n" );
 908             return false;
 909         }
 910         //printf( "Sent HostID, %d bytes\n", length );
 911         req_hostid = false;
 912     }
 913     else if ( req_sbas ) {
 914         gen_sbas_message();
 915         if ( ! io->write( buf, length ) ) {
 916             SG_LOG( SG_IO, SG_WARN, "Error writing data." );
 917             printf( "Error sending SBAS\n" );
 918             return false;
 919         }
 920         //printf( "Sent SBAS, %d bytes\n", length );
 921         req_sbas = false;
 922     }
 923
 924     // Send the 5Hz messages
 925     gen_Wt_message();
 926     if ( ! io->write( buf, length ) ) {
 927         SG_LOG( SG_IO, SG_WARN, "Error writing data." );
 928         printf( "Error writing hz message\n" );
 929         return false;
 930     }
 931     //printf( "Sent Wt, %d bytes\n", length );
 932
 933     gen_Wm_message();
 934     if ( ! io->write( buf, length ) ) {
 935         SG_LOG( SG_IO, SG_WARN, "Error writing data." );
 936         printf( "Error writing hz message\n" );
 937         return false;
 938     }
 939     //printf( "Sent Wm, %d bytes\n", length );
 940
 941     gen_Wv_message();
 942     if ( ! io->write( buf, length ) ) {
 943         SG_LOG( SG_IO, SG_WARN, "Error writing data." );
 944         printf( "Error writing hz message\n" );
 945         return false;
 946     }
 947     //printf( "Sent Wv, %d bytes\n", length );
 948
 949     hz2count++;
 950     if ( hz2 > 0 && ( hz2count % hz2cycles == 0 ) ) {
 951         // Send the 1Hz messages
 952         gen_Wh_message();
 953         if ( ! io->write( buf, length ) ) {
 954             SG_LOG( SG_IO, SG_WARN, "Error writing data." );
 955             printf( "Error writing hz2 message\n" );
 956             return false;
 957         }
 958         //printf( "Sent Wh, %d bytes\n", length );
 959
 960         gen_Wx_message();
 961         if ( ! io->write( buf, length ) ) {
 962             SG_LOG( SG_IO, SG_WARN, "Error writing data." );
 963             printf( "Error writing hz2 message\n" );
 964             return false;
 965         }
 966         //printf( "Sent Wx, %d bytes\n", length );
 967     }
 968
 969     // read the device messages back again to make sure we don't miss anything
 970     bufptr = buf;
 971     templen = 0;
 972     gotCr = false;
 973     gotLf = false;
 974     som1 = false;
 975     som2 = false;
 976     length = 0;
 977
 978     while ( ( templen = io->read( readbuf, 1 ) ) == 1 ) {
 979         if ( !som1 && !som2 ) {
 980             if ( *readbuf == '@' ) {
 981                 som1 = true;
 982             }
 983             else {
 984                 continue;
 985             }
 986         }
 987         else if ( !som2 ) {
 988             if ( *readbuf == '@' ) {
 989                 som2 = true;
 990             }
 991             else {
 992                 som1 = false;
 993                 continue;
 994             }
 995         }
 996         else if ( som1 && som2 ) {
 997             if ( *readbuf == '\n' && !gotCr ) {  // check for a carriage return
 998                 gotCr = true;
 999             }
1000             else if ( *readbuf == '\n' && gotCr ) { // see if we got a cr/lf
1001                 gotLf = true;
1002             }
1003             else if ( gotCr ) { // we had a cr but the next char was not a lf, so just must be data
1004                 gotCr = false;
1005             }
1006         }
1007
1008         *bufptr++ = *readbuf;
1009         length++;
1010
1011         if ( gotCr && gotLf ) { // message done
1012             //string msg = buffer_to_string();
1013             //string ascmsg = asciitize_message( msg );
1014             //printf( "Received message\n" );
1015             //printf( "%s\n", ascmsg.c_str() );
1016             //printf( "got message\n" );
1017             if ( parse_message() ) {
1018                 // SG_LOG( SG_IO, SG_ALERT, "Success parsing data." );
1019             } else {
1020                 // SG_LOG( SG_IO, SG_ALERT, "Error parsing data." );
1021             }
1022             length = 0;
1023             break;
1024         }
1025     }
1026
1027
1028     outputctr++;
1029     if ( outputctr % 10 == 0 ) {
1030         //printf( "AV400WSimB::process finished\n" );
1031     }
1032
1033     return true;
1034 }
1035
1036
1037 // close the channel
1038 bool FGAV400WSimB::close() {
1039     SGIOChannel *io = get_io_channel();
1040
1041     set_enabled( false );
1042
1043     if ( ! io->close() ) {
1044         return false;
1045     }
1046
1047     return true;
1048 }
1049
1050