HAL_SITL: support a 2nd GPS

libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp

@@ -40,6 +40,7 @@ static SITLUARTDriver sitlUart0Driver(0, &sitlState);
 static SITLUARTDriver sitlUart1Driver(1, &sitlState);
 static SITLUARTDriver sitlUart2Driver(2, &sitlState);
 static SITLUARTDriver sitlUart3Driver(3, &sitlState);
+static SITLUARTDriver sitlUart4Driver(4, &sitlState);
 
 static SITLUtil utilInstance;
 
@@ -49,6 +50,7 @@ HAL_AVR_SITL::HAL_AVR_SITL() :
         &sitlUart1Driver,  /* uartB */
         &sitlUart2Driver,  /* uartC */
         &sitlUart3Driver,  /* uartD */
+        &sitlUart4Driver,  /* uartE */
         &emptyI2C, /* i2c */
         &emptySPI, /* spi */
         &sitlAnalogIn, /* analogin */

libraries/AP_HAL_AVR_SITL/SITL_State.cpp

@@ -588,6 +588,16 @@ void SITL_State::loop_hook(void)
         FD_SET(fd, &fds);
         max_fd = max(fd, max_fd);
     }
+    fd = ((AVR_SITL::SITLUARTDriver*)hal.uartD)->_fd;
+    if (fd != -1) {
+        FD_SET(fd, &fds);
+        max_fd = max(fd, max_fd);
+    }
+    fd = ((AVR_SITL::SITLUARTDriver*)hal.uartE)->_fd;
+    if (fd != -1) {
+        FD_SET(fd, &fds);
+        max_fd = max(fd, max_fd);
+    }
     tv.tv_sec = 0;
     tv.tv_usec = 100;
     fflush(stdout);

libraries/AP_HAL_AVR_SITL/SITL_State.h

@@ -34,6 +34,7 @@ public:
     };
 
     int gps_pipe(void);
+    int gps2_pipe(void);
     ssize_t gps_read(int fd, void *buf, size_t count);
     static uint16_t pwm_output[11];
     static uint16_t last_pwm_output[11];

libraries/AP_HAL_AVR_SITL/UARTDriver.cpp

@@ -62,6 +62,12 @@ void SITLUARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
         _connected = true;
         _fd = _sitlState->gps_pipe();
         break;
+
+    case 4:
+        /* gps2 */
+        _connected = true;
+        _fd = _sitlState->gps2_pipe();
+        break;
         
     default:
         _tcp_start_connection(false);
@@ -120,7 +126,7 @@ int16_t SITLUARTDriver::read(void)
         return -1;
     }
 
-    if (_portNumber == 1) {
+    if (_portNumber == 1 || _portNumber == 4) {
         if (_sitlState->gps_read(_fd, &c, 1) == 1) {
             return (uint8_t)c;
         }

libraries/AP_HAL_AVR_SITL/sitl_gps.cpp

@@ -35,11 +35,11 @@ static uint8_t gps_delay;
 SITL_State::gps_data SITL_State::_gps_data[MAX_GPS_DELAY];
 
 // state of GPS emulation
-static struct {
+static struct gps_state {
 	/* pipe emulating UBLOX GPS serial stream */
 	int gps_fd, client_fd;
 	uint32_t last_update; // milliseconds
-} gps_state;
+} gps_state, gps2_state;
 
 /*
   hook for reading from the GPS pipe
@@ -78,6 +78,24 @@ int SITL_State::gps_pipe(void)
 	return gps_state.client_fd;
 }
 
+/*
+  setup GPS2 input pipe
+ */
+int SITL_State::gps2_pipe(void)
+{
+	int fd[2];
+	if (gps2_state.client_fd != 0) {
+		return gps2_state.client_fd;
+	}
+	pipe(fd);
+	gps2_state.gps_fd    = fd[1];
+	gps2_state.client_fd = fd[0];
+	gps2_state.last_update = _scheduler->millis();
+	AVR_SITL::SITLUARTDriver::_set_nonblocking(gps2_state.gps_fd);
+	AVR_SITL::SITLUARTDriver::_set_nonblocking(fd[0]);
+	return gps2_state.client_fd;
+}
+
 /*
   write some bytes from the simulated GPS
  */
@@ -92,7 +110,9 @@ void SITL_State::_gps_write(const uint8_t *p, uint16_t size)
 				continue;
 			}
 		}
-		write(gps_state.gps_fd, p++, 1);
+		write(gps_state.gps_fd, p, 1);
+		write(gps2_state.gps_fd, p, 1);
+        p++;
 	}
 }
 
@@ -547,8 +567,12 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
 	if (gps_state.gps_fd != 0) {
 		read(gps_state.gps_fd, &c, 1);
 	}
+	if (gps2_state.gps_fd != 0) {
+		read(gps2_state.gps_fd, &c, 1);
+	}
 
 	gps_state.last_update = hal.scheduler->millis();
+	gps2_state.last_update = hal.scheduler->millis();
 
 	d.latitude = latitude + glitch_offsets.x;
 	d.longitude = longitude + glitch_offsets.y;
@@ -574,7 +598,7 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
 		}
 	}
 
-	if (gps_state.gps_fd == 0) {
+	if (gps_state.gps_fd == 0 && gps2_state.gps_fd == 0) {
 		return;
 	}