Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: support a 2nd GPS

This commit is contained in:
Andrew Tridgell 2013-12-21 22:26:30 +11:00
parent 836f473db2
commit 0115b9fa07
5 changed files with 48 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp
View File

@ -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 */

10
libraries/AP_HAL_AVR_SITL/SITL_State.cpp
View File

@ -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);

1
libraries/AP_HAL_AVR_SITL/SITL_State.h
View File

@ -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];

8
libraries/AP_HAL_AVR_SITL/UARTDriver.cpp
View File

@ -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;
}

32
libraries/AP_HAL_AVR_SITL/sitl_gps.cpp
View File

@ -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;
}