* add support for self-contained tags like <test/>

[flightgear.git] / utils / GPSsmooth / UGear_main.cxx

diff --git a/utils/GPSsmooth/UGear_main.cxx b/utils/GPSsmooth/UGear_main.cxx
index afdd94081eb8d7623066de03607f4aaaf6542951..4352e1c721e7d787421b3986e387fcc17edc1480 100644 (file)
--- a/utils/GPSsmooth/UGear_main.cxx
+++ b/utils/GPSsmooth/UGear_main.cxx
@@ -2,7 +2,7 @@
 #  include <config.h>
 #endif
 
-#ifndef _MSC_VER
+#if !defined( _MSC_VER ) && !defined( __MINGW32__ )
 #  include <strings.h>         // for bzero()
 #else
 #  define bzero(a,b) memset(a,0,b)
@@ -26,11 +26,12 @@
 #include "UGear.hxx"
 #include "UGear_command.hxx"
 #include "UGear_opengc.hxx"
+#include "UGear_telnet.hxx"
 
 
-SG_USING_STD(cout);
-SG_USING_STD(endl);
-SG_USING_STD(string);
+using std::cout;
+using std::endl;
+using std::string;
 
 
 // Network channels
@@ -712,6 +713,9 @@ int main( int argc, char **argv ) {
         track.set_stargate_swap_mode();
     }
 
+    UGTelnet telnet( 5402 );
+    telnet.open();
+
     if ( infile.length() || flight_dir.length() ) {
         if ( infile.length() ) {
             // Load data from a stream log data file
@@ -952,13 +956,13 @@ int main( int argc, char **argv ) {
 
                 current_time_stamp.stamp();
                 /* Convert to ms */
-                double elapsed_us = current_time_stamp - last_time_stamp;
+                double elapsed_us = (current_time_stamp - last_time_stamp).toUSecs();
                 if ( elapsed_us < (frame_us - 2000) ) {
                     double requested_us = (frame_us - elapsed_us) - 2000 ;
                     ulMilliSecondSleep ( (int)(requested_us / 1000.0) ) ;
                 }
                 current_time_stamp.stamp();
-                while ( current_time_stamp - last_time_stamp < frame_us ) {
+                while ( (current_time_stamp - last_time_stamp).toUSecs() < frame_us ) {
                     current_time_stamp.stamp();
                 }
             }
@@ -981,7 +985,7 @@ int main( int argc, char **argv ) {
         printf("<gpx>\n");
 
         cout << "Processed " << imu_count << " entries in "
-             << (current_time_stamp - start_time) / 1000000 << " seconds."
+             << current_time_stamp - start_time << " seconds."
              << endl;
     } else if ( serialdev.length() ) {
         // process incoming data from the serial port
@@ -1023,14 +1027,11 @@ int main( int argc, char **argv ) {
             return false;
         }
 
-        // create the command channel manager
-        UGCommand command;
-
         // add some test commands
-        //command.add("ap,alt,1000");
-        //command.add("home,158.0,32.5");
-        //command.add("go,home");
-        //command.add("go,route");
+        //command_mgr.add("ap,alt,1000");
+        //command_mgr.add("home,158.0,32.5");
+        //command_mgr.add("go,home");
+        //command_mgr.add("go,route");
 
         while ( uavcom.is_enabled() ) {
            // cout << "looking for next message ..." << endl;
@@ -1040,6 +1041,8 @@ int main( int argc, char **argv ) {
             // cout << "message id = " << id << endl;
             count++;
 
+            telnet.process();
+
             if ( id == GPS_PACKET ) {
                 if ( gpspacket.time > gps_time ) {
                     gps_time = gpspacket.time;
@@ -1073,8 +1076,8 @@ int main( int argc, char **argv ) {
                     health_time = healthpacket.time;
                     current_time = health_time;
                     printf("Received a health packet, sequence: %d\n",
-                           healthpacket.command_sequence);
-                    command.update_cmd_sequence(healthpacket.command_sequence);
+                           (int)healthpacket.command_sequence);
+                    command_mgr.update_cmd_sequence(healthpacket.command_sequence);
                 } else {
                     cout << "oops health back in time: " << healthpacket.time << " " << health_time << endl;
                 }
@@ -1092,15 +1095,15 @@ int main( int argc, char **argv ) {
                 gps_status = 1.0;
             }
 
-            // Generate a ground station heart beat every 5 seconds
-            if ( current_time >= command_heartbeat + 5 ) {
-                command.add("hb");
+            // Generate a ground station heart beat every 4 seconds
+            if ( current_time >= command_heartbeat + 4 ) {
+                command_mgr.add("hb");
                 command_heartbeat = current_time;
             }
                 
             // Command update @ 1hz
             if ( current_time >= command_time + 1 ) {
-                command.update(&uavcom);
+                command_mgr.update(&uavcom);
                 command_time = current_time;
             }
 
@@ -1108,9 +1111,9 @@ int main( int argc, char **argv ) {
             if ( current_time >= last_time + (1/hertz) ) {
                 // if ( gpspacket.lat > -500 ) {
                 int londeg = (int)navpacket.lon;
-                double lonmin = fabs(navpacket.lon - londeg);
+                // double lonmin = fabs(navpacket.lon - londeg);
                 int latdeg = (int)navpacket.lat;
-                double latmin = fabs(navpacket.lat - latdeg);
+                // double latmin = fabs(navpacket.lat - latdeg);
                 char londir = 'E'; if ( londeg < 0 ) londir = 'W';
                 char latdir = 'N'; if ( latdeg < 0 ) latdir = 'S';
                 londeg = abs(londeg);