X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FNetwork%2Fnmea.cxx;h=bbfd3fc0dc5b7ef1c07f481c6d251d115fb1b8b3;hb=ae50c054a9007b98f1a8dafe6d589d0b4cab8873;hp=d6c9726e135b774f5c581b20677aef05c905a3b4;hpb=96a9152b0254d17a6cc51b31c7e0277aec270c5e;p=flightgear.git diff --git a/src/Network/nmea.cxx b/src/Network/nmea.cxx index d6c9726e1..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 "nmea.hxx" - FGNMEA::FGNMEA() { + fdm = new FlightProperties(); } FGNMEA::~FGNMEA() { + delete fdm; } @@ -62,7 +70,7 @@ 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; @@ -74,59 +82,82 @@ bool FGNMEA::gen_message() { 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() * SGD_RADIANS_TO_DEGREES; + 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() * SGD_RADIANS_TO_DEGREES; + 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() * 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) ); - - 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" ); 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 ); @@ -241,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 @@ -270,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 @@ -292,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 @@ -305,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" ) { @@ -346,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 @@ -375,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 @@ -427,11 +462,11 @@ bool FGNMEA::parse_message() { string alt_units = msg.substr(begin, end - begin); begin = end + 1; - if ( alt_units != (string)"F" ) { + if ( alt_units != "F" ) { altitude *= SG_METER_TO_FEET; } - cur_fdm_state->set_Altitude( altitude ); + fdm->set_Altitude( altitude ); SG_LOG( SG_IO, SG_INFO, " altitude = " << altitude ); @@ -475,20 +510,20 @@ bool FGNMEA::process() { if ( get_direction() == SG_IO_OUT ) { gen_message(); if ( ! io->write( buf, length ) ) { - SG_LOG( SG_IO, SG_ALERT, "Error writing data." ); + 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 ) { parse_message(); } else { - SG_LOG( SG_IO, SG_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 { - SG_LOG( SG_IO, SG_ALERT, "Error reading data." ); + SG_LOG( SG_IO, SG_WARN, "Error reading data." ); return false; } }