Add a telnet command parser to UGear so we can feed commands remotely to

be passed up to the UAS.

utils/GPSsmooth/Makefile.am

@@ -21,7 +21,8 @@ UGsmooth_SOURCES = \
         UGear.cxx UGear.hxx \
 	UGear_command.cxx UGear_command.hxx \
         UGear_main.cxx \
-	UGear_opengc.hxx
+	UGear_opengc.hxx \
+	UGear_telnet.cxx UGear_telnet.hxx
 
 UGsmooth_LDADD = \
         -lsgio -lsgserial -lsgtiming -lsgmath -lsgbucket -lsgmisc -lsgdebug \

utils/GPSsmooth/UGear_command.cxx

@@ -96,3 +96,8 @@ void UGCommand::add( const string command )
     printf("command queue: %s\n", command.c_str());
     cmd_queue.push( command );
 }
+
+
+// create the global command channel manager
+UGCommand command_mgr;
+

utils/GPSsmooth/UGear_command.hxx

@@ -50,4 +50,7 @@ public:
 };
 
 
+extern UGCommand command_mgr;
+
+
 #endif // _FG_UGEAR_COMMAND_HXX

utils/GPSsmooth/UGear_main.cxx

@@ -26,6 +26,7 @@
 #include "UGear.hxx"
 #include "UGear_command.hxx"
 #include "UGear_opengc.hxx"
+#include "UGear_telnet.hxx"
 
 
 SG_USING_STD(cout);
@@ -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
@@ -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;
@@ -1074,7 +1077,7 @@ int main( int argc, char **argv ) {
                     current_time = health_time;
                     printf("Received a health packet, sequence: %d\n",
                            healthpacket.command_sequence);
-                    command.update_cmd_sequence(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;
             }
 

utils/GPSsmooth/UGear_telnet.cxx

@@ -0,0 +1,232 @@
+// \file props.cxx
+// Property server class.
+//
+// Written by Curtis Olson, started September 2000.
+// Modified by Bernie Bright, May 2002.
+//
+// Copyright (C) 2000  Curtis L. Olson - http://www.flightgear.org/~curt
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+//
+// $Id$
+
+
+#include <simgear/structure/commands.hxx>
+#include <simgear/misc/strutils.hxx>
+
+#include <sstream>
+
+#include <plib/netChat.h>
+
+#include "UGear_command.hxx"
+#include "UGear_telnet.hxx"
+
+using std::stringstream;
+using std::ends;
+
+/**
+ * Props connection class.
+ * This class represents a connection to props client.
+ */
+class PropsChannel : public netChat
+{
+    netBuffer buffer;
+
+    /**
+     * Current property node name.
+     */
+    string path;
+
+    enum Mode {
+	PROMPT,
+	DATA
+    };
+    Mode mode;
+
+public:
+
+    /**
+     * Constructor.
+     */
+    PropsChannel();
+    
+    /**
+     * Append incoming data to our request buffer.
+     *
+     * @param s Character string to append to buffer
+     * @param n Number of characters to append.
+     */
+    void collectIncomingData( const char* s, int n );
+
+    /**
+     * Process a complete request from the props client.
+     */
+    void foundTerminator();
+
+private:
+    /**
+     * Return a "Node no found" error message to the client.
+     */
+    void node_not_found_error( const string& node_name );
+};
+
+/**
+ * 
+ */
+PropsChannel::PropsChannel()
+    : buffer(512),
+      path("/"),
+      mode(PROMPT)
+{
+    setTerminator( "\r\n" );
+}
+
+/**
+ * 
+ */
+void
+PropsChannel::collectIncomingData( const char* s, int n )
+{
+    buffer.append( s, n );
+}
+
+/**
+ * 
+ */
+void
+PropsChannel::node_not_found_error( const string& node_name )
+{
+    string error = "-ERR Node \"";
+    error += node_name;
+    error += "\" not found.";
+    push( error.c_str() );
+    push( getTerminator() );
+}
+
+/**
+ * We have a command.
+ * 
+ */
+void
+PropsChannel::foundTerminator()
+{
+    const char* cmd = buffer.getData();
+    SG_LOG( SG_IO, SG_INFO, "processing command = \"" << cmd << "\"" );
+
+    vector<string> tokens = simgear::strutils::split( cmd );
+
+    if (!tokens.empty()) {
+	string command = tokens[0];
+
+        if ( command == "send" ) {
+            command_mgr.add( tokens[1] );
+        } else if ( command == "quit" ) {
+	    close();
+	    shouldDelete();
+	    return;
+	} else if ( command == "data" ) {
+	    mode = DATA;
+	} else if ( command == "prompt" ) {
+	    mode = PROMPT;
+	} else {
+	    const char* msg = "\
+Valid commands are:\r\n\
+\r\n\
+data               switch to raw data mode\r\n\
+prompt             switch to interactive mode (default)\r\n\
+quit               terminate connection\r\n\
+send <command>     send <command> to UAS\r\n";
+	    push( msg );
+	}
+    }
+
+    if (mode == PROMPT) {
+	string prompt = "> ";
+	push( prompt.c_str() );
+    }
+
+    buffer.remove();
+}
+
+/**
+ * 
+ */
+UGTelnet::UGTelnet( const int port_num ):
+    enabled(false)
+{
+    port = port_num;
+}
+
+/**
+ * 
+ */
+UGTelnet::~UGTelnet()
+{
+}
+
+/**
+ * 
+ */
+bool
+UGTelnet::open()
+{
+    if (enabled ) {
+	printf("This shouldn't happen, but the telnet channel is already in use, ignoring\n" );
+	return false;
+    }
+
+    netChannel::open();
+    netChannel::bind( "", port );
+    netChannel::listen( 5 );
+    printf("Telnet server started on port %d\n", port );
+
+    enabled = true;
+
+    return true;
+}
+
+/**
+ * 
+ */
+bool
+UGTelnet::close()
+{
+    SG_LOG( SG_IO, SG_INFO, "closing UGTelnet" );   
+    return true;
+}
+
+/**
+ * 
+ */
+bool
+UGTelnet::process()
+{
+    netChannel::poll();
+    return true;
+}
+
+/**
+ * 
+ */
+void
+UGTelnet::handleAccept()
+{
+    netAddress addr;
+    int handle = netChannel::accept( &addr );
+    printf("Telent server accepted connection from %s:%d\n",
+           addr.getHost(), addr.getPort() );
+    PropsChannel* channel = new PropsChannel();
+    channel->setHandle( handle );
+}

utils/GPSsmooth/UGear_telnet.hxx

@@ -0,0 +1,92 @@
+// \file UGear_telnet.hxx
+// telnet server class.
+//
+// Adapted from FlightGear props.hxx/cxx code
+// Written by Curtis Olson, started September 2000.
+// Modified by Bernie Bright, May 2002.
+//
+// Copyright (C) 2000  Curtis L. Olson - http://www.flightgear.org/~curt
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+//
+// $Id$
+
+
+#ifndef _UG_TELNET_HXX
+#define _UG_TELNET_HXX
+
+#include <string>
+#include <vector>
+
+using std::string;
+using std::vector;
+
+#include <plib/netChannel.h>
+
+
+/**
+ * Telent server class.
+ * This class provides a telnet-like server for remote access to
+ * FlightGear properties.
+ */
+class UGTelnet: netChannel
+{
+
+private:
+
+    /**
+     * Server port to listen on.
+     */
+    int port;
+    bool enabled;
+
+public:
+
+    /**
+     * Create a new TCP server.
+     * 
+     * @param tokens Tokenized configuration parameters
+     */
+    UGTelnet( const int port_num );
+
+    /**
+     * Destructor.
+     */
+    ~UGTelnet();
+
+    /**
+     * Start the telnet server.
+     */
+    bool open();
+
+    /**
+     * Process network activity.
+     */
+    bool process();
+
+    /**
+     * 
+     */
+    bool close();
+
+    /**
+     * Accept a new client connection.
+     */
+    void handleAccept();
+
+};
+
+#endif // _UG_TELNET_HXX
+