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

SITL: fixed GPS and UART startup

This commit is contained in:
Andrew Tridgell 2012-12-18 13:34:13 +11:00
parent 334b96b375
commit 5f7f6966af
4 changed files with 51 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_HAL_AVR_SITL/SITL_State.h
View File

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

64
libraries/AP_HAL_AVR_SITL/UARTDriver.cpp
View File

@ -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_family = AF_INET;
sockaddr.sin_port = htons(LISTEN_BASE_PORT + _portNumber);
sockaddr.sin_family = AF_INET;
_listen_fd = socket(AF_INET, SOCK_STREAM, 0);
if (_listen_fd == -1) {
_listen_fd = socket(AF_INET, SOCK_STREAM, 0);
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 */
setsockopt(_listen_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
ret = bind(_listen_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
if (ret == -1) {
fprintf(stderr, "bind port %u for %u\n",
(unsigned)ntohs(sockaddr.sin_port),
(unsigned)_portNumber),
ret = bind(_listen_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
if (ret == -1) {
fprintf(stderr, "bind failed on port %u - %s\n",
(unsigned)ntohs(sockaddr.sin_port),
strerror(errno));
(unsigned)ntohs(sockaddr.sin_port),
strerror(errno));
exit(1);
}
}
ret = listen(_listen_fd, 5);
if (ret == -1) {
fprintf(stderr, "listen failed - %s\n", strerror(errno));
exit(1);
}
ret = listen(_listen_fd, 5);
if (ret == -1) {
fprintf(stderr, "listen failed - %s\n", strerror(errno));
exit(1);
}
printf("Serial port %u on TCP port %u\n", _portNumber, LISTEN_BASE_PORT + _portNumber);
fflush(stdout);
printf("Serial port %u on TCP port %u\n", _portNumber, LISTEN_BASE_PORT + _portNumber);
fflush(stdout);
}
if (wait_for_connection) {
printf("Waiting for connection ....\n");

8
libraries/AP_HAL_AVR_SITL/UARTDriver.h
View File

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

4
libraries/AP_HAL_AVR_SITL/sitl_gps.cpp
View File

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