X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FNetwork%2Fgarmin.cxx;h=f2c8f32880486396370c51482e8c93f3181687c3;hb=cc3d0221ea5fb1def56f53305560829fc66c2ab6;hp=1e3a0464a277065ae55cb56c4b131e9b50e862ab;hpb=d1e4a4bc6f6a18a1f8c646e9eb6fbd7e2c6f98ec;p=flightgear.git diff --git a/src/Network/garmin.cxx b/src/Network/garmin.cxx index 1e3a0464a..f2c8f3288 100644 --- a/src/Network/garmin.cxx +++ b/src/Network/garmin.cxx @@ -22,14 +22,17 @@ #include -#include -#include +#include +#include +#include #include +#include
+#include
-#include "iochannel.hxx" #include "garmin.hxx" +SG_USING_NAMESPACE(std); FGGarmin::FGGarmin() { } @@ -62,71 +65,84 @@ static char calc_nmea_cksum(char *sentence) { bool FGGarmin::gen_message() { // cout << "generating garmin message" << endl; - char rmc[256], rmc_sum[256], rmz[256], rmz_sum[256]; + char rmc[256], rmc_sum[256], rmz[256], rmz_sum[256], gsa[256]; 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 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( rmc_lat, "%02d%07.4f,%c", abs(deg), min, dir); - char lon[20]; - double lond = cur_fdm_state->get_Longitude() * RAD_TO_DEG; + char 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( rmc_lon, "%03d%07.4f,%c", abs(deg), min, dir); char speed[10]; sprintf( speed, "%05.1f", cur_fdm_state->get_V_equiv_kts() ); char heading[10]; - sprintf( heading, "%05.1f", cur_fdm_state->get_Psi() * RAD_TO_DEG ); + sprintf( heading, "%05.1f", cur_fdm_state->get_Psi() * SGD_RADIANS_TO_DEGREES ); 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() ); + (int)(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 ); + + char magvar[10]; + float magdeg = fgGetDouble( "/environment/magnetic-variation-deg" ); + if ( magdeg < 0.0 ) { + magdeg = -magdeg; + dir = 'W'; + } else { + dir = 'E'; + } + sprintf( magvar, "%05.1f,%c", magdeg, dir ); - // $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,000.0,E", - utc, lat, lon, speed, heading, date ); + // $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", + utc, rmc_lat, rmc_lon, speed, heading, date, magvar ); sprintf( rmc_sum, "%02X", calc_nmea_cksum(rmc) ); // sprintf( gga, "$GPGGA,%s,%s,%s,1,04,0.0,%s,M,00.0,M,,*00\r\n", // utc, lat, lon, altitude_m ); - sprintf( rmz, "PGRMZ,%s,f,3", altitude_ft ); + sprintf( rmz, "PGRMZ,%s,M,3", altitude_m ); sprintf( rmz_sum, "%02X", calc_nmea_cksum(rmz) ); - FG_LOG( FG_IO, FG_DEBUG, rmc ); - FG_LOG( FG_IO, FG_DEBUG, rmz ); + 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, rmz ); + SG_LOG( SG_IO, SG_DEBUG, gsa ); string garmin_sentence; @@ -135,14 +151,18 @@ bool FGGarmin::gen_message() { garmin_sentence += rmc; garmin_sentence += "*"; garmin_sentence += rmc_sum; - garmin_sentence += "\n"; + garmin_sentence += "\r\n"; // RMZ sentence (garmin proprietary) garmin_sentence += "$"; garmin_sentence += rmz; garmin_sentence += "*"; garmin_sentence += rmz_sum; - garmin_sentence += "\n"; + garmin_sentence += "\r\n"; + + // GSA sentence (totally faked) + garmin_sentence += gsa; + garmin_sentence += "\r\n"; cout << garmin_sentence; @@ -155,11 +175,11 @@ bool FGGarmin::gen_message() { // parse Garmin message bool FGGarmin::parse_message() { - FG_LOG( FG_IO, FG_INFO, "parse garmin message" ); + SG_LOG( SG_IO, SG_INFO, "parse garmin 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; @@ -169,12 +189,12 @@ bool FGGarmin::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); @@ -184,7 +204,7 @@ bool FGGarmin::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; @@ -198,7 +218,7 @@ bool FGGarmin::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); @@ -208,7 +228,7 @@ bool FGGarmin::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); @@ -236,8 +256,8 @@ bool FGGarmin::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); @@ -265,16 +285,18 @@ bool FGGarmin::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); @@ -285,9 +307,9 @@ bool FGGarmin::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); @@ -300,8 +322,8 @@ bool FGGarmin::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 == "PGRMZ" ) { // altitude end = msg.find(",", begin); @@ -323,12 +345,12 @@ bool FGGarmin::parse_message() { begin = end + 1; if ( alt_units != "F" && 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 ); } @@ -345,15 +367,15 @@ bool FGGarmin::parse_message() { // open hailing frequencies bool FGGarmin::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; } @@ -365,35 +387,35 @@ bool FGGarmin::open() { // process work for this port bool FGGarmin::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 ) { - FG_LOG( FG_IO, FG_ALERT, "Success reading data." ); + SG_LOG( SG_IO, SG_ALERT, "Success reading data." ); if ( parse_message() ) { - FG_LOG( FG_IO, FG_ALERT, "Success parsing data." ); + SG_LOG( SG_IO, SG_ALERT, "Success parsing data." ); } else { - FG_LOG( FG_IO, FG_ALERT, "Error parsing data." ); + SG_LOG( SG_IO, SG_ALERT, "Error parsing data." ); } } else { - FG_LOG( FG_IO, FG_ALERT, "Error reading data." ); + SG_LOG( SG_IO, SG_ALERT, "Error reading data." ); return false; } if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) { - FG_LOG( FG_IO, FG_ALERT, "Success reading data." ); + SG_LOG( SG_IO, SG_ALERT, "Success reading data." ); if ( parse_message() ) { - FG_LOG( FG_IO, FG_ALERT, "Success parsing data." ); + SG_LOG( SG_IO, SG_ALERT, "Success parsing data." ); } else { - FG_LOG( FG_IO, FG_ALERT, "Error parsing data." ); + SG_LOG( SG_IO, SG_ALERT, "Error parsing data." ); } } else { - FG_LOG( FG_IO, FG_ALERT, "Error reading data." ); + SG_LOG( SG_IO, SG_ALERT, "Error reading data." ); return false; } } @@ -404,7 +426,7 @@ bool FGGarmin::process() { // close the channel bool FGGarmin::close() { - FGIOChannel *io = get_io_channel(); + SGIOChannel *io = get_io_channel(); set_enabled( false );