From 2d6f291d4f5d39344ec70c59f377b1e429da9ed1 Mon Sep 17 00:00:00 2001 From: curt Date: Sun, 20 Aug 2006 23:37:13 +0000 Subject: [PATCH] Add an initial implementation of the Garmin 400 series "Aviation In" data format. I have a Garmin 295 to test with, but so far I haven't been able to make this work (code should compile cleanly though.) I don't know if I've made a mistake in the protocol or if my 295 just doesn't support this. More work on this to come. --- src/Main/fg_io.cxx | 4 + src/Main/options.cxx | 3 +- src/Network/AV400.cxx | 442 ++++++++++++++++++++++++++++++++++++++++ src/Network/AV400.hxx | 61 ++++++ src/Network/Makefile.am | 1 + 5 files changed, 510 insertions(+), 1 deletion(-) create mode 100644 src/Network/AV400.cxx create mode 100644 src/Network/AV400.hxx diff --git a/src/Main/fg_io.cxx b/src/Main/fg_io.cxx index 5c1628dba..f65e952f5 100644 --- a/src/Main/fg_io.cxx +++ b/src/Main/fg_io.cxx @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #ifdef FG_JPEG_SERVER @@ -130,6 +131,9 @@ FGIO::parse_port_config( const string& config ) // printf("Parsed opengc\n"); cin >> wait; FGOpenGC *opengc = new FGOpenGC; io = opengc; + } else if ( protocol == "AV400" ) { + FGAV400 *av400 = new FGAV400; + io = av400; } else if ( protocol == "garmin" ) { FGGarmin *garmin = new FGGarmin; io = garmin; diff --git a/src/Main/options.cxx b/src/Main/options.cxx index fd5465d5b..fe4d35ad1 100644 --- a/src/Main/options.cxx +++ b/src/Main/options.cxx @@ -504,7 +504,7 @@ parse_fov( const string& arg ) { // // Format is "--protocol=medium,direction,hz,medium_options,..." // -// protocol = { native, nmea, garmin, fgfs, rul, pve, etc. } +// protocol = { native, nmea, garmin, AV400, fgfs, rul, pve, etc. } // medium = { serial, socket, file, etc. } // direction = { in, out, bi } // hz = number of times to process channel per second (floating @@ -1357,6 +1357,7 @@ struct OptionDesc { {"native-fdm", true, OPTION_CHANNEL, "", false, "", 0 }, {"native-gui", true, OPTION_CHANNEL, "", false, "", 0 }, {"opengc", true, OPTION_CHANNEL, "", false, "", 0 }, + {"AV400", 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/AV400.cxx b/src/Network/AV400.cxx new file mode 100644 index 000000000..ee7b7fa73 --- /dev/null +++ b/src/Network/AV400.cxx @@ -0,0 +1,442 @@ +// AV400.cxx -- Garmin 400 series protocal class +// +// Written by Curtis Olson, started August 2006. +// +// Copyright (C) 2006 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$ + + +#ifdef HAVE_CONFIG_H +# include "config.h" +#endif + +#include +#include +#include +#include + +#include +#include
+#include
+ +#include "AV400.hxx" + +SG_USING_NAMESPACE(std); + +FGAV400::FGAV400() { +} + +FGAV400::~FGAV400() { +} + + +// calculate the garmin check sum +static char calc_nmea_cksum(char *sentence) { + unsigned char sum = 0; + int i, len; + + // cout << sentence << endl; + + len = strlen(sentence); + sum = sentence[0]; + for ( i = 1; i < len; i++ ) { + // cout << sentence[i]; + sum ^= sentence[i]; + } + // cout << endl; + + // printf("sum = %02x\n", sum); + return sum; +} + + +// generate AV400 message +bool FGAV400::gen_message() { + // cout << "generating garmin message" << endl; + + char msg_z[32], msg_A[32], msg_B[32], msg_C[32], msg_D[32]; + char msg_Q[32], msg_T[32], msg_type2[256]; + // the following could be implemented, but currently are unused + // char msg_E[32], msg_G[32], msg_I[32], msg_K[32], msg_L[32], msg_S[32]; + // char msg_l[32]; + + char dir; + int deg; + double min; + + // create msg_z + sprintf( msg_z, "z%05.0f\r\n", cur_fdm_state->get_Altitude() ); + + // create msg_A + sprintf( msg_A, "A"); + + double latd = cur_fdm_state->get_Latitude() * SGD_RADIANS_TO_DEGREES; + if ( latd < 0.0 ) { + latd = -latd; + dir = 'S'; + } else { + dir = 'N'; + } + deg = (int)latd; + min = (latd - (double)deg) * 60.0 * 100.0; + sprintf( msg_A, "A%c %02d %04.0f\r\n", dir, deg, min); + + // create msg_B + double lond = cur_fdm_state->get_Longitude() * SGD_RADIANS_TO_DEGREES; + if ( lond < 0.0 ) { + lond = -lond; + dir = 'W'; + } else { + dir = 'E'; + } + deg = (int)lond; + min = (lond - (double)deg) * 60.0 * 100.0; + sprintf( msg_B, "B%c %03d %04.0f\r\n", dir, deg, min); + + // create msg_C + double vn = fgGetDouble( "/velocities/speed-north-fps" ); + double ve = fgGetDouble( "/velocities/speed-east-fps" ); + double gnd_trk_true = atan2( ve, vn ) * SGD_RADIANS_TO_DEGREES; + if ( gnd_trk_true < 0.0 ) { gnd_trk_true += 360.0; } + sprintf( msg_C, "C%03.0f\r\n", gnd_trk_true); + + // create msg_D + double speed_kt = sqrt( vn*vn + ve*ve ) * SG_FPS_TO_KT; + if ( speed_kt > 999.0 ) { + speed_kt = 999.0; + } + sprintf( msg_D, "D%03.0f\r\n", speed_kt); + + // create msg_E (not implemented) + // create msg_G (not implemented) + // create msg_I (not implemented) + // create msg_K (not implemented) + // create msg_L (not implemented) + + // create msg_Q + float magdeg = fgGetDouble( "/environment/magnetic-variation-deg" ); + if ( magdeg < 0.0 ) { + magdeg = -magdeg; + dir = 'W'; + } else { + dir = 'E'; + } + sprintf( msg_Q, "Q%c%03.0f\r\n", dir, magdeg * 10.0 ); + + // create msg_S (not implemented) + + // create msg_T + sprintf( msg_T, "T---------\r\n" ); + + // create msg_l (not implemented) + + // sentence type 2 + sprintf( msg_type2, "w01%c\r\n", (char)65 ); + + // assemble message + string sentence; + sentence += '\002'; // STX + sentence += msg_z; // altitude + sentence += msg_A; // latitude + sentence += msg_B; // longitude + sentence += msg_C; // ground track + sentence += msg_D; // ground speed (kt) + sentence += "E-----\r\n"; + sentence += "G-----\r\n"; + sentence += "I----\r\n"; + sentence += "K-----\r\n"; + sentence += "L----\r\n"; + sentence += msg_Q; // magvar + sentence += "S-----\r\n"; + sentence += msg_T; // end of type 1 messages (must be sent) + sentence += msg_type2; // type2 message + sentence += "l------\r\n"; + sentence += '\003'; // ETX + + cout << sentence; + + length = sentence.length(); + strncpy( buf, sentence.c_str(), length ); + + return true; +} + + +// parse AV400 message +bool FGAV400::parse_message() { + SG_LOG( SG_IO, SG_INFO, "parse garmin message" ); + + string msg = buf; + msg = msg.substr( 0, length ); + SG_LOG( SG_IO, SG_INFO, "entire message = " << msg ); + + string::size_type begin_line, end_line, begin, end; + begin_line = begin = 0; + + // extract out each line + end_line = msg.find("\n", begin_line); + while ( end_line != string::npos ) { + string line = msg.substr(begin_line, end_line - begin_line); + begin_line = end_line + 1; + SG_LOG( SG_IO, SG_INFO, " input line = " << line ); + + // leading character + string start = msg.substr(begin, 1); + ++begin; + SG_LOG( SG_IO, SG_INFO, " start = " << start ); + + // sentence + end = msg.find(",", begin); + if ( end == string::npos ) { + return false; + } + + string sentence = msg.substr(begin, end - begin); + begin = end + 1; + SG_LOG( SG_IO, SG_INFO, " sentence = " << sentence ); + + double lon_deg, lon_min, lat_deg, lat_min; + double lon, lat, speed, heading, altitude; + + if ( sentence == "GPRMC" ) { + // time + end = msg.find(",", begin); + if ( end == string::npos ) { + return false; + } + + string utc = msg.substr(begin, end - begin); + begin = end + 1; + SG_LOG( SG_IO, SG_INFO, " utc = " << utc ); + + // junk + end = msg.find(",", begin); + if ( end == string::npos ) { + return false; + } + + string junk = msg.substr(begin, end - begin); + begin = end + 1; + SG_LOG( SG_IO, SG_INFO, " junk = " << junk ); + + // lat val + end = msg.find(",", begin); + if ( end == string::npos ) { + return false; + } + + string lat_str = msg.substr(begin, end - begin); + begin = end + 1; + + lat_deg = atof( lat_str.substr(0, 2).c_str() ); + lat_min = atof( lat_str.substr(2).c_str() ); + + // lat dir + end = msg.find(",", begin); + if ( end == string::npos ) { + return false; + } + + string lat_dir = msg.substr(begin, end - begin); + begin = end + 1; + + lat = lat_deg + ( lat_min / 60.0 ); + if ( lat_dir == "S" ) { + lat *= -1; + } + + cur_fdm_state->set_Latitude( lat * SGD_DEGREES_TO_RADIANS ); + SG_LOG( SG_IO, SG_INFO, " lat = " << lat ); + + // lon val + end = msg.find(",", begin); + if ( end == string::npos ) { + return false; + } + + string lon_str = msg.substr(begin, end - begin); + begin = end + 1; + + lon_deg = atof( lon_str.substr(0, 3).c_str() ); + lon_min = atof( lon_str.substr(3).c_str() ); + + // lon dir + end = msg.find(",", begin); + if ( end == string::npos ) { + return false; + } + + string lon_dir = msg.substr(begin, end - begin); + begin = end + 1; + + lon = lon_deg + ( lon_min / 60.0 ); + if ( lon_dir == "W" ) { + lon *= -1; + } + + cur_fdm_state->set_Longitude( lon * SGD_DEGREES_TO_RADIANS ); + SG_LOG( SG_IO, SG_INFO, " lon = " << lon ); + +#if 0 + double sl_radius, lat_geoc; + sgGeodToGeoc( cur_fdm_state->get_Latitude(), + cur_fdm_state->get_Altitude(), + &sl_radius, &lat_geoc ); + cur_fdm_state->set_Geocentric_Position( lat_geoc, + cur_fdm_state->get_Longitude(), + sl_radius + cur_fdm_state->get_Altitude() ); +#endif + + // speed + end = msg.find(",", begin); + if ( end == string::npos ) { + return false; + } + + string speed_str = msg.substr(begin, end - begin); + begin = end + 1; + speed = atof( speed_str.c_str() ); + cur_fdm_state->set_V_calibrated_kts( speed ); + // cur_fdm_state->set_V_ground_speed( speed ); + SG_LOG( SG_IO, SG_INFO, " speed = " << speed ); + + // heading + end = msg.find(",", begin); + if ( end == string::npos ) { + return false; + } + + string hdg_str = msg.substr(begin, end - begin); + begin = end + 1; + heading = atof( hdg_str.c_str() ); + cur_fdm_state->set_Euler_Angles( cur_fdm_state->get_Phi(), + cur_fdm_state->get_Theta(), + heading * SGD_DEGREES_TO_RADIANS ); + SG_LOG( SG_IO, SG_INFO, " heading = " << heading ); + } else if ( sentence == "PGRMZ" ) { + // altitude + end = msg.find(",", begin); + if ( end == string::npos ) { + return false; + } + + string alt_str = msg.substr(begin, end - begin); + altitude = atof( alt_str.c_str() ); + begin = end + 1; + + // altitude units + end = msg.find(",", begin); + if ( end == string::npos ) { + return false; + } + + string alt_units = msg.substr(begin, end - begin); + begin = end + 1; + + if ( alt_units != "F" && alt_units != "f" ) { + altitude *= SG_METER_TO_FEET; + } + + cur_fdm_state->set_Altitude( altitude ); + + SG_LOG( SG_IO, SG_INFO, " altitude = " << altitude ); + + } + + // printf("%.8f %.8f\n", lon, lat); + + begin = begin_line; + end_line = msg.find("\n", begin_line); + } + + return true; +} + + +// open hailing frequencies +bool FGAV400::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 FGAV400::process() { + SGIOChannel *io = get_io_channel(); + + if ( get_direction() == SG_IO_OUT ) { + gen_message(); + if ( ! io->write( buf, length ) ) { + SG_LOG( SG_IO, SG_WARN, "Error writing data." ); + return false; + } + } else if ( get_direction() == SG_IO_IN ) { + if ( (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." ); + } + } else { + SG_LOG( SG_IO, SG_ALERT, "Error reading data." ); + return false; + } + if ( (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." ); + } + } else { + SG_LOG( SG_IO, SG_ALERT, "Error reading data." ); + return false; + } + } + + return true; +} + + +// close the channel +bool FGAV400::close() { + SGIOChannel *io = get_io_channel(); + + set_enabled( false ); + + if ( ! io->close() ) { + return false; + } + + return true; +} diff --git a/src/Network/AV400.hxx b/src/Network/AV400.hxx new file mode 100644 index 000000000..2256c6854 --- /dev/null +++ b/src/Network/AV400.hxx @@ -0,0 +1,61 @@ +// AV400.hxx -- Garmin 400 series protocal class +// +// Written by Curtis Olson, started August 2006. +// +// Copyright (C) 2006 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_AV400_HXX +#define _FG_AV400_HXX + + +#include + +#include STL_STRING + +#include "protocol.hxx" + +SG_USING_STD(string); + + +class FGAV400 : public FGProtocol { + + char buf[ FG_MAX_MSG_SIZE ]; + int length; + +public: + + FGAV400(); + ~FGAV400(); + + bool gen_message(); + bool parse_message(); + + // open hailing frequencies + bool open(); + + // process work for this port + bool process(); + + // close the channel + bool close(); +}; + + +#endif // _FG_AV400_HXX diff --git a/src/Network/Makefile.am b/src/Network/Makefile.am index d7bf80785..ef53355e4 100644 --- a/src/Network/Makefile.am +++ b/src/Network/Makefile.am @@ -17,6 +17,7 @@ libNetwork_a_SOURCES = \ ATC-Inputs.cxx ATC-Inputs.hxx \ ATC-Outputs.cxx ATC-Outputs.hxx \ atlas.cxx atlas.hxx \ + AV400.cxx AV400.hxx \ garmin.cxx garmin.hxx \ lfsglass.cxx lfsglass.hxx lfsglass_data.hxx \ httpd.cxx httpd.hxx \ -- 2.39.5