Sync. w. JSBSim CVS

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

diff --git a/src/Network/AV400.cxx b/src/Network/AV400.cxx
index ee7b7fa73129e1a7df76c7788b34bbb37cd613fe..1b59d9e4c19b9d8a8fec060a0c0bf9d0d0a96128 100644 (file)
--- a/src/Network/AV400.cxx
+++ b/src/Network/AV400.cxx
@@ -36,8 +36,6 @@
 
 #include "AV400.hxx"
 
-SG_USING_NAMESPACE(std);
-
 FGAV400::FGAV400() {
 }
 
@@ -109,11 +107,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 +130,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 +156,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;