From e81479d85745660cdfa002cd3a503b6877b69603 Mon Sep 17 00:00:00 2001 From: "Curtis L. Olson" Date: Tue, 3 May 2011 11:12:55 -0500 Subject: [PATCH] Bruce Hellstrom @ ATC Flight Sim. New module contributed: AV400WSim. Supports communication with external (aka real) Garmin 400/500 WAAS flight sim units. Includes changes to options.cxx and fg_io.cxx to support invoking and configuring the new module. --- src/Main/fg_io.cxx | 20 +- src/Main/options.cxx | 2 + src/Network/AV400WSim.cxx | 1046 +++++++++++++++++++++++++++++++++++++ src/Network/AV400WSim.hxx | 144 +++++ src/Network/Makefile.am | 3 +- 5 files changed, 1213 insertions(+), 2 deletions(-) create mode 100644 src/Network/AV400WSim.cxx create mode 100644 src/Network/AV400WSim.hxx diff --git a/src/Main/fg_io.cxx b/src/Main/fg_io.cxx index 47c302f87..6d6be65f5 100644 --- a/src/Main/fg_io.cxx +++ b/src/Main/fg_io.cxx @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #ifdef FG_JPEG_SERVER @@ -142,7 +143,13 @@ FGIO::parse_port_config( const string& config ) } else if ( protocol == "AV400Sim" ) { FGAV400Sim *av400sim = new FGAV400Sim; io = av400sim; - } else if ( protocol == "garmin" ) { + } else if ( protocol == "AV400WSimA" ) { + FGAV400WSimA *av400wsima = new FGAV400WSimA; + io = av400wsima; + } else if ( protocol == "AV400WSimB" ) { + FGAV400WSimB *av400wsimb = new FGAV400WSimB; + io = av400wsimb; + } else if ( protocol == "garmin" ) { FGGarmin *garmin = new FGGarmin; io = garmin; } else if ( protocol == "httpd" ) { @@ -263,6 +270,17 @@ FGIO::parse_port_config( const string& config ) SGSerial *ch = new SGSerial( device, baud ); io->set_io_channel( ch ); + + if ( protocol == "AV400WSimB" ) { + if ( tokens.size() < 7 ) { + SG_LOG( SG_IO, SG_ALERT, "Missing second hz for AV400WSimB."); + return NULL; + } + FGAV400WSimB *fgavb = static_cast(io); + string hz2_str = tokens[6]; + double hz2 = atof(hz2_str.c_str()); + fgavb->set_hz2(hz2); + } } else if ( medium == "file" ) { // file name if ( tokens.size() < 4) { diff --git a/src/Main/options.cxx b/src/Main/options.cxx index 99ecbf43b..d61ffcf02 100644 --- a/src/Main/options.cxx +++ b/src/Main/options.cxx @@ -1467,6 +1467,8 @@ struct OptionDesc { {"opengc", true, OPTION_CHANNEL, "", false, "", 0 }, {"AV400", true, OPTION_CHANNEL, "", false, "", 0 }, {"AV400Sim", true, OPTION_CHANNEL, "", false, "", 0 }, + {"AV400WSimA", true, OPTION_CHANNEL, "", false, "", 0 }, + {"AV400WSimB", true, OPTION_CHANNEL, "", false, "", 0 }, {"garmin", true, OPTION_CHANNEL, "", false, "", 0 }, {"nmea", true, OPTION_CHANNEL, "", false, "", 0 }, {"generic", true, OPTION_CHANNEL, "", false, "", 0 }, diff --git a/src/Network/AV400WSim.cxx b/src/Network/AV400WSim.cxx new file mode 100644 index 000000000..83037dd46 --- /dev/null +++ b/src/Network/AV400WSim.cxx @@ -0,0 +1,1046 @@ +// AV400WSim.cxx -- Garmin 400 series protocal class. This AV400WSim +// protocol generates the set of "simulator" commands a garmin 400 WAAS +// series gps would expect as input in simulator mode. The AV400W +// protocol parses the set of commands that a garmin 400W series gps +// would emit. +// +// The Garmin WAAS GPS uses 2 serial channels to communicate with the +// simulator. These 2 channels are represented by the FGAV400WSimA and +// the FGAV400WSimB classes. The "A" channel is similar to the previous +// AVSim400 protocol. The "B" channel is considered the "GPS" channel and +// uses a different protocol than the "A" channel. The GPS unit expects +// input on the "B" channel at two different frequencies (1hz and 5hz, +// normally). The "B" channel also expects responses to certain output +// messages. +// +// Original AV400Sim code Written by Curtis Olson, started Janauary 2009. +// This AV400W code written by Bruce Hellstrom, March 2011. +// +// Copyright (C) 2009 Curtis L. Olson - http://www.flightgear.org/~curt +// Copyright (c) 2011 Bruce Hellstrom - http://www.celebritycc.com +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +// +// +// $Id$ + + +#ifdef HAVE_CONFIG_H +# include "config.h" +#endif + +#include +#include +#include +#include + +#include +#include
+#include
+ +#include "AV400WSim.hxx" + +FGAV400WSimA::FGAV400WSimA() { +} + +FGAV400WSimA::~FGAV400WSimA() { +} + + +// generate AV400WSimA message +bool FGAV400WSimA::gen_message() { + // cout << "generating garmin message" << endl; + + char msg_h[32], msg_i[32], msg_j[32], msg_k[32], msg_l[32]; + //char msg_type2[256]; + + double alt; + + // create msg_h + double obs = fgGetDouble( "/instrumentation/nav[0]/radials/selected-deg" ); + sprintf( msg_h, "h%04d\r\n", (int)(obs*10) ); + + // create msg_i + double fuel = fgGetDouble( "/consumables/fuel/total-fuel-gals" ); + if ( fuel > 999.9 ) { fuel = 999.9; } + sprintf( msg_i, "i%04.0f\r\n", fuel*10.0 ); + + // create msg_j + double gph = fgGetDouble( "/engines/engine[0]/fuel-flow-gph" ); + gph += fgGetDouble( "/engines/engine[1]/fuel-flow-gph" ); + gph += fgGetDouble( "/engines/engine[2]/fuel-flow-gph" ); + gph += fgGetDouble( "/engines/engine[3]/fuel-flow-gph" ); + if ( gph > 999.9 ) { gph = 999.9; } + sprintf( msg_j, "j%04.0f\r\n", gph*10.0 ); + + // create msg_k + sprintf( msg_k, "k%04d%02d%02d%02d%02d%02d\r\n", + fgGetInt( "/sim/time/utc/year"), + fgGetInt( "/sim/time/utc/month"), + fgGetInt( "/sim/time/utc/day"), + fgGetInt( "/sim/time/utc/hour"), + fgGetInt( "/sim/time/utc/minute"), + fgGetInt( "/sim/time/utc/second") ); + + // create msg_l + alt = fgGetDouble( "/instrumentation/pressure-alt-ft" ); + if ( alt > 99999.0 ) { alt = 99999.0; } + sprintf( msg_l, "l%05.0f\r\n", alt ); + + // sentence type 2 + //sprintf( msg_type2, "w01%c\r\n", (char)65 ); + + // assemble message + string sentence; + sentence += '\002'; // STX + sentence += msg_h; // obs heading in deg (*10) + sentence += msg_i; // total fuel in gal (*10) + sentence += msg_j; // fuel flow gph (*10) + sentence += msg_k; // date/time (UTC) + sentence += msg_l; // pressure altitude + //sentence += msg_type2; // type2 message + sentence += '\003'; // ETX + + // cout << sentence; + length = sentence.length(); + // cout << endl << "length = " << length << endl; + strncpy( buf, sentence.c_str(), length ); + + return true; +} + + +// parse AV400SimA message +bool FGAV400WSimA::parse_message() { + SG_LOG( SG_IO, SG_INFO, "parse AV400WSimA message" ); + + string msg = buf; + msg = msg.substr( 0, length ); + SG_LOG( SG_IO, SG_INFO, "entire message = " << msg ); + + string ident = msg.substr(0, 1); + if ( ident == "i" ) { + string side = msg.substr(1,1); + string num = msg.substr(2,3); + if ( side == "-" ) { + fgSetDouble("/instrumentation/av400w/cdi-deflection", 0.0); + } + else { + int pos = atoi(num.c_str()); + if ( side == "L" ) { + pos *= -1; + } + fgSetDouble("/instrumentation/av400w/cdi-deflection", + (double)pos / 10.0); + //printf( "i, %s%s, %f\n", side.c_str(), num.c_str(), (double)(pos / 10.0) ); + } + } + else if ( ident == "j" ) { + string side = msg.substr(1,1); + string num = msg.substr(2,3); + if ( side == "-" ) { + fgSetDouble("/instrumentation/av400w/gs-deflection", 0.0); + } + else { + int pos = atoi(num.c_str()); + if ( side == "B" ) { + pos *= -1; + } + // convert glideslope to -3.5 to 3.5 + fgSetDouble("/instrumentation/av400w/gs-deflection", + (double)pos / 28.57); + //printf( "j, %s%s, %f\n", side.c_str(), num.c_str(), (double)(pos / 28.57) ); + } + } + else if ( ident == "k" ) { + string ind = msg.substr(1,1); + if ( ind == "T" ) { + fgSetBool("/instrumentation/av400w/to-flag", true); + fgSetBool("/instrumentation/av400w/from-flag", false); + //printf( "set to-flag\n" ); + } else if ( ind == "F" ) { + fgSetBool("/instrumentation/av400w/to-flag", false); + fgSetBool("/instrumentation/av400w/from-flag", true); + //printf( "set from flag\n" ); + } else { + fgSetBool("/instrumentation/av400w/to-flag", false); + fgSetBool("/instrumentation/av400w/from-flag", false); + //printf( "set t/f both false\n" ); + } + //printf( "k, %s\n", ind.c_str() ); + } + else if ( ident == "S" ) { + string ind = msg.substr(1,5); + //printf( "S - %s\n", ind.c_str() ); + } + else { + // SG_LOG( SG_IO, SG_ALERT, "unknown AV400Sim message = " << msg ); + } + + return true; +} + + +// open hailing frequencies +bool FGAV400WSimA::open() { + if ( is_enabled() ) { + SG_LOG( SG_IO, SG_ALERT, "This shouldn't happen, but the channel " + << "is already in use, ignoring" ); + return false; + } + + SGIOChannel *io = get_io_channel(); + + if ( ! io->open( get_direction() ) ) { + SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." ); + return false; + } + + set_enabled( true ); + + return true; +} + + +// process work for this port +bool FGAV400WSimA::process() { + SGIOChannel *io = get_io_channel(); + + // until we have parsers/generators for the reverse direction, + // this is hardwired to expect that the physical GPS is slaving + // from FlightGear. + + // Send FlightGear data to the external device + gen_message(); + if ( ! io->write( buf, length ) ) { + SG_LOG( SG_IO, SG_WARN, "Error writing data." ); + return false; + } + + // read the device messages back + while ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) { + // SG_LOG( SG_IO, SG_ALERT, "Success reading data." ); + if ( parse_message() ) { + // SG_LOG( SG_IO, SG_ALERT, "Success parsing data." ); + } else { + // SG_LOG( SG_IO, SG_ALERT, "Error parsing data." ); + } + } + + return true; +} + + +// close the channel +bool FGAV400WSimA::close() { + SGIOChannel *io = get_io_channel(); + + set_enabled( false ); + + if ( ! io->close() ) { + return false; + } + + return true; +} + +// Start of FGAV400WSimB class methods +FGAV400WSimB::FGAV400WSimB() : +hz2(0.0), +hz2count(0), +hz2cycles(0), +flight_phase(0xFF), +req_hostid(true), +req_raimap(false), +req_sbas(false) +{ + hal.clear(); + val.clear(); + hal.append( "\0\0", 2 ); + val.append( "\0\0", 2 ); + outputctr = 0; + sbas_sel.append( "\0\x01", 2 ); + fdm = new FlightProperties; +} + +FGAV400WSimB::~FGAV400WSimB() { + delete fdm; +} + + +bool FGAV400WSimB::gen_hostid_message() { + char chksum = 0; + string data = "Cj\r\n"; + data += "COPYRIGHT 2008 GARMIN LTD. \r\n"; + data += "SFTW P/N # 006-B0339-0A\r\n"; + data += "SOFTWARE VER # 3\r\n"; + data += "SOFTWARE REV # 2\r\n"; + data += "SOFTWARE DATE 11/03/2008\r\n"; + data += "SW CRC 8F5E7DD1 AE5D4563\r\n"; + data += "HDWR P/N # 012-00857-01 \r\n"; + data += "SERIAL # 085701214976140\r\n"; + data += "MANUFACTUR DATE 02/26/2007\r\n"; + data += "OPTIONS LIST iiiiiiiiii"; + + // calculate the checksum + for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) { + chksum ^= *cli; + } + + string sentence( "@@" ); + sentence += data; + sentence.push_back( chksum ); + sentence += "\x0D\n"; + + length = sentence.length(); + char *bufptr = buf; + for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) { + *bufptr++ = *cli; + } + + return true; +} + +bool FGAV400WSimB::gen_sbas_message() { + char chksum = 0; + string data = "WA"; + data.push_back( '\0' ); + data += sbas_sel; + + // calculate the checksum + for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) { + chksum ^= *cli; + } + + string sentence( "@@" ); + sentence += data; + sentence.push_back( chksum ); + sentence += "\x0D\n"; + + length = sentence.length(); + char *bufptr = buf; + for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) { + *bufptr++ = *cli; + } + + return true; +} + +// Wh - Visible SBAS Satellites (hz2) +bool FGAV400WSimB::gen_Wh_message() { + char chksum = 0; + + // generate the Wh message + string data = "Wh"; + data.push_back( '\x0F' ); + data.append( "\x3f\x00\x00\x20\x00\x20", 6 ); + data.append( "\x4f\x00\x00\x28\x00\x30", 6 ); + data.append( "\x2d\x00\x00\x48\x01\x05", 6 ); + data.append( "\x1d\x00\x00\x10\x01\x10", 6 ); + data.append( "\x50\x00\x00\x33\x00\x50", 6 ); + data.append( "\x22\x00\x00\x16\x00\x90", 6 ); + data.append( "\x40\x00\x00\x20\x00\x20", 6 ); + data.append( "\x50\x00\x00\x28\x00\x30", 6 ); + data.append( "\x2e\x00\x00\x48\x01\x05", 6 ); + data.append( "\x1e\x00\x00\x10\x01\x10", 6 ); + data.append( "\x51\x00\x00\x33\x00\x50", 6 ); + data.append( "\x23\x00\x00\x16\x00\x90", 6 ); + data.append( "\x1f\x00\x00\x10\x01\x10", 6 ); + data.append( "\x52\x00\x00\x33\x00\x50", 6 ); + data.append( "\x24\x00\x00\x16\x00\x90", 6 ); + data.push_back( '0' ); + + // calculate the checksum + for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) { + chksum ^= *cli; + } + + string sentence( "@@" ); + sentence += data; + sentence.push_back( chksum ); + sentence += "\x0D\n"; + + length = sentence.length(); + char *bufptr = buf; + for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) { + *bufptr++ = *cli; + } + + return true; +} + + +// Wx - Channel Status Message (hz2) +bool FGAV400WSimB::gen_Wx_message() { + char chksum = 0; + + // Now process the Wx message + string data = "Wx"; + data.push_back( (char)( fgGetInt( "/sim/time/utc/month") & 0xFF ) ); + data.push_back( (char)( fgGetInt( "/sim/time/utc/day") & 0xFF ) ); + data.push_back( (char)( (fgGetInt( "/sim/time/utc/year") >> 8 ) & 0xFF ) ); + data.push_back( (char)( fgGetInt( "/sim/time/utc/year") & 0xFF ) ); + data.push_back( (char)( fgGetInt( "/sim/time/utc/hour") & 0xFF ) ); + data.push_back( (char)( fgGetInt( "/sim/time/utc/minute") & 0xFF ) ); + data.push_back( (char)( fgGetInt( "/sim/time/utc/second") & 0xFF ) ); + data.append( "\x00\x00\x00\x00", 4 ); + + for ( int xctr = 0; xctr < 15; xctr++ ) { + data.append( "\x00\x00\x00\x00\x00\x00\x00\x00", 8 ); + } + data.push_back( '\0' ); + + // calculate the checksum + for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) { + chksum ^= *cli; + } + + string sentence( "@@" ); + sentence += data; + sentence.push_back( chksum ); + sentence += "\x0D\n"; + + // cout << sentence; + length = sentence.length(); + char *bufptr = buf; + for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) { + *bufptr++ = *cli; + } + + return true; +} + + +// Wt - Position and Navigation status +bool FGAV400WSimB::gen_Wt_message() { + char chksum = 0; + + // generate the Wt message + string data = "Wt"; + data.push_back( (char)( fgGetInt( "/sim/time/utc/month") & 0xFF ) ); + data.push_back( (char)( fgGetInt( "/sim/time/utc/day") & 0xFF ) ); + data.push_back( (char)( (fgGetInt( "/sim/time/utc/year") >> 8 ) & 0xFF ) ); + data.push_back( (char)( fgGetInt( "/sim/time/utc/year") & 0xFF ) ); + data.push_back( (char)( fgGetInt( "/sim/time/utc/hour") & 0xFF ) ); + data.push_back( (char)( fgGetInt( "/sim/time/utc/minute") & 0xFF ) ); + data.push_back( (char)( fgGetInt( "/sim/time/utc/second") & 0xFF ) ); + data.append( "\x00\x00\x00\x00", 4 ); + + // get latitude in milliarcseconds + double latd = fdm->get_Latitude() * SGD_RADIANS_TO_DEGREES; + latd *= DEG_TO_MILLIARCSECS; + int latitude = (int)latd; + data.push_back( (char)( ( latitude >> 24 ) & 0xFF ) ); + data.push_back( (char)( ( latitude >> 16 ) & 0xFF ) ); + data.push_back( (char)( ( latitude >> 8 ) & 0xFF ) ); + data.push_back( (char)( latitude & 0xFF ) ); + + // get longitude in milliarcseconds + double lond = fdm->get_Longitude() * SGD_RADIANS_TO_DEGREES; + lond *= DEG_TO_MILLIARCSECS; + int longitude = (int)lond; + data.push_back( (char)( ( longitude >> 24 ) & 0xFF ) ); + data.push_back( (char)( ( longitude >> 16 ) & 0xFF ) ); + data.push_back( (char)( ( longitude >> 8 ) & 0xFF ) ); + data.push_back( (char)( longitude & 0xFF ) ); + + + // Altitude settings + double alt = fdm->get_Altitude(); + if ( alt > 99999.0 ) { alt = 99999.0; } + + // send the WGS-84 ellipsoid height om /-1, (just use regular altitude) + alt *= SG_FEET_TO_METER; + int altm = (int)( alt * 100.0f ); + data.push_back( (char)( ( altm >> 24 ) & 0xFF ) ); + data.push_back( (char)( ( altm >> 16 ) & 0xFF ) ); + data.push_back( (char)( ( altm >> 8 ) & 0xFF ) ); + data.push_back( (char)( altm & 0xFF ) ); + + // put in the geoid height in 0.1 meters + data.push_back( (char)( ( altm >> 24 ) & 0xFF ) ); + data.push_back( (char)( ( altm >> 16 ) & 0xFF ) ); + data.push_back( (char)( ( altm >> 8 ) & 0xFF ) ); + data.push_back( (char)( altm & 0xFF ) ); + + // get ground speed + double gskt = fgGetDouble( "/velocities/groundspeed-kt" ); + gskt *= SG_KT_TO_MPS; + int gsm = (int)( gskt * 100.0f ); + data.push_back( (char)( ( gsm >> 8 ) & 0xFF ) ); + data.push_back( (char)( gsm & 0xFF ) ); + + // ground track + double trkdeg = fgGetDouble("/orientation/heading-deg"); + int hdg = (int)(trkdeg * 10.0f); + data.push_back( (char)( ( hdg >> 8 ) & 0xFF ) ); + data.push_back( (char)( hdg & 0xFF ) ); + + // vertical velocity + double climb_fpm = fgGetDouble( "/velocities/vertical-speed-fps" ); + climb_fpm *= SG_FEET_TO_METER; + int vvm = (int)( climb_fpm * 50.0f ); + data.push_back( (char)( ( vvm >> 8 ) & 0xFF ) ); + data.push_back( (char)( vvm & 0xFF ) ); + + // navigation solution status + data.push_back( '\0' ); + + // HFOM/VFOM + data.append( "\0\x09\0\x09", 4 ); + + // ARINC 748 Mode + data.push_back( '\x0D' ); + + // Channel Tracking + data += "\x7F\xFF"; + + // calculate the checksum + for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) { + chksum ^= *cli; + } + + string sentence( "@@" ); + sentence += data; + sentence.push_back( chksum ); + sentence += "\x0D\n"; + + length = sentence.length(); + char *bufptr = buf; + for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) { + *bufptr++ = *cli; + } + + return true; +} + + +// Wm - Data integrity status +bool FGAV400WSimB::gen_Wm_message() { + char chksum = 0; + + // generate the Wt message + string data = "Wm"; + + // flight phase + data.push_back( flight_phase ); + + // HAL and VAL + if ( hal.empty() ) { + data.append( "\0\0", 2 ); + } + else { + data += hal; + } + + if ( val.empty() ) { + data.append( "\0\0", 2 ); + } + else { + data += val; + } + + // Integrity status + data.append( "\x00\x00\x00", 3 ); + data.append( "\x00\x01\x00\x01\x00\x01\x00\x01", 8 ); + data.append( "\x00\x0F\x00\x0F\x00\x0F", 6 ); + + // calculate the checksum + for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) { + chksum ^= *cli; + } + + string sentence( "@@" ); + sentence += data; + sentence.push_back( chksum ); + sentence += "\x0D\n"; + + length = sentence.length(); + char *bufptr = buf; + for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) { + *bufptr++ = *cli; + } + + return true; +} + +// Wv - 3d velocity +bool FGAV400WSimB::gen_Wv_message() { + char chksum = 0; + + // generate the Wt message + string data = "Wv"; + + // data is valid + data += "1"; + + // N velocity in .01 m/s + double vn_mps = fgGetDouble( "/velocities/speed-north-fps" ) * SG_FEET_TO_METER; + int vnm = (int)( vn_mps * 100 ); + data.push_back( (char)( ( vnm >> 24 ) & 0xFF ) ); + data.push_back( (char)( ( vnm >> 16 ) & 0xFF ) ); + data.push_back( (char)( ( vnm >> 8 ) & 0xFF ) ); + data.push_back( (char)( vnm & 0xFF ) ); + + // E velocity in .01 m/s + double ve_mps = fgGetDouble( "/velocities/speed-east-fps" ) * SG_FEET_TO_METER; + int vne = (int)( ve_mps * 100 ); + data.push_back( (char)( ( vne >> 24 ) & 0xFF ) ); + data.push_back( (char)( ( vne >> 16 ) & 0xFF ) ); + data.push_back( (char)( ( vne >> 8 ) & 0xFF ) ); + data.push_back( (char)( vne & 0xFF ) ); + + // Up velocity in .01 m/s + double climb_mps = fgGetDouble( "/velocities/vertical-speed-fps" ) * SG_FEET_TO_METER; + int vnup = (int)( climb_mps * 100 ); + data.push_back( (char)( ( vnup >> 24 ) & 0xFF ) ); + data.push_back( (char)( ( vnup >> 16 ) & 0xFF ) ); + data.push_back( (char)( ( vnup >> 8 ) & 0xFF ) ); + data.push_back( (char)( vnup & 0xFF ) ); + + // calculate the checksum + for ( string::const_iterator cli = data.begin(); cli != data.end(); cli++ ) { + chksum ^= *cli; + } + + string sentence( "@@" ); + sentence += data; + sentence.push_back( chksum ); + sentence += "\x0D\n"; + + // cout << sentence; + length = sentence.length(); + char *bufptr = buf; + for ( string::const_iterator cli = sentence.begin(); cli != sentence.end(); cli++ ) { + *bufptr++ = *cli; + } + + return true; +} + + +bool FGAV400WSimB::verify_checksum( string message, int datachars ) { + bool bRet = false; + string dataseg = message.substr(SOM_SIZE, datachars); + char chksum = 0; + char cs = message[SOM_SIZE + datachars]; + for ( string::const_iterator cli = dataseg.begin(); + cli != dataseg.end(); cli++ ) { + chksum ^= *cli; + } + + if ( chksum == cs ) { + bRet = true; + } + else { + SG_LOG( SG_IO, SG_INFO, "bad input checksum: " << message ); + //string msgid = asciitize_message( message ); + //printf( "FGAV400SimB::verify_checksum bad input checksum:\n%s\n", msgid.c_str() ); + } + + return( bRet ); +} + + +string FGAV400WSimB::asciitize_message( string message ) { + string asciimsg; + + for ( string::const_iterator cli = message.begin(); + cli != message.end(); cli++ ) { + if ( *cli >= 32 && *cli <= 127 ) { + asciimsg += *cli; + } + else { + char tempbuf[20]; + sprintf( tempbuf, "\\x%02X", (unsigned char)(*cli) ); + asciimsg += tempbuf; + } + } + + return( asciimsg ); +} + +string FGAV400WSimB::buffer_to_string() { + string message; + char *bufctr = buf; + + for ( int xctr = 0; xctr < length; xctr++ ) { + message.push_back( *bufctr++ ); + } + return( message ); +} + + +// parse AV400Sim message +bool FGAV400WSimB::parse_message() { + SG_LOG( SG_IO, SG_INFO, "parse AV400WSimB message" ); + + string msg = buffer_to_string(); + + string som = msg.substr(0, 2); + if ( som != "@@" ) { + SG_LOG( SG_IO, SG_INFO, "bad start message" ); + return false; + } + + string ident = msg.substr(2,2); + + if ( ident == "AH" ) { // Flight Phase + if ( verify_checksum( msg, 3 ) ) { + flight_phase = msg[4]; + //string ascmsg = asciitize_message( msg ); + //printf( "%10d received AH %s\n", outputctr, ascmsg.c_str() ); + switch( flight_phase ) { + case FGAV400WSimB::PHASE_OCEANIC: // Oceanic + if ( hal.empty() ) { + hal = "\x39\xE0"; + } + if ( val.empty() ) { + val = "\x00\x00"; + } + fgSetBool( "/instrumentation/av400w/has-gs", false ); + break; + + case PHASE_ENROUTE: // Enroute + if ( hal.empty() ) { + hal = "\x1C\xF0"; + } + if ( val.empty() ) { + val = "\x00\x00"; + } + fgSetBool( "/instrumentation/av400w/has-gs", false ); + break; + + case PHASE_TERM: // Terminal + if ( hal.empty() ) { + hal = "\x0E\x78"; + } + if ( val.empty() ) { + val = "\x00\x00"; + } + fgSetBool( "/instrumentation/av400w/has-gs", false ); + break; + + case PHASE_NONPREC: // Non Precision Approach + if ( hal.empty() ) { + hal = "\x04\x57"; + } + if ( val.empty() ) { + val = "\x00\x00"; + } + fgSetBool( "/instrumentation/av400w/has-gs", false ); + break; + + case PHASE_LNAVVNAV: // LNAV/VNAV + if ( hal.empty() ) { + hal = "\x04\x57"; + } + if ( val.empty() ) { + val = "\x00\x64"; + } + fgSetBool( "/instrumentation/av400w/has-gs", true ); + break; + + case PHASE_LPVLP: // LPV/LP + if ( hal.empty() ) { + hal = "\x00\x00"; + } + if ( val.empty() ) { + val = "\x00\x00"; + } + fgSetBool( "/instrumentation/av400w/has-gs", true ); + break; + + default: + if ( hal.empty() ) { + hal = "\x00\x00"; + } + if ( val.empty() ) { + val = "\x00\x00"; + } + fgSetBool( "/instrumentation/av400w/has-gs", false ); + break; + } + //printf( "AH flight status: %c\n", flight_phase + '0' ); + } + } + else if ( ident == "AI" ) { // HAL + if ( verify_checksum( msg, 4 ) ) { + hal = msg.substr(4,2); + //printf( "%10d received AI\n", outputctr ); + } + } + else if ( ident == "Cj" ) { // Host ID + if ( verify_checksum( msg, 2 ) ) { + req_hostid = true; + //printf( "%10d received Cj\n", outputctr ); + } + } + else if ( ident == "WA" ) { // SBAS selection + if ( verify_checksum( msg, 5 ) ) { + sbas_sel = msg.substr( 5, 2 ); + req_sbas = true; + //printf( "%10d received WA\n", outputctr ); + } + } + else if ( ident == "Wd" ) { // VAL + if ( verify_checksum( msg, 4 ) ) { + val = msg.substr( 4, 2 ); + //printf( "%10d received Wd\n", outputctr ); + } + } + else if ( ident == "WY" ) { // ???? Not listed in protocol document + // Do nothing until we know what it does + } + else { + string unkmsg = msg.substr( 0, 4 ); + printf( "parse_message unknown: %s\n", unkmsg.c_str() ); + } + + return true; +} + + +// open hailing frequencies +bool FGAV400WSimB::open() { + if ( is_enabled() ) { + SG_LOG( SG_IO, SG_ALERT, "This shouldn't happen, but the channel " + << "is already in use, ignoring" ); + return false; + } + + SGIOChannel *io = get_io_channel(); + + if ( ! io->open( get_direction() ) ) { + SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." ); + return false; + } + + set_enabled( true ); + + return true; +} + + +// process work for this port +bool FGAV400WSimB::process() { + SGIOChannel *io = get_io_channel(); + + // read the device messages back + // Because the protocol allows for binary data, we can't just read + // ascii lines. + char readbuf[10]; + char *bufptr = buf; + int templen; + bool gotCr = false; + bool gotLf = false; + bool som1 = false; + bool som2 = false; + length = 0; + + while ( ( templen = io->read( readbuf, 1 ) ) == 1 ) { + if ( !som1 && !som2 ) { + if ( *readbuf == '@' ) { + som1 = true; + } + else { + continue; + } + } + else if ( !som2 ) { + if ( *readbuf == '@' ) { + som2 = true; + } + else { + som1 = false; + continue; + } + } + else if ( som1 && som2 ) { + if ( *readbuf == '\n' && !gotCr ) { // check for a carriage return + gotCr = true; + } + else if ( *readbuf == '\n' && gotCr ) { // see if we got a cr/lf + gotLf = true; + } + else if ( gotCr ) { // we had a cr but the next char was not a lf, so just must be data + gotCr = false; + } + } + + *bufptr++ = *readbuf; + length++; + + if ( gotCr && gotLf ) { // message done + if ( parse_message() ) { + // SG_LOG( SG_IO, SG_ALERT, "Success parsing data." ); + } else { + // SG_LOG( SG_IO, SG_ALERT, "Error parsing data." ); + } + length = 0; + break; + } + } + + + // Check for polled messages + if ( req_hostid ) { + gen_hostid_message(); + if ( ! io->write( buf, length ) ) { + SG_LOG( SG_IO, SG_WARN, "Error writing data." ); + printf( "Error sending HostID\n" ); + return false; + } + //printf( "Sent HostID, %d bytes\n", length ); + req_hostid = false; + } + else if ( req_sbas ) { + gen_sbas_message(); + if ( ! io->write( buf, length ) ) { + SG_LOG( SG_IO, SG_WARN, "Error writing data." ); + printf( "Error sending SBAS\n" ); + return false; + } + //printf( "Sent SBAS, %d bytes\n", length ); + req_sbas = false; + } + + // Send the 5Hz messages + gen_Wt_message(); + if ( ! io->write( buf, length ) ) { + SG_LOG( SG_IO, SG_WARN, "Error writing data." ); + printf( "Error writing hz message\n" ); + return false; + } + //printf( "Sent Wt, %d bytes\n", length ); + + gen_Wm_message(); + if ( ! io->write( buf, length ) ) { + SG_LOG( SG_IO, SG_WARN, "Error writing data." ); + printf( "Error writing hz message\n" ); + return false; + } + //printf( "Sent Wm, %d bytes\n", length ); + + gen_Wv_message(); + if ( ! io->write( buf, length ) ) { + SG_LOG( SG_IO, SG_WARN, "Error writing data." ); + printf( "Error writing hz message\n" ); + return false; + } + //printf( "Sent Wv, %d bytes\n", length ); + + hz2count++; + if ( hz2 > 0 && ( hz2count % hz2cycles == 0 ) ) { + // Send the 1Hz messages + gen_Wh_message(); + if ( ! io->write( buf, length ) ) { + SG_LOG( SG_IO, SG_WARN, "Error writing data." ); + printf( "Error writing hz2 message\n" ); + return false; + } + //printf( "Sent Wh, %d bytes\n", length ); + + gen_Wx_message(); + if ( ! io->write( buf, length ) ) { + SG_LOG( SG_IO, SG_WARN, "Error writing data." ); + printf( "Error writing hz2 message\n" ); + return false; + } + //printf( "Sent Wx, %d bytes\n", length ); + } + + // read the device messages back again to make sure we don't miss anything + bufptr = buf; + templen = 0; + gotCr = false; + gotLf = false; + som1 = false; + som2 = false; + length = 0; + + while ( ( templen = io->read( readbuf, 1 ) ) == 1 ) { + if ( !som1 && !som2 ) { + if ( *readbuf == '@' ) { + som1 = true; + } + else { + continue; + } + } + else if ( !som2 ) { + if ( *readbuf == '@' ) { + som2 = true; + } + else { + som1 = false; + continue; + } + } + else if ( som1 && som2 ) { + if ( *readbuf == '\n' && !gotCr ) { // check for a carriage return + gotCr = true; + } + else if ( *readbuf == '\n' && gotCr ) { // see if we got a cr/lf + gotLf = true; + } + else if ( gotCr ) { // we had a cr but the next char was not a lf, so just must be data + gotCr = false; + } + } + + *bufptr++ = *readbuf; + length++; + + if ( gotCr && gotLf ) { // message done + //string msg = buffer_to_string(); + //string ascmsg = asciitize_message( msg ); + //printf( "Received message\n" ); + //printf( "%s\n", ascmsg.c_str() ); + //printf( "got message\n" ); + if ( parse_message() ) { + // SG_LOG( SG_IO, SG_ALERT, "Success parsing data." ); + } else { + // SG_LOG( SG_IO, SG_ALERT, "Error parsing data." ); + } + length = 0; + break; + } + } + + + outputctr++; + if ( outputctr % 10 == 0 ) { + //printf( "AV400WSimB::process finished\n" ); + } + + return true; +} + + +// close the channel +bool FGAV400WSimB::close() { + SGIOChannel *io = get_io_channel(); + + set_enabled( false ); + + if ( ! io->close() ) { + return false; + } + + return true; +} + + diff --git a/src/Network/AV400WSim.hxx b/src/Network/AV400WSim.hxx new file mode 100644 index 000000000..3ff7a4190 --- /dev/null +++ b/src/Network/AV400WSim.hxx @@ -0,0 +1,144 @@ +// AV400Sim.hxx -- Garmin 400 series protocal class. This AV400Sim +// protocol generates the set of "simulator" commands a garmin 400 +// series gps would expect as input in simulator mode. The AV400 +// protocol generates the set of commands that a garmin 400 series gps +// would emit. +// +// Written by Curtis Olson, started Januar 2009. +// +// Copyright (C) 2009 Curtis L. Olson - http://www.flightgear.org/~curt +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +// +// $Id$ + + +#ifndef _FG_AV400WSIM_HXX +#define _FG_AV400WSIM_HXX + + +#include +#include +#include + +#include + +#include "protocol.hxx" + +using std::string; + +class FlightProperties; + +////////////////////////////////////////////////////////////////////////////// +// Class FGAV400WSimA handles the input/output over the first serial port. +// This is very similar to the way previous Garmin non-WAAS models communicated +// but some items have been stripped out and just a minimal amount of +// info is necessary to be transmitted over this port. + +class FGAV400WSimA : public FGProtocol { + + char buf[ FG_MAX_MSG_SIZE ]; + int length; + +public: + + FGAV400WSimA(); + ~FGAV400WSimA(); + + bool gen_message(); + bool parse_message(); + + // open hailing frequencies + bool open(); + + // process work for this port + bool process(); + + // close the channel + bool close(); +}; + +////////////////////////////////////////////////////////////////////////////// +// Class FGAV400WSimB handles the input/output over the second serial port +// which Garmin refers to as the "GPS Port". Some messages are handled on +// fixed cycle (usually 1 and 5 Hz) and some immediate responses are needed +// to certain messages upon requet by the GPS unit + +class FGAV400WSimB : public FGProtocol { + + char buf[ FG_MAX_MSG_SIZE ]; + int length; + double hz2; + int hz2count; + int hz2cycles; + char flight_phase; + string hal; + string val; + string sbas_sel; + bool req_hostid; + bool req_raimap; + bool req_sbas; + int outputctr; + + FlightProperties* fdm; + + static const int SOM_SIZE = 2; + static const int DEG_TO_MILLIARCSECS = ( 60.0 * 60.0 * 1000 ); + + // Flight Phases + static const int PHASE_OCEANIC = 4; + static const int PHASE_ENROUTE = 5; + static const int PHASE_TERM = 6; + static const int PHASE_NONPREC = 7; + static const int PHASE_LNAVVNAV = 8; + static const int PHASE_LPVLP = 9; + +public: + + FGAV400WSimB(); + ~FGAV400WSimB(); + + bool gen_hostid_message(); + bool gen_sbas_message(); + + bool gen_Wh_message(); + bool gen_Wx_message(); + + bool gen_Wt_message(); + bool gen_Wm_message(); + bool gen_Wv_message(); + + bool verify_checksum( string message, int datachars ); + string asciitize_message( string message ); + string buffer_to_string(); + bool parse_message(); + + // open hailing frequencies + bool open(); + + // process work for this port + bool process(); + + // close the channel + bool close(); + + inline double get_hz2() const { return hz2; } + inline void set_hz2( double t ) { hz2 = t, hz2cycles = get_hz() / hz2; } + +}; + + + +#endif // _FG_AV400WSIM_HXX diff --git a/src/Network/Makefile.am b/src/Network/Makefile.am index 1f9b207c3..0935d3ca4 100644 --- a/src/Network/Makefile.am +++ b/src/Network/Makefile.am @@ -19,6 +19,7 @@ libNetwork_a_SOURCES = \ atlas.cxx atlas.hxx \ AV400.cxx AV400.hxx \ AV400Sim.cxx AV400Sim.hxx \ + AV400WSim.cxx AV400WSim.hxx \ garmin.cxx garmin.hxx \ lfsglass.cxx lfsglass.hxx lfsglass_data.hxx \ httpd.cxx httpd.hxx \ @@ -43,4 +44,4 @@ INCLUDES = -I$(top_srcdir) -I$(top_srcdir)/src if WITH_HLA SUBDIRS = HLA -endif \ No newline at end of file +endif