X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FNetwork%2Fnmea.cxx;h=bbfd3fc0dc5b7ef1c07f481c6d251d115fb1b8b3;hb=ae50c054a9007b98f1a8dafe6d589d0b4cab8873;hp=8966a11b19950f896541a071c0e42fb634b831be;hpb=d1e4a4bc6f6a18a1f8c646e9eb6fbd7e2c6f98ec;p=flightgear.git diff --git a/src/Network/nmea.cxx b/src/Network/nmea.cxx index 8966a11b1..bbfd3fc0d 100644 --- a/src/Network/nmea.cxx +++ b/src/Network/nmea.cxx @@ -2,7 +2,7 @@ // // Written by Curtis Olson, started November 1999. // -// Copyright (C) 1999 Curtis L. Olson - curt@flightgear.org +// Copyright (C) 1999 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 @@ -16,25 +16,33 @@ // // 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., 675 Mass Ave, Cambridge, MA 02139, USA. +// 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 +#include +#include
+#include
-#include "iochannel.hxx" #include "nmea.hxx" - FGNMEA::FGNMEA() { + fdm = new FlightProperties(); } FGNMEA::~FGNMEA() { + delete fdm; } @@ -62,71 +70,94 @@ static char calc_nmea_cksum(char *sentence) { bool FGNMEA::gen_message() { // cout << "generating nmea message" << endl; - char rmc[256], gga[256]; + char rmc[256], gga[256], gsa[256]; char rmc_sum[10], gga_sum[10]; char dir; int deg; double min; - FGTime *t = FGTime::cur_time_params; + SGTime *t = globals->get_time_params(); char utc[10]; sprintf( utc, "%02d%02d%02d", t->getGmt()->tm_hour, t->getGmt()->tm_min, t->getGmt()->tm_sec ); - char lat[20]; - double latd = cur_fdm_state->get_Latitude() * RAD_TO_DEG; + char gga_lat[20], rmc_lat[20]; + double latd = fdm->get_Latitude() * SGD_RADIANS_TO_DEGREES; if ( latd < 0.0 ) { - latd *= -1.0; + latd = -latd; dir = 'S'; } else { dir = 'N'; } deg = (int)(latd); min = (latd - (double)deg) * 60.0; - sprintf( lat, "%02d%06.3f,%c", abs(deg), min, dir); + sprintf( gga_lat, "%02d%07.4f,%c", abs(deg), min, dir); + sprintf( rmc_lat, "%02d%07.4f,%c", abs(deg), min, dir); - char lon[20]; - double lond = cur_fdm_state->get_Longitude() * RAD_TO_DEG; + char gga_lon[20], rmc_lon[20]; + double lond = fdm->get_Longitude() * SGD_RADIANS_TO_DEGREES; if ( lond < 0.0 ) { - lond *= -1.0; + lond = -lond; dir = 'W'; } else { dir = 'E'; } deg = (int)(lond); min = (lond - (double)deg) * 60.0; - sprintf( lon, "%03d%06.3f,%c", abs(deg), min, dir); - + sprintf( gga_lon, "%03d%07.4f,%c", abs(deg), min, dir); + sprintf( rmc_lon, "%03d%07.4f,%c", abs(deg), min, dir); + + double vn = fgGetDouble( "/velocities/speed-north-fps" ); + double ve = fgGetDouble( "/velocities/speed-east-fps" ); + double fps = sqrt( vn*vn + ve*ve ); + double mps = fps * SG_FEET_TO_METER; + double kts = mps * SG_METER_TO_NM * 3600; char speed[10]; - sprintf( speed, "%05.1f", cur_fdm_state->get_V_equiv_kts() ); + sprintf( speed, "%.1f", kts ); + double hdg_true = atan2( ve, vn ) * SGD_RADIANS_TO_DEGREES; + if ( hdg_true < 0 ) { + hdg_true += 360.0; + } char heading[10]; - sprintf( heading, "%05.1f", cur_fdm_state->get_Psi() * RAD_TO_DEG ); + sprintf( heading, "%.1f", hdg_true ); char altitude_m[10]; - sprintf( altitude_m, "%02d", - (int)(cur_fdm_state->get_Altitude() * FEET_TO_METER) ); - - char altitude_ft[10]; - sprintf( altitude_ft, "%02d", (int)cur_fdm_state->get_Altitude() ); + sprintf( altitude_m, "%.1f", + fdm->get_Altitude() * SG_FEET_TO_METER ); char date[10]; + int year = t->getGmt()->tm_year; + while ( year >= 100 ) { year -= 100; } sprintf( date, "%02d%02d%02d", t->getGmt()->tm_mday, - t->getGmt()->tm_mon+1, t->getGmt()->tm_year ); + t->getGmt()->tm_mon+1, year ); - // $GPRMC,HHMMSS,A,DDMM.MMM,N,DDDMM.MMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E*XX - sprintf( rmc, "GPRMC,%s,A,%s,%s,%s,%s,%s,0.000,E", - utc, lat, lon, speed, heading, date ); + char magvar[10]; + float magdeg = fgGetDouble( "/environment/magnetic-variation-deg" ); + if ( magdeg < 0.0 ) { + magdeg = -magdeg; + dir = 'W'; + } else { + dir = 'E'; + } + sprintf( magvar, "%.1f,%c", magdeg, dir ); + + // $GPRMC,HHMMSS,A,DDMM.MMMM,N,DDDMM.MMMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E,A*XX + sprintf( rmc, "GPRMC,%s,A,%s,%s,%s,%s,%s,%s,A", + utc, rmc_lat, rmc_lon, speed, heading, date, magvar ); sprintf( rmc_sum, "%02X", calc_nmea_cksum(rmc) ); - sprintf( gga, "GPGGA,%s,%s,%s,1,,,%s,F,,,,", - utc, lat, lon, altitude_ft ); + // $GPGGA,HHMMSS,DDMM.MMMM,N,DDDMM.MMMM,W,1,NN,H.H,AAAA.A,M,GG.G,M,,*XX + sprintf( gga, "GPGGA,%s,%s,%s,1,08,0.9,%s,M,0.0,M,,", + utc, gga_lat, gga_lon, altitude_m ); sprintf( gga_sum, "%02X", calc_nmea_cksum(gga) ); + sprintf( gsa, "%s", + "$GPGSA,A,3,01,02,03,,05,,07,,09,,11,12,0.9,0.9,2.0*38" ); - - FG_LOG( FG_IO, FG_DEBUG, rmc ); - FG_LOG( FG_IO, FG_DEBUG, gga ); + SG_LOG( SG_IO, SG_DEBUG, rmc ); + SG_LOG( SG_IO, SG_DEBUG, gga ); + SG_LOG( SG_IO, SG_DEBUG, gsa ); string nmea_sentence; @@ -144,7 +175,11 @@ bool FGNMEA::gen_message() { nmea_sentence += gga_sum; nmea_sentence += "\n"; - cout << nmea_sentence; + // GSA sentence (totally faked) + nmea_sentence += gsa; + nmea_sentence += "\n"; + + // cout << nmea_sentence; length = nmea_sentence.length(); strncpy( buf, nmea_sentence.c_str(), length ); @@ -160,11 +195,11 @@ bool FGNMEA::gen_message() { // $GPGGA,163227,3321.173,N,11039.855,W,1,,,3333,F,,,,*0F bool FGNMEA::parse_message() { - FG_LOG( FG_IO, FG_INFO, "parse nmea message" ); + SG_LOG( SG_IO, SG_INFO, "parse nmea message" ); string msg = buf; msg = msg.substr( 0, length ); - FG_LOG( FG_IO, FG_INFO, "entire message = " << msg ); + SG_LOG( SG_IO, SG_INFO, "entire message = " << msg ); string::size_type begin_line, end_line, begin, end; begin_line = begin = 0; @@ -174,12 +209,12 @@ bool FGNMEA::parse_message() { while ( end_line != string::npos ) { string line = msg.substr(begin_line, end_line - begin_line); begin_line = end_line + 1; - FG_LOG( FG_IO, FG_INFO, " input line = " << line ); + SG_LOG( SG_IO, SG_INFO, " input line = " << line ); // leading character string start = msg.substr(begin, 1); ++begin; - FG_LOG( FG_IO, FG_INFO, " start = " << start ); + SG_LOG( SG_IO, SG_INFO, " start = " << start ); // sentence end = msg.find(",", begin); @@ -189,7 +224,7 @@ bool FGNMEA::parse_message() { string sentence = msg.substr(begin, end - begin); begin = end + 1; - FG_LOG( FG_IO, FG_INFO, " sentence = " << sentence ); + SG_LOG( SG_IO, SG_INFO, " sentence = " << sentence ); double lon_deg, lon_min, lat_deg, lat_min; double lon, lat, speed, heading, altitude; @@ -203,7 +238,7 @@ bool FGNMEA::parse_message() { string utc = msg.substr(begin, end - begin); begin = end + 1; - FG_LOG( FG_IO, FG_INFO, " utc = " << utc ); + SG_LOG( SG_IO, SG_INFO, " utc = " << utc ); // junk end = msg.find(",", begin); @@ -213,7 +248,7 @@ bool FGNMEA::parse_message() { string junk = msg.substr(begin, end - begin); begin = end + 1; - FG_LOG( FG_IO, FG_INFO, " junk = " << junk ); + SG_LOG( SG_IO, SG_INFO, " junk = " << junk ); // lat val end = msg.find(",", begin); @@ -241,8 +276,8 @@ bool FGNMEA::parse_message() { lat *= -1; } - cur_fdm_state->set_Latitude( lat * DEG_TO_RAD ); - FG_LOG( FG_IO, FG_INFO, " lat = " << lat ); + fdm->set_Latitude( lat * SGD_DEGREES_TO_RADIANS ); + SG_LOG( SG_IO, SG_INFO, " lat = " << lat ); // lon val end = msg.find(",", begin); @@ -270,16 +305,18 @@ bool FGNMEA::parse_message() { lon *= -1; } - cur_fdm_state->set_Longitude( lon * DEG_TO_RAD ); - FG_LOG( FG_IO, FG_INFO, " lon = " << lon ); + fdm->set_Longitude( lon * SGD_DEGREES_TO_RADIANS ); + SG_LOG( SG_IO, SG_INFO, " lon = " << lon ); +#if 0 double sl_radius, lat_geoc; - fgGeodToGeoc( cur_fdm_state->get_Latitude(), - cur_fdm_state->get_Altitude(), + sgGeodToGeoc( fdm->get_Latitude(), + fdm->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() ); + fdm->set_Geocentric_Position( lat_geoc, + fdm->get_Longitude(), + sl_radius + fdm->get_Altitude() ); +#endif // speed end = msg.find(",", begin); @@ -290,9 +327,9 @@ bool FGNMEA::parse_message() { string speed_str = msg.substr(begin, end - begin); begin = end + 1; speed = atof( speed_str.c_str() ); - cur_fdm_state->set_V_equiv_kts( speed ); - cur_fdm_state->set_V_ground_speed( speed ); - FG_LOG( FG_IO, FG_INFO, " speed = " << speed ); + fdm->set_V_calibrated_kts( speed ); + // fdm->set_V_ground_speed( speed ); + SG_LOG( SG_IO, SG_INFO, " speed = " << speed ); // heading end = msg.find(",", begin); @@ -303,10 +340,10 @@ bool FGNMEA::parse_message() { 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 * DEG_TO_RAD ); - FG_LOG( FG_IO, FG_INFO, " heading = " << heading ); + fdm->set_Euler_Angles( fdm->get_Phi(), + fdm->get_Theta(), + heading * SGD_DEGREES_TO_RADIANS ); + SG_LOG( SG_IO, SG_INFO, " heading = " << heading ); } else if ( sentence == "GPGGA" ) { // time end = msg.find(",", begin); @@ -316,7 +353,7 @@ bool FGNMEA::parse_message() { string utc = msg.substr(begin, end - begin); begin = end + 1; - FG_LOG( FG_IO, FG_INFO, " utc = " << utc ); + SG_LOG( SG_IO, SG_INFO, " utc = " << utc ); // lat val end = msg.find(",", begin); @@ -344,8 +381,8 @@ bool FGNMEA::parse_message() { lat *= -1; } - // cur_fdm_state->set_Latitude( lat * DEG_TO_RAD ); - FG_LOG( FG_IO, FG_INFO, " lat = " << lat ); + // fdm->set_Latitude( lat * SGD_DEGREES_TO_RADIANS ); + SG_LOG( SG_IO, SG_INFO, " lat = " << lat ); // lon val end = msg.find(",", begin); @@ -373,8 +410,8 @@ bool FGNMEA::parse_message() { lon *= -1; } - // cur_fdm_state->set_Longitude( lon * DEG_TO_RAD ); - FG_LOG( FG_IO, FG_INFO, " lon = " << lon ); + // fdm->set_Longitude( lon * SGD_DEGREES_TO_RADIANS ); + SG_LOG( SG_IO, SG_INFO, " lon = " << lon ); // junk end = msg.find(",", begin); @@ -384,7 +421,7 @@ bool FGNMEA::parse_message() { string junk = msg.substr(begin, end - begin); begin = end + 1; - FG_LOG( FG_IO, FG_INFO, " junk = " << junk ); + SG_LOG( SG_IO, SG_INFO, " junk = " << junk ); // junk end = msg.find(",", begin); @@ -394,7 +431,7 @@ bool FGNMEA::parse_message() { junk = msg.substr(begin, end - begin); begin = end + 1; - FG_LOG( FG_IO, FG_INFO, " junk = " << junk ); + SG_LOG( SG_IO, SG_INFO, " junk = " << junk ); // junk end = msg.find(",", begin); @@ -404,7 +441,7 @@ bool FGNMEA::parse_message() { junk = msg.substr(begin, end - begin); begin = end + 1; - FG_LOG( FG_IO, FG_INFO, " junk = " << junk ); + SG_LOG( SG_IO, SG_INFO, " junk = " << junk ); // altitude end = msg.find(",", begin); @@ -426,12 +463,12 @@ bool FGNMEA::parse_message() { begin = end + 1; if ( alt_units != "F" ) { - altitude *= METER_TO_FEET; + altitude *= SG_METER_TO_FEET; } - cur_fdm_state->set_Altitude( altitude ); + fdm->set_Altitude( altitude ); - FG_LOG( FG_IO, FG_INFO, " altitude = " << altitude ); + SG_LOG( SG_IO, SG_INFO, " altitude = " << altitude ); } @@ -448,15 +485,15 @@ bool FGNMEA::parse_message() { // open hailing frequencies bool FGNMEA::open() { if ( is_enabled() ) { - FG_LOG( FG_IO, FG_ALERT, "This shouldn't happen, but the channel " + SG_LOG( SG_IO, SG_ALERT, "This shouldn't happen, but the channel " << "is already in use, ignoring" ); return false; } - FGIOChannel *io = get_io_channel(); + SGIOChannel *io = get_io_channel(); if ( ! io->open( get_direction() ) ) { - FG_LOG( FG_IO, FG_ALERT, "Error opening channel communication layer." ); + SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." ); return false; } @@ -468,25 +505,25 @@ bool FGNMEA::open() { // process work for this port bool FGNMEA::process() { - FGIOChannel *io = get_io_channel(); + SGIOChannel *io = get_io_channel(); - if ( get_direction() == out ) { + if ( get_direction() == SG_IO_OUT ) { gen_message(); if ( ! io->write( buf, length ) ) { - FG_LOG( FG_IO, FG_ALERT, "Error writing data." ); + SG_LOG( SG_IO, SG_WARN, "Error writing data." ); return false; } - } else if ( get_direction() == in ) { + } else if ( get_direction() == SG_IO_IN ) { if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) { parse_message(); } else { - FG_LOG( FG_IO, FG_ALERT, "Error reading data." ); + SG_LOG( SG_IO, SG_WARN, "Error reading data." ); return false; } if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) { parse_message(); } else { - FG_LOG( FG_IO, FG_ALERT, "Error reading data." ); + SG_LOG( SG_IO, SG_WARN, "Error reading data." ); return false; } } @@ -497,7 +534,7 @@ bool FGNMEA::process() { // close the channel bool FGNMEA::close() { - FGIOChannel *io = get_io_channel(); + SGIOChannel *io = get_io_channel(); set_enabled( false );