Fix crashes (activating the route-manager) with a default GPS.

[flightgear.git] / src / Network / AV400.cxx

diff --git a/src/Network/AV400.cxx b/src/Network/AV400.cxx
index ee7b7fa73129e1a7df76c7788b34bbb37cd613fe..bed03b48bd04dc00e20c413b486c7325e23bf353 100644 (file)
--- a/src/Network/AV400.cxx
+++ b/src/Network/AV400.cxx
@@ -25,19 +25,18 @@
 #  include "config.h"
 #endif
 
+#include <cstring>
+
 #include <simgear/debug/logstream.hxx>
 #include <simgear/math/sg_geodesy.hxx>
 #include <simgear/io/iochannel.hxx>
 #include <simgear/timing/sg_time.hxx>
 
-#include <FDM/flight.hxx>
 #include <Main/fg_props.hxx>
 #include <Main/globals.hxx>
 
 #include "AV400.hxx"
 
-SG_USING_NAMESPACE(std);
-
 FGAV400::FGAV400() {
 }
 
@@ -45,6 +44,7 @@ FGAV400::~FGAV400() {
 }
 
 
+#if 0
 // calculate the garmin check sum
 static char calc_nmea_cksum(char *sentence) {
     unsigned char sum = 0;
@@ -63,6 +63,7 @@ static char calc_nmea_cksum(char *sentence) {
     // printf("sum = %02x\n", sum);
     return sum;
 }
+#endif
 
 
 // generate AV400 message
@@ -80,12 +81,12 @@ bool FGAV400::gen_message() {
     double min;
 
     // create msg_z
-    sprintf( msg_z, "z%05.0f\r\n", cur_fdm_state->get_Altitude() );
+    sprintf( msg_z, "z%05.0f\r\n", fdm.get_Altitude() );
 
     // create msg_A
     sprintf( msg_A, "A");
 
-    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';
@@ -97,7 +98,7 @@ bool FGAV400::gen_message() {
     sprintf( msg_A, "A%c %02d %04.0f\r\n", dir, deg, min);
 
     // create msg_B
-    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';
@@ -109,11 +110,14 @@ bool FGAV400::gen_message() {
     sprintf( msg_B, "B%c %03d %04.0f\r\n", dir, deg, min);
 
     // create msg_C
+    float magdeg = fgGetDouble( "/environment/magnetic-variation-deg" );
     double vn = fgGetDouble( "/velocities/speed-north-fps" );
     double ve = fgGetDouble( "/velocities/speed-east-fps" );
     double gnd_trk_true = atan2( ve, vn ) * SGD_RADIANS_TO_DEGREES;
-    if ( gnd_trk_true < 0.0 ) { gnd_trk_true += 360.0; }
-    sprintf( msg_C, "C%03.0f\r\n", gnd_trk_true);
+    double gnd_trk_mag = gnd_trk_true - magdeg;
+    if ( gnd_trk_mag < 0.0 ) { gnd_trk_mag += 360.0; }
+    if ( gnd_trk_mag >= 360.0 ) { gnd_trk_mag -= 360.0; }
+    sprintf( msg_C, "C%03.0f\r\n", gnd_trk_mag);
 
     // create msg_D
     double speed_kt = sqrt( vn*vn + ve*ve ) * SG_FPS_TO_KT;
@@ -129,7 +133,6 @@ bool FGAV400::gen_message() {
     // create msg_L (not implemented)
 
     // create msg_Q
-    float magdeg = fgGetDouble( "/environment/magnetic-variation-deg" );
     if ( magdeg < 0.0 ) {
         magdeg = -magdeg;
         dir = 'W';
@@ -156,21 +159,21 @@ bool FGAV400::gen_message() {
     sentence += msg_B;          // longitude
     sentence += msg_C;          // ground track
     sentence += msg_D;          // ground speed (kt)
-    sentence += "E-----\r\n";
-    sentence += "G-----\r\n";
-    sentence += "I----\r\n";
-    sentence += "K-----\r\n";
-    sentence += "L----\r\n";
+    // sentence += "E-----\r\n";
+    // sentence += "G-----\r\n";
+    // sentence += "I----\r\n";
+    // sentence += "K-----\r\n";
+    // sentence += "L----\r\n";
     sentence += msg_Q;          // magvar
-    sentence += "S-----\r\n";
+    // sentence += "S-----\r\n";
     sentence += msg_T;          // end of type 1 messages (must be sent)
     sentence += msg_type2;      // type2 message
-    sentence += "l------\r\n";
+    // sentence += "l------\r\n";
     sentence += '\003';         // ETX
 
-    cout << sentence;
-
+    // cout << sentence;
     length = sentence.length();
+    // cout << endl << "length = " << length << endl;
     strncpy( buf, sentence.c_str(), length );
 
     return true;
@@ -260,7 +263,7 @@ bool FGAV400::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
@@ -289,17 +292,17 @@ bool FGAV400::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
@@ -311,8 +314,8 @@ bool FGAV400::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
@@ -324,8 +327,8 @@ bool FGAV400::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 == "PGRMZ" ) {
@@ -352,7 +355,7 @@ bool FGAV400::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 );