X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FNetwork%2Fnmea.cxx;h=dfc7d5edba02081cc64d9c407ef6c58d3b49a5e7;hb=0aabc112b4f586f94365ccf1e906d6e4fb78dc10;hp=c5f33701fcde746bd44e33f7fd28acf0b2e55a85;hpb=0ffa19cd32c02405d0e740393b37f4009ed5a312;p=flightgear.git diff --git a/src/Network/nmea.cxx b/src/Network/nmea.cxx index c5f33701f..dfc7d5edb 100644 --- a/src/Network/nmea.cxx +++ b/src/Network/nmea.cxx @@ -22,14 +22,18 @@ #include -#include +#include +#include #include #include +#include
+#include
-#include "iochannel.hxx" #include "nmea.hxx" +SG_USING_NAMESPACE(std); + FGNMEA::FGNMEA() { } @@ -62,71 +66,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; - SGTime *t = SGTime::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 = cur_fdm_state->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 = cur_fdm_state->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", + cur_fdm_state->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 +171,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 +191,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 +205,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 +220,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 +234,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 +244,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 +272,8 @@ bool FGNMEA::parse_message() { lat *= -1; } - cur_fdm_state->set_Latitude( lat * DEG_TO_RAD ); - FG_LOG( FG_IO, FG_INFO, " lat = " << lat ); + cur_fdm_state->set_Latitude( lat * SGD_DEGREES_TO_RADIANS ); + SG_LOG( SG_IO, SG_INFO, " lat = " << lat ); // lon val end = msg.find(",", begin); @@ -270,16 +301,18 @@ bool FGNMEA::parse_message() { lon *= -1; } - cur_fdm_state->set_Longitude( lon * DEG_TO_RAD ); - FG_LOG( FG_IO, FG_INFO, " lon = " << lon ); + 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; - fgGeodToGeoc( cur_fdm_state->get_Latitude(), + 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); @@ -290,9 +323,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 ); + 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); @@ -305,8 +338,8 @@ bool FGNMEA::parse_message() { 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 ); + heading * SGD_DEGREES_TO_RADIANS ); + SG_LOG( SG_IO, SG_INFO, " heading = " << heading ); } else if ( sentence == "GPGGA" ) { // time end = msg.find(",", begin); @@ -316,7 +349,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 +377,8 @@ bool FGNMEA::parse_message() { lat *= -1; } - // cur_fdm_state->set_Latitude( lat * DEG_TO_RAD ); - FG_LOG( FG_IO, FG_INFO, " lat = " << lat ); + // cur_fdm_state->set_Latitude( lat * SGD_DEGREES_TO_RADIANS ); + SG_LOG( SG_IO, SG_INFO, " lat = " << lat ); // lon val end = msg.find(",", begin); @@ -373,8 +406,8 @@ bool FGNMEA::parse_message() { lon *= -1; } - // cur_fdm_state->set_Longitude( lon * DEG_TO_RAD ); - FG_LOG( FG_IO, FG_INFO, " lon = " << lon ); + // cur_fdm_state->set_Longitude( lon * SGD_DEGREES_TO_RADIANS ); + SG_LOG( SG_IO, SG_INFO, " lon = " << lon ); // junk end = msg.find(",", begin); @@ -384,7 +417,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 +427,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 +437,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 +459,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 ); - FG_LOG( FG_IO, FG_INFO, " altitude = " << altitude ); + SG_LOG( SG_IO, SG_INFO, " altitude = " << altitude ); } @@ -448,15 +481,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 +501,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 +530,7 @@ bool FGNMEA::process() { // close the channel bool FGNMEA::close() { - FGIOChannel *io = get_io_channel(); + SGIOChannel *io = get_io_channel(); set_enabled( false );