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.
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>
51 #include <FDM/flightProperties.hxx>
52 #include <Main/fg_props.hxx>
53 #include <Main/globals.hxx>
55 #include "AV400WSim.hxx"
57 FGAV400WSimA::FGAV400WSimA() {
60 FGAV400WSimA::~FGAV400WSimA() {
64 // generate AV400WSimA message
65 bool FGAV400WSimA::gen_message() {
66 // cout << "generating garmin message" << endl;
68 char msg_h[32], msg_i[32], msg_j[32], msg_k[32], msg_l[32];
69 //char msg_type2[256];
74 double obs = fgGetDouble( "/instrumentation/nav[0]/radials/selected-deg" );
75 sprintf( msg_h, "h%04d\r\n", (int)(obs*10) );
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 );
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 );
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") );
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 );
105 //sprintf( msg_type2, "w01%c\r\n", (char)65 );
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
119 length = sentence.length();
120 // cout << endl << "length = " << length << endl;
121 strncpy( buf, sentence.c_str(), length );
127 // parse AV400SimA message
128 bool FGAV400WSimA::parse_message() {
129 SG_LOG( SG_IO, SG_INFO, "parse AV400WSimA message" );
132 msg = msg.substr( 0, length );
133 SG_LOG( SG_IO, SG_INFO, "entire message = " << msg );
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);
140 fgSetDouble("/instrumentation/av400w/cdi-deflection", 0.0);
143 int pos = atoi(num.c_str());
147 fgSetDouble("/instrumentation/av400w/cdi-deflection",
149 //printf( "i, %s%s, %f\n", side.c_str(), num.c_str(), (double)(pos / 10.0) );
152 else if ( ident == "j" ) {
153 string side = msg.substr(1,1);
154 string num = msg.substr(2,3);
156 fgSetDouble("/instrumentation/av400w/gs-deflection", 0.0);
159 int pos = atoi(num.c_str());
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) );
169 else if ( ident == "k" ) {
170 string ind = msg.substr(1,1);
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" );
180 fgSetBool("/instrumentation/av400w/to-flag", false);
181 fgSetBool("/instrumentation/av400w/from-flag", false);
182 //printf( "set t/f both false\n" );
184 //printf( "k, %s\n", ind.c_str() );
186 else if ( ident == "S" ) {
187 string ind = msg.substr(1,5);
188 //printf( "S - %s\n", ind.c_str() );
191 // SG_LOG( SG_IO, SG_ALERT, "unknown AV400Sim message = " << msg );
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" );
206 SGIOChannel *io = get_io_channel();
208 if ( ! io->open( get_direction() ) ) {
209 SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." );
219 // process work for this port
220 bool FGAV400WSimA::process() {
221 SGIOChannel *io = get_io_channel();
223 // until we have parsers/generators for the reverse direction,
224 // this is hardwired to expect that the physical GPS is slaving
227 // Send FlightGear data to the external device
229 if ( ! io->write( buf, length ) ) {
230 SG_LOG( SG_IO, SG_WARN, "Error writing data." );
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." );
240 // SG_LOG( SG_IO, SG_ALERT, "Error parsing data." );
249 bool FGAV400WSimA::close() {
250 SGIOChannel *io = get_io_channel();
252 set_enabled( false );
254 if ( ! io->close() ) {
261 // Start of FGAV400WSimB class methods
262 FGAV400WSimB::FGAV400WSimB() :
273 hal.append( "\0\0", 2 );
274 val.append( "\0\0", 2 );
276 sbas_sel.append( "\0\x01", 2 );
277 fdm = new FlightProperties;
280 FGAV400WSimB::~FGAV400WSimB() {
285 bool FGAV400WSimB::gen_hostid_message() {
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";
299 // calculate the checksum
300 for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
304 string sentence( "@@" );
306 sentence.push_back( chksum );
307 sentence += "\x0D\n";
309 length = sentence.length();
311 for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
318 bool FGAV400WSimB::gen_sbas_message() {
321 data.push_back( '\0' );
324 // calculate the checksum
325 for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
329 string sentence( "@@" );
331 sentence.push_back( chksum );
332 sentence += "\x0D\n";
334 length = sentence.length();
336 for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
343 // Wh - Visible SBAS Satellites (hz2)
344 bool FGAV400WSimB::gen_Wh_message() {
347 // generate the Wh message
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' );
367 // calculate the checksum
368 for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
372 string sentence( "@@" );
374 sentence.push_back( chksum );
375 sentence += "\x0D\n";
377 length = sentence.length();
379 for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
387 // Wx - Channel Status Message (hz2)
388 bool FGAV400WSimB::gen_Wx_message() {
391 // Now process the Wx message
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 );
402 for ( int xctr = 0; xctr < 15; xctr++ ) {
403 data.append( "\x00\x00\x00\x00\x00\x00\x00\x00", 8 );
405 data.push_back( '\0' );
407 // calculate the checksum
408 for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
412 string sentence( "@@" );
414 sentence.push_back( chksum );
415 sentence += "\x0D\n";
418 length = sentence.length();
420 for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
428 // Wt - Position and Navigation status
429 bool FGAV400WSimB::gen_Wt_message() {
432 // generate the Wt message
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 );
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 ) );
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 ) );
463 double alt = fdm->get_Altitude();
464 if ( alt > 99999.0 ) { alt = 99999.0; }
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 ) );
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 ) );
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 ) );
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 ) );
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 ) );
500 // navigation solution status
501 data.push_back( '\0' );
504 data.append( "\0\x09\0\x09", 4 );
507 data.push_back( '\x0D' );
512 // calculate the checksum
513 for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
517 string sentence( "@@" );
519 sentence.push_back( chksum );
520 sentence += "\x0D\n";
522 length = sentence.length();
524 for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
532 // Wm - Data integrity status
533 bool FGAV400WSimB::gen_Wm_message() {
536 // generate the Wt message
540 data.push_back( flight_phase );
544 data.append( "\0\0", 2 );
551 data.append( "\0\0", 2 );
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 );
562 // calculate the checksum
563 for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
567 string sentence( "@@" );
569 sentence.push_back( chksum );
570 sentence += "\x0D\n";
572 length = sentence.length();
574 for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
582 bool FGAV400WSimB::gen_Wv_message() {
585 // generate the Wt message
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 ) );
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 ) );
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 ) );
615 // calculate the checksum
616 for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) {
620 string sentence( "@@" );
622 sentence.push_back( chksum );
623 sentence += "\x0D\n";
626 length = sentence.length();
628 for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) {
636 bool FGAV400WSimB::verify_checksum( string message, int datachars ) {
638 string dataseg = message.substr(SOM_SIZE, datachars);
640 char cs = message[SOM_SIZE + datachars];
641 for ( string::const_iterator cli = dataseg.begin();
642 cli != dataseg.end(); cli++ ) {
646 if ( chksum == cs ) {
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() );
659 string FGAV400WSimB::asciitize_message( string message ) {
662 for ( string::const_iterator cli = message.begin();
663 cli != message.end(); cli++ )
665 unsigned char uc = static_cast<unsigned char>(*cli);
666 if ( uc >= 32 && uc <= 127 ) {
671 sprintf( tempbuf, "\\x%02X", uc );
679 string FGAV400WSimB::buffer_to_string() {
683 for ( int xctr = 0; xctr < length; xctr++ ) {
684 message.push_back( *bufctr++ );
690 // parse AV400Sim message
691 bool FGAV400WSimB::parse_message() {
692 SG_LOG( SG_IO, SG_INFO, "parse AV400WSimB message" );
694 string msg = buffer_to_string();
696 string som = msg.substr(0, 2);
698 SG_LOG( SG_IO, SG_INFO, "bad start message" );
702 string ident = msg.substr(2,2);
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
717 fgSetBool( "/instrumentation/av400w/has-gs", false );
720 case PHASE_ENROUTE: // Enroute
727 fgSetBool( "/instrumentation/av400w/has-gs", false );
730 case PHASE_TERM: // Terminal
737 fgSetBool( "/instrumentation/av400w/has-gs", false );
740 case PHASE_NONPREC: // Non Precision Approach
747 fgSetBool( "/instrumentation/av400w/has-gs", false );
750 case PHASE_LNAVVNAV: // LNAV/VNAV
757 fgSetBool( "/instrumentation/av400w/has-gs", true );
760 case PHASE_LPVLP: // LPV/LP
767 fgSetBool( "/instrumentation/av400w/has-gs", true );
777 fgSetBool( "/instrumentation/av400w/has-gs", false );
780 //printf( "AH flight status: %c\n", flight_phase + '0' );
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 );
789 else if ( ident == "Cj" ) { // Host ID
790 if ( verify_checksum( msg, 2 ) ) {
792 //printf( "%10d received Cj\n", outputctr );
795 else if ( ident == "WA" ) { // SBAS selection
796 if ( verify_checksum( msg, 5 ) ) {
797 sbas_sel = msg.substr( 5, 2 );
799 //printf( "%10d received WA\n", outputctr );
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 );
808 else if ( ident == "WY" ) { // ???? Not listed in protocol document
809 // Do nothing until we know what it does
812 string unkmsg = msg.substr( 0, 4 );
813 printf( "parse_message unknown: %s\n", unkmsg.c_str() );
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" );
828 SGIOChannel *io = get_io_channel();
830 if ( ! io->open( get_direction() ) ) {
831 SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." );
841 // process work for this port
842 bool FGAV400WSimB::process() {
843 SGIOChannel *io = get_io_channel();
845 // read the device messages back
846 // Because the protocol allows for binary data, we can't just read
857 while ( ( templen = io->read( readbuf, 1 ) ) == 1 ) {
858 if ( !som1 && !som2 ) {
859 if ( *readbuf == '@' ) {
867 if ( *readbuf == '@' ) {
875 else if ( som1 && som2 ) {
876 if ( *readbuf == '\n' && !gotCr ) { // check for a carriage return
879 else if ( *readbuf == '\n' && gotCr ) { // see if we got a cr/lf
882 else if ( gotCr ) { // we had a cr but the next char was not a lf, so just must be data
887 *bufptr++ = *readbuf;
890 if ( gotCr && gotLf ) { // message done
891 if ( parse_message() ) {
892 // SG_LOG( SG_IO, SG_ALERT, "Success parsing data." );
894 // SG_LOG( SG_IO, SG_ALERT, "Error parsing data." );
902 // Check for polled messages
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" );
910 //printf( "Sent HostID, %d bytes\n", length );
913 else if ( req_sbas ) {
915 if ( ! io->write( buf, length ) ) {
916 SG_LOG( SG_IO, SG_WARN, "Error writing data." );
917 printf( "Error sending SBAS\n" );
920 //printf( "Sent SBAS, %d bytes\n", length );
924 // Send the 5Hz messages
926 if ( ! io->write( buf, length ) ) {
927 SG_LOG( SG_IO, SG_WARN, "Error writing data." );
928 printf( "Error writing hz message\n" );
931 //printf( "Sent Wt, %d bytes\n", length );
934 if ( ! io->write( buf, length ) ) {
935 SG_LOG( SG_IO, SG_WARN, "Error writing data." );
936 printf( "Error writing hz message\n" );
939 //printf( "Sent Wm, %d bytes\n", length );
942 if ( ! io->write( buf, length ) ) {
943 SG_LOG( SG_IO, SG_WARN, "Error writing data." );
944 printf( "Error writing hz message\n" );
947 //printf( "Sent Wv, %d bytes\n", length );
950 if ( hz2 > 0 && ( hz2count % hz2cycles == 0 ) ) {
951 // Send the 1Hz messages
953 if ( ! io->write( buf, length ) ) {
954 SG_LOG( SG_IO, SG_WARN, "Error writing data." );
955 printf( "Error writing hz2 message\n" );
958 //printf( "Sent Wh, %d bytes\n", length );
961 if ( ! io->write( buf, length ) ) {
962 SG_LOG( SG_IO, SG_WARN, "Error writing data." );
963 printf( "Error writing hz2 message\n" );
966 //printf( "Sent Wx, %d bytes\n", length );
969 // read the device messages back again to make sure we don't miss anything
978 while ( ( templen = io->read( readbuf, 1 ) ) == 1 ) {
979 if ( !som1 && !som2 ) {
980 if ( *readbuf == '@' ) {
988 if ( *readbuf == '@' ) {
996 else if ( som1 && som2 ) {
997 if ( *readbuf == '\n' && !gotCr ) { // check for a carriage return
1000 else if ( *readbuf == '\n' && gotCr ) { // see if we got a cr/lf
1003 else if ( gotCr ) { // we had a cr but the next char was not a lf, so just must be data
1008 *bufptr++ = *readbuf;
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." );
1020 // SG_LOG( SG_IO, SG_ALERT, "Error parsing data." );
1029 if ( outputctr % 10 == 0 ) {
1030 //printf( "AV400WSimB::process finished\n" );
1037 // close the channel
1038 bool FGAV400WSimB::close() {
1039 SGIOChannel *io = get_io_channel();
1041 set_enabled( false );
1043 if ( ! io->close() ) {