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
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
16 // Original AV400Sim code Written by Curtis Olson, started Janauary 2009.
17 // This AV400W code written by Bruce Hellstrom, March 2011.
19 // Copyright (C) 2009 Curtis L. Olson - http://www.flightgear.org/~curt
20 // Copyright (c) 2011 Bruce Hellstrom - http://www.celebritycc.com
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.
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.
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.
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>
49 #include <FDM/flightProperties.hxx>
50 #include <Main/fg_props.hxx>
51 #include <Main/globals.hxx>
53 #include "AV400WSim.hxx"
55 FGAV400WSimA::FGAV400WSimA() {
58 FGAV400WSimA::~FGAV400WSimA() {
62 // generate AV400WSimA message
63 bool FGAV400WSimA::gen_message() {
64 // cout << "generating garmin message" << endl;
66 char msg_h[32], msg_i[32], msg_j[32], msg_k[32], msg_l[32];
67 //char msg_type2[256];
72 double obs = fgGetDouble( "/instrumentation/nav[0]/radials/selected-deg" );
73 sprintf( msg_h, "h%04d\r\n", (int)(obs*10) );
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 );
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 );
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") );
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 );
103 //sprintf( msg_type2, "w01%c\r\n", (char)65 );
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
117 length = sentence.length();
118 // cout << endl << "length = " << length << endl;
119 strncpy( buf, sentence.c_str(), length );
125 // parse AV400SimA message
126 bool FGAV400WSimA::parse_message() {
127 SG_LOG( SG_IO, SG_INFO, "parse AV400WSimA message" );
130 msg = msg.substr( 0, length );
131 SG_LOG( SG_IO, SG_INFO, "entire message = " << msg );
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);
138 fgSetDouble("/instrumentation/av400w/cdi-deflection", 0.0);
141 int pos = atoi(num.c_str());
145 fgSetDouble("/instrumentation/av400w/cdi-deflection",
147 //printf( "i, %s%s, %f\n", side.c_str(), num.c_str(), (double)(pos / 10.0) );
150 else if ( ident == "j" ) {
151 string side = msg.substr(1,1);
152 string num = msg.substr(2,3);
154 fgSetDouble("/instrumentation/av400w/gs-deflection", 0.0);
157 int pos = atoi(num.c_str());
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) );
167 else if ( ident == "k" ) {
168 string ind = msg.substr(1,1);
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" );
178 fgSetBool("/instrumentation/av400w/to-flag", false);
179 fgSetBool("/instrumentation/av400w/from-flag", false);
180 //printf( "set t/f both false\n" );
182 //printf( "k, %s\n", ind.c_str() );
184 else if ( ident == "S" ) {
185 string ind = msg.substr(1,5);
186 //printf( "S - %s\n", ind.c_str() );
189 // SG_LOG( SG_IO, SG_ALERT, "unknown AV400Sim message = " << msg );
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" );
204 SGIOChannel *io = get_io_channel();
206 if ( ! io->open( get_direction() ) ) {
207 SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." );
217 // process work for this port
218 bool FGAV400WSimA::process() {
219 SGIOChannel *io = get_io_channel();
221 // until we have parsers/generators for the reverse direction,
222 // this is hardwired to expect that the physical GPS is slaving
225 // Send FlightGear data to the external device
227 if ( ! io->write( buf, length ) ) {
228 SG_LOG( SG_IO, SG_WARN, "Error writing data." );
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." );
238 // SG_LOG( SG_IO, SG_ALERT, "Error parsing data." );
247 bool FGAV400WSimA::close() {
248 SGIOChannel *io = get_io_channel();
250 set_enabled( false );
252 if ( ! io->close() ) {
259 // Start of FGAV400WSimB class methods
260 FGAV400WSimB::FGAV400WSimB() :
271 hal.append( "\0\0", 2 );
272 val.append( "\0\0", 2 );
274 sbas_sel.append( "\0\x01", 2 );
275 fdm = new FlightProperties;
278 FGAV400WSimB::~FGAV400WSimB() {
283 bool FGAV400WSimB::gen_hostid_message() {
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";
297 // calculate the checksum
298 for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
302 string sentence( "@@" );
304 sentence.push_back( chksum );
305 sentence += "\x0D\n";
307 length = sentence.length();
309 for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
316 bool FGAV400WSimB::gen_sbas_message() {
319 data.push_back( '\0' );
322 // calculate the checksum
323 for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
327 string sentence( "@@" );
329 sentence.push_back( chksum );
330 sentence += "\x0D\n";
332 length = sentence.length();
334 for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
341 // Wh - Visible SBAS Satellites (hz2)
342 bool FGAV400WSimB::gen_Wh_message() {
345 // generate the Wh message
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' );
365 // calculate the checksum
366 for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
370 string sentence( "@@" );
372 sentence.push_back( chksum );
373 sentence += "\x0D\n";
375 length = sentence.length();
377 for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
385 // Wx - Channel Status Message (hz2)
386 bool FGAV400WSimB::gen_Wx_message() {
389 // Now process the Wx message
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 );
400 for ( int xctr = 0; xctr < 15; xctr++ ) {
401 data.append( "\x00\x00\x00\x00\x00\x00\x00\x00", 8 );
403 data.push_back( '\0' );
405 // calculate the checksum
406 for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
410 string sentence( "@@" );
412 sentence.push_back( chksum );
413 sentence += "\x0D\n";
416 length = sentence.length();
418 for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
426 // Wt - Position and Navigation status
427 bool FGAV400WSimB::gen_Wt_message() {
430 // generate the Wt message
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 );
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 ) );
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 ) );
461 double alt = fdm->get_Altitude();
462 if ( alt > 99999.0 ) { alt = 99999.0; }
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 ) );
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 ) );
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 ) );
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 ) );
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 ) );
498 // navigation solution status
499 data.push_back( '\0' );
502 data.append( "\0\x09\0\x09", 4 );
505 data.push_back( '\x0D' );
510 // calculate the checksum
511 for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
515 string sentence( "@@" );
517 sentence.push_back( chksum );
518 sentence += "\x0D\n";
520 length = sentence.length();
522 for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
530 // Wm - Data integrity status
531 bool FGAV400WSimB::gen_Wm_message() {
534 // generate the Wt message
538 data.push_back( flight_phase );
542 data.append( "\0\0", 2 );
549 data.append( "\0\0", 2 );
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 );
560 // calculate the checksum
561 for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
565 string sentence( "@@" );
567 sentence.push_back( chksum );
568 sentence += "\x0D\n";
570 length = sentence.length();
572 for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
580 bool FGAV400WSimB::gen_Wv_message() {
583 // generate the Wt message
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 ) );
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 ) );
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 ) );
613 // calculate the checksum
614 for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
618 string sentence( "@@" );
620 sentence.push_back( chksum );
621 sentence += "\x0D\n";
624 length = sentence.length();
626 for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
634 bool FGAV400WSimB::verify_checksum( string message, int datachars ) {
636 string dataseg = message.substr(SOM_SIZE, datachars);
638 char cs = message[SOM_SIZE + datachars];
639 for ( string::const_iterator cli = dataseg.begin();
640 cli != dataseg.end(); cli++ ) {
644 if ( chksum == cs ) {
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() );
657 string FGAV400WSimB::asciitize_message( string message ) {
660 for ( string::const_iterator cli = message.begin();
661 cli != message.end(); cli++ )
663 unsigned char uc = static_cast<unsigned char>(*cli);
664 if ( uc >= 32 && uc <= 127 ) {
669 sprintf( tempbuf, "\\x%02X", uc );
677 string FGAV400WSimB::buffer_to_string() {
681 for ( int xctr = 0; xctr < length; xctr++ ) {
682 message.push_back( *bufctr++ );
688 // parse AV400Sim message
689 bool FGAV400WSimB::parse_message() {
690 SG_LOG( SG_IO, SG_INFO, "parse AV400WSimB message" );
692 string msg = buffer_to_string();
694 string som = msg.substr(0, 2);
696 SG_LOG( SG_IO, SG_INFO, "bad start message" );
700 string ident = msg.substr(2,2);
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
715 fgSetBool( "/instrumentation/av400w/has-gs", false );
718 case PHASE_ENROUTE: // Enroute
725 fgSetBool( "/instrumentation/av400w/has-gs", false );
728 case PHASE_TERM: // Terminal
735 fgSetBool( "/instrumentation/av400w/has-gs", false );
738 case PHASE_NONPREC: // Non Precision Approach
745 fgSetBool( "/instrumentation/av400w/has-gs", false );
748 case PHASE_LNAVVNAV: // LNAV/VNAV
755 fgSetBool( "/instrumentation/av400w/has-gs", true );
758 case PHASE_LPVLP: // LPV/LP
765 fgSetBool( "/instrumentation/av400w/has-gs", true );
775 fgSetBool( "/instrumentation/av400w/has-gs", false );
778 //printf( "AH flight status: %c\n", flight_phase + '0' );
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 );
787 else if ( ident == "Cj" ) { // Host ID
788 if ( verify_checksum( msg, 2 ) ) {
790 //printf( "%10d received Cj\n", outputctr );
793 else if ( ident == "WA" ) { // SBAS selection
794 if ( verify_checksum( msg, 5 ) ) {
795 sbas_sel = msg.substr( 5, 2 );
797 //printf( "%10d received WA\n", outputctr );
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 );
806 else if ( ident == "WY" ) { // ???? Not listed in protocol document
807 // Do nothing until we know what it does
810 string unkmsg = msg.substr( 0, 4 );
811 printf( "parse_message unknown: %s\n", unkmsg.c_str() );
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" );
826 SGIOChannel *io = get_io_channel();
828 if ( ! io->open( get_direction() ) ) {
829 SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." );
839 // process work for this port
840 bool FGAV400WSimB::process() {
841 SGIOChannel *io = get_io_channel();
843 // read the device messages back
844 // Because the protocol allows for binary data, we can't just read
855 while ( ( templen = io->read( readbuf, 1 ) ) == 1 ) {
856 if ( !som1 && !som2 ) {
857 if ( *readbuf == '@' ) {
865 if ( *readbuf == '@' ) {
873 else if ( som1 && som2 ) {
874 if ( *readbuf == '\n' && !gotCr ) { // check for a carriage return
877 else if ( *readbuf == '\n' && gotCr ) { // see if we got a cr/lf
880 else if ( gotCr ) { // we had a cr but the next char was not a lf, so just must be data
885 *bufptr++ = *readbuf;
888 if ( gotCr && gotLf ) { // message done
889 if ( parse_message() ) {
890 // SG_LOG( SG_IO, SG_ALERT, "Success parsing data." );
892 // SG_LOG( SG_IO, SG_ALERT, "Error parsing data." );
900 // Check for polled messages
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" );
908 //printf( "Sent HostID, %d bytes\n", length );
911 else if ( req_sbas ) {
913 if ( ! io->write( buf, length ) ) {
914 SG_LOG( SG_IO, SG_WARN, "Error writing data." );
915 printf( "Error sending SBAS\n" );
918 //printf( "Sent SBAS, %d bytes\n", length );
922 // Send the 5Hz messages
924 if ( ! io->write( buf, length ) ) {
925 SG_LOG( SG_IO, SG_WARN, "Error writing data." );
926 printf( "Error writing hz message\n" );
929 //printf( "Sent Wt, %d bytes\n", length );
932 if ( ! io->write( buf, length ) ) {
933 SG_LOG( SG_IO, SG_WARN, "Error writing data." );
934 printf( "Error writing hz message\n" );
937 //printf( "Sent Wm, %d bytes\n", length );
940 if ( ! io->write( buf, length ) ) {
941 SG_LOG( SG_IO, SG_WARN, "Error writing data." );
942 printf( "Error writing hz message\n" );
945 //printf( "Sent Wv, %d bytes\n", length );
948 if ( hz2 > 0 && ( hz2count % hz2cycles == 0 ) ) {
949 // Send the 1Hz messages
951 if ( ! io->write( buf, length ) ) {
952 SG_LOG( SG_IO, SG_WARN, "Error writing data." );
953 printf( "Error writing hz2 message\n" );
956 //printf( "Sent Wh, %d bytes\n", length );
959 if ( ! io->write( buf, length ) ) {
960 SG_LOG( SG_IO, SG_WARN, "Error writing data." );
961 printf( "Error writing hz2 message\n" );
964 //printf( "Sent Wx, %d bytes\n", length );
967 // read the device messages back again to make sure we don't miss anything
976 while ( ( templen = io->read( readbuf, 1 ) ) == 1 ) {
977 if ( !som1 && !som2 ) {
978 if ( *readbuf == '@' ) {
986 if ( *readbuf == '@' ) {
994 else if ( som1 && som2 ) {
995 if ( *readbuf == '\n' && !gotCr ) { // check for a carriage return
998 else if ( *readbuf == '\n' && gotCr ) { // see if we got a cr/lf
1001 else if ( gotCr ) { // we had a cr but the next char was not a lf, so just must be data
1006 *bufptr++ = *readbuf;
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." );
1018 // SG_LOG( SG_IO, SG_ALERT, "Error parsing data." );
1027 if ( outputctr % 10 == 0 ) {
1028 //printf( "AV400WSimB::process finished\n" );
1035 // close the channel
1036 bool FGAV400WSimB::close() {
1037 SGIOChannel *io = get_io_channel();
1039 set_enabled( false );
1041 if ( ! io->close() ) {