X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FNetwork%2Fnmea.cxx;h=bbfd3fc0dc5b7ef1c07f481c6d251d115fb1b8b3;hb=dc76290d6e7d2f24d6c352d8f9da4b0f9f1e5f40;hp=a8aaadc26bd74550db7a36a524a5f8ef52bf27f6;hpb=633e0fde435f3e82ff8a3123495395739508fa42;p=flightgear.git diff --git a/src/Network/nmea.cxx b/src/Network/nmea.cxx index a8aaadc26..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,29 +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 "nmea.hxx" -SG_USING_NAMESPACE(std); - - FGNMEA::FGNMEA() { + fdm = new FlightProperties(); } FGNMEA::~FGNMEA() { + delete fdm; } @@ -79,7 +83,7 @@ bool FGNMEA::gen_message() { t->getGmt()->tm_hour, t->getGmt()->tm_min, t->getGmt()->tm_sec ); char gga_lat[20], rmc_lat[20]; - double latd = cur_fdm_state->get_Latitude() * SGD_RADIANS_TO_DEGREES; + double latd = fdm->get_Latitude() * SGD_RADIANS_TO_DEGREES; if ( latd < 0.0 ) { latd = -latd; dir = 'S'; @@ -88,11 +92,11 @@ bool FGNMEA::gen_message() { } deg = (int)(latd); min = (latd - (double)deg) * 60.0; - sprintf( gga_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 gga_lon[20], rmc_lon[20]; - double lond = cur_fdm_state->get_Longitude() * SGD_RADIANS_TO_DEGREES; + double lond = fdm->get_Longitude() * SGD_RADIANS_TO_DEGREES; if ( lond < 0.0 ) { lond = -lond; dir = 'W'; @@ -101,18 +105,27 @@ bool FGNMEA::gen_message() { } deg = (int)(lond); min = (lond - (double)deg) * 60.0; - sprintf( gga_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() * SGD_RADIANS_TO_DEGREES ); + sprintf( heading, "%.1f", hdg_true ); char altitude_m[10]; - sprintf( altitude_m, "%02d", - (int)(cur_fdm_state->get_Altitude() * SG_FEET_TO_METER) ); + sprintf( altitude_m, "%.1f", + fdm->get_Altitude() * SG_FEET_TO_METER ); char date[10]; int year = t->getGmt()->tm_year; @@ -128,14 +141,15 @@ bool FGNMEA::gen_message() { } else { dir = 'E'; } - sprintf( magvar, "%05.1f,%c", magdeg, dir ); + sprintf( magvar, "%.1f,%c", magdeg, dir ); - // $GPRMC,HHMMSS,A,DDMM.MMMM,N,DDDMM.MMMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E*XX - sprintf( rmc, "GPRMC,%s,A,%s,%s,%s,%s,%s,%s", + // $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,08,0.9,%s,M, , ", + // $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", @@ -152,20 +166,20 @@ bool FGNMEA::gen_message() { nmea_sentence += rmc; nmea_sentence += "*"; nmea_sentence += rmc_sum; - nmea_sentence += "\r\n"; + nmea_sentence += "\n"; // GGA sentence nmea_sentence += "$"; nmea_sentence += gga; nmea_sentence += "*"; nmea_sentence += gga_sum; - nmea_sentence += "\r\n"; + nmea_sentence += "\n"; // GSA sentence (totally faked) nmea_sentence += gsa; - nmea_sentence += "\r\n"; + nmea_sentence += "\n"; -// cout << nmea_sentence; + // cout << nmea_sentence; length = nmea_sentence.length(); strncpy( buf, nmea_sentence.c_str(), length ); @@ -262,7 +276,7 @@ bool FGNMEA::parse_message() { lat *= -1; } - cur_fdm_state->set_Latitude( lat * SGD_DEGREES_TO_RADIANS ); + fdm->set_Latitude( lat * SGD_DEGREES_TO_RADIANS ); SG_LOG( SG_IO, SG_INFO, " lat = " << lat ); // lon val @@ -291,17 +305,17 @@ bool FGNMEA::parse_message() { lon *= -1; } - cur_fdm_state->set_Longitude( lon * SGD_DEGREES_TO_RADIANS ); + fdm->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(), + 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 @@ -313,8 +327,8 @@ 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_calibrated_kts( speed ); - // cur_fdm_state->set_V_ground_speed( speed ); + fdm->set_V_calibrated_kts( speed ); + // fdm->set_V_ground_speed( speed ); SG_LOG( SG_IO, SG_INFO, " speed = " << speed ); // heading @@ -326,8 +340,8 @@ 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(), + 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" ) { @@ -367,7 +381,7 @@ bool FGNMEA::parse_message() { lat *= -1; } - // cur_fdm_state->set_Latitude( lat * SGD_DEGREES_TO_RADIANS ); + // fdm->set_Latitude( lat * SGD_DEGREES_TO_RADIANS ); SG_LOG( SG_IO, SG_INFO, " lat = " << lat ); // lon val @@ -396,7 +410,7 @@ bool FGNMEA::parse_message() { lon *= -1; } - // cur_fdm_state->set_Longitude( lon * SGD_DEGREES_TO_RADIANS ); + // fdm->set_Longitude( lon * SGD_DEGREES_TO_RADIANS ); SG_LOG( SG_IO, SG_INFO, " lon = " << lon ); // junk @@ -452,7 +466,7 @@ bool FGNMEA::parse_message() { altitude *= SG_METER_TO_FEET; } - cur_fdm_state->set_Altitude( altitude ); + fdm->set_Altitude( altitude ); SG_LOG( SG_IO, SG_INFO, " altitude = " << altitude );