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

SITL: improved Morse socket handling

This commit is contained in:
Andrew Tridgell 2018-12-04 09:04:21 +11:00
parent 505bc2b1fc
commit a4f388e0f3
2 changed files with 33 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/SITL/SIM_Morse.cpp
View File

@ -159,28 +159,40 @@ bool Morse::parse_sensors(const char *json)
*/
bool Morse::connect_sockets(void)
{
if (!sensors_sock_connected) {
if (!sensors_sock.connect(morse_ip, morse_sensors_port)) {
if (!sensors_sock) {
sensors_sock = new SocketAPM(false);
if (!sensors_sock) {
AP_HAL::panic("Out of memory for sensors socket");
}
if (!sensors_sock->connect(morse_ip, morse_sensors_port)) {
if (connect_counter++ == 1000) {
printf("Waiting to connect to sensors control on %s:%u\n",
morse_ip, morse_sensors_port);
connect_counter = 0;
}
delete sensors_sock;
sensors_sock = nullptr;
return false;
}
sensors_sock->reuseaddress();
printf("Sensors connected\n");
sensors_sock_connected = true;
}
if (!control_sock_connected) {
if (!control_sock.connect(morse_ip, morse_control_port)) {
if (!control_sock) {
control_sock = new SocketAPM(false);
if (!control_sock) {
AP_HAL::panic("Out of memory for control socket");
}
if (!control_sock->connect(morse_ip, morse_control_port)) {
if (connect_counter++ == 1000) {
printf("Waiting to connect to control control on %s:%u\n",
morse_ip, morse_control_port);
connect_counter = 0;
}
delete control_sock;
control_sock = nullptr;
return false;
}
control_sock_connected = true;
control_sock->reuseaddress();
printf("Control connected\n");
}
return true;
@ -191,10 +203,19 @@ bool Morse::connect_sockets(void)
*/
bool Morse::sensors_receive(void)
{
ssize_t ret = sensors_sock.recv(&sensor_buffer[sensor_buffer_len], sizeof(sensor_buffer)-sensor_buffer_len, 0);
ssize_t ret = sensors_sock->recv(&sensor_buffer[sensor_buffer_len], sizeof(sensor_buffer)-sensor_buffer_len, 0);
if (ret <= 0) {
no_data_counter++;
if (no_data_counter == 1000) {
no_data_counter = 0;
delete sensors_sock;
delete control_sock;
sensors_sock = nullptr;
control_sock = nullptr;
}
return false;
}
no_data_counter = 0;
// convert '\n' into nul
while (uint8_t *p = (uint8_t *)memchr(&sensor_buffer[sensor_buffer_len], '\n', ret)) {
@ -238,7 +259,7 @@ void Morse::output_rover(const struct sitl_input &input)
speed_ms, -steering_rps);
buf[sizeof(buf)-1] = 0;
control_sock.send(buf, strlen(buf));
control_sock->send(buf, strlen(buf));
}
/*
@ -268,7 +289,7 @@ void Morse::output_quad(const struct sitl_input &input)
m_back, m_right, m_front, m_left);
buf[sizeof(buf)-1] = 0;
control_sock.send(buf, strlen(buf));
control_sock->send(buf, strlen(buf));
}

7
libraries/SITL/SIM_Morse.h
View File

@ -63,11 +63,10 @@ private:
uint8_t sensor_buffer[2048];
uint32_t sensor_buffer_len;
SocketAPM sensors_sock{false};
SocketAPM control_sock{false};
SocketAPM *sensors_sock;
SocketAPM *control_sock;
bool sensors_sock_connected;
bool control_sock_connected;
uint32_t no_data_counter;
uint32_t connect_counter;
double initial_time_s;