SITL: fixed GPS and UART startup

libraries/AP_HAL_AVR_SITL/SITL_State.h

@@ -36,6 +36,9 @@ public:
 	    ArduPlane
     };
 
+    int gps_pipe(void);
+    ssize_t gps_read(int fd, void *buf, size_t count);
+
 private:
     void _parse_command_line(int argc, char * const argv[]);
     void _usage(void);
@@ -62,8 +65,6 @@ private:
     static Vector3f _rand_vec3f(void);
     static Vector3f _heading_to_mag(float roll, float pitch, float yaw);
     static void _gps_send(uint8_t msgid, uint8_t *buf, uint16_t size);
-    ssize_t _gps_read(int fd, void *buf, size_t count);
-    int _gps_pipe(void);
 
     // signal handlers
     static void _sig_fpe(int signum);

libraries/AP_HAL_AVR_SITL/UARTDriver.cpp

@@ -27,6 +27,8 @@
 
 #include "vprintf.h"
 #include "UARTDriver.h"
+#include "SITL_State.h"
+
 using namespace AVR_SITL;
 
 #define LISTEN_BASE_PORT 5760
@@ -51,7 +53,7 @@ void SITLUARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
     case 1:
         /* gps */
         _connected = true;
-        _fd = -1; // sitl_gps_pipe();
+        _fd = _sitlState->gps_pipe();
         break;
         
     default:
@@ -111,14 +113,12 @@ int16_t SITLUARTDriver::read(void)
         return -1;
     }
 
-#if 0
     if (_portNumber == 1) {
-        if (sitl_gps_read(_fd, &c, 1) == 1) {
+        if (_sitlState->gps_read(_fd, &c, 1) == 1) {
             return (uint8_t)c;
         }
         return -1;
     }
-#endif
 
     if (_console) {
         return ::read(0, &c, 1);
@@ -204,6 +204,10 @@ void SITLUARTDriver::_tcp_start_connection(bool wait_for_connection)
 	struct sockaddr_in sockaddr;
 	int ret;
 
+        if (_connected) {
+            return;
+        }
+
 	if (_console) {
             // hack for console access
             _connected = true;
@@ -213,39 +217,49 @@ void SITLUARTDriver::_tcp_start_connection(bool wait_for_connection)
             return;
 	}
 
-	memset(&sockaddr,0,sizeof(sockaddr));
+        if (_fd != -1) {
+            close(_fd);
+        }
+
+        if (_listen_fd == -1) {
+            memset(&sockaddr,0,sizeof(sockaddr));
 
 #ifdef HAVE_SOCK_SIN_LEN
-	sockaddr.sin_len = sizeof(sockaddr);
+            sockaddr.sin_len = sizeof(sockaddr);
 #endif
-	sockaddr.sin_port = htons(LISTEN_BASE_PORT + _portNumber);
+            sockaddr.sin_port = htons(LISTEN_BASE_PORT + _portNumber);
-	sockaddr.sin_family = AF_INET;
+            sockaddr.sin_family = AF_INET;
 
-	_listen_fd = socket(AF_INET, SOCK_STREAM, 0);
+            _listen_fd = socket(AF_INET, SOCK_STREAM, 0);
-	if (_listen_fd == -1) {
+            if (_listen_fd == -1) {
 		fprintf(stderr, "socket failed - %s\n", strerror(errno));
 		exit(1);
-	}
+            }
+
+            /* we want to be able to re-use ports quickly */
+            setsockopt(_listen_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
 
-	/* we want to be able to re-use ports quickly */
+            fprintf(stderr, "bind port %u for %u\n", 
-	setsockopt(_listen_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
+                    (unsigned)ntohs(sockaddr.sin_port),
+                    (unsigned)_portNumber),
 
-	ret = bind(_listen_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
+                ret = bind(_listen_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
-	if (ret == -1) {
+            if (ret == -1) {
 		fprintf(stderr, "bind failed on port %u - %s\n",
-				(unsigned)ntohs(sockaddr.sin_port),
+                        (unsigned)ntohs(sockaddr.sin_port),
-				strerror(errno));
+                        strerror(errno));
 		exit(1);
-	}
+            }
 
-	ret = listen(_listen_fd, 5);
+            ret = listen(_listen_fd, 5);
-	if (ret == -1) {
+            if (ret == -1) {
-            fprintf(stderr, "listen failed - %s\n", strerror(errno));
+                fprintf(stderr, "listen failed - %s\n", strerror(errno));
-            exit(1);
+                exit(1);
-	}
+            }
 
-	printf("Serial port %u on TCP port %u\n", _portNumber, LISTEN_BASE_PORT + _portNumber);
+            printf("Serial port %u on TCP port %u\n", _portNumber, LISTEN_BASE_PORT + _portNumber);
-	fflush(stdout);
+            fflush(stdout);
+        }
 
 	if (wait_for_connection) {
             printf("Waiting for connection ....\n");

libraries/AP_HAL_AVR_SITL/UARTDriver.h

@@ -13,10 +13,14 @@ class AVR_SITL::SITLUARTDriver : public AP_HAL::UARTDriver {
 public:
     friend class AVR_SITL::SITL_State;
 
-	SITLUARTDriver(const uint8_t portNumber) {
+	SITLUARTDriver(const uint8_t portNumber, SITL_State *sitlState) {
 		_portNumber = portNumber;
         _rxSpace = _default_rx_buffer_size;
         _txSpace = _default_tx_buffer_size;
+        _sitlState = sitlState;
+        
+        _fd = -1;
+        _listen_fd = -1;
 	}
 
     /* Implementations of UARTDriver virtual methods */
@@ -80,6 +84,8 @@ private:
 	///
 	static const uint16_t _max_buffer_size = 512;
 
+    SITL_State *_sitlState;
+
 };
 
 #endif

libraries/AP_HAL_AVR_SITL/sitl_gps.cpp

@@ -50,7 +50,7 @@ static struct {
 /*
   hook for reading from the GPS pipe
  */
-ssize_t SITL_State::_gps_read(int fd, void *buf, size_t count)
+ssize_t SITL_State::gps_read(int fd, void *buf, size_t count)
 {
 #ifdef FIONREAD
 	// use FIONREAD to get exact value if possible
@@ -69,7 +69,7 @@ ssize_t SITL_State::_gps_read(int fd, void *buf, size_t count)
 /*
   setup GPS input pipe
  */
-int SITL_State::_gps_pipe(void)
+int SITL_State::gps_pipe(void)
 {
 	int fd[2];
 	if (gps_state.client_fd != 0) {