aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: Airsim: Resend servo output after timeout

Browse Source 
Plus some cleanup
This commit is contained in:
Rajat Singhal 2020-08-06 16:36:12 +05:30 committed by Andrew Tridgell
parent 829cd29d7c
commit 6085614364
2 changed files with 53 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
libraries/SITL/SIM_AirSim.cpp
View File

@ -12,7 +12,7 @@
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
/*
Simulator Connector for AirSim
*/
@ -26,6 +26,8 @@
#include <AP_Logger/AP_Logger.h>
#include <AP_HAL/utility/replace.h>
#define UDP_TIMEOUT_MS 100
extern const AP_HAL::HAL& hal;
using namespace SITL;
@ -70,7 +72,7 @@ void AirSim::set_interface_ports(const char* address, const int port_in, const i
/*
Decode and send servos
*/
void AirSim::output_copter(const struct sitl_input &input)
void AirSim::output_copter(const sitl_input& input)
{
servo_packet pkt;
@ -89,7 +91,7 @@ void AirSim::output_copter(const struct sitl_input &input)
}
}
void AirSim::output_rover(const struct sitl_input &input)
void AirSim::output_rover(const sitl_input& input)
{
rover_packet pkt;
@ -107,6 +109,22 @@ void AirSim::output_rover(const struct sitl_input &input)
}
}
/*
Wrapper function to send servo output
*/
void AirSim::output_servos(const sitl_input& input)
{
switch (output_type) {
case OutputType::Copter:
output_copter(input);
break;
case OutputType::Rover:
output_rover(input);
break;
}
}
/*
very simple JSON parser for sensor data
called with pointer to one row of sensor data, nul terminated
@ -166,7 +184,7 @@ bool AirSim::parse_sensors(const char *json)
return false;
}
uint16_t n = 0;
struct vector3f_array *v = (struct vector3f_array *)key.ptr;
vector3f_array *v = (vector3f_array *)key.ptr;
while (true) {
if (n >= v->length) {
Vector3f *d = (Vector3f *)realloc(v->data, sizeof(Vector3f)*(n+1));
@ -212,7 +230,7 @@ bool AirSim::parse_sensors(const char *json)
return false;
}
uint16_t n = 0;
struct float_array *v = (struct float_array *)key.ptr;
float_array *v = (float_array *)key.ptr;
while (true) {
if (n >= v->length) {
float *d = (float *)realloc(v->data, sizeof(float)*(n+1));
@ -242,13 +260,23 @@ bool AirSim::parse_sensors(const char *json)
Receive new sensor data from simulator
This is a blocking function
*/
void AirSim::recv_fdm()
void AirSim::recv_fdm(const sitl_input& input)
{
// Receive sensor packet
ssize_t ret = sock.recv(&sensor_buffer[sensor_buffer_len], sizeof(sensor_buffer)-sensor_buffer_len, 100);
ssize_t ret = sock.recv(&sensor_buffer[sensor_buffer_len], sizeof(sensor_buffer)-sensor_buffer_len, UDP_TIMEOUT_MS);
uint32_t wait_ms = UDP_TIMEOUT_MS;
while (ret <= 0) {
printf("No sensor message received - %s\n", strerror(errno));
ret = sock.recv(&sensor_buffer[sensor_buffer_len], sizeof(sensor_buffer)-sensor_buffer_len, 100);
// printf("No sensor message received - %s\n", strerror(errno));
ret = sock.recv(&sensor_buffer[sensor_buffer_len], sizeof(sensor_buffer)-sensor_buffer_len, UDP_TIMEOUT_MS);
wait_ms += UDP_TIMEOUT_MS;
// If no sensor message is received after 1 second, resend servos
// this helps if messages are lost on the way, and both AP & Airsim are waiting for each ther
if (wait_ms > 1000) {
wait_ms = 0;
printf("No sensor message received in last 1s, error - %s, resending servos\n", strerror(errno));
output_servos(input);
}
}
// convert '\n' into nul
@ -271,17 +299,9 @@ void AirSim::recv_fdm()
memmove(sensor_buffer, p2, sensor_buffer_len - (p2 - sensor_buffer));
sensor_buffer_len = sensor_buffer_len - (p2 - sensor_buffer);
accel_body = Vector3f(state.imu.linear_acceleration[0],
state.imu.linear_acceleration[1],
state.imu.linear_acceleration[2]);
gyro = Vector3f(state.imu.angular_velocity[0],
state.imu.angular_velocity[1],
state.imu.angular_velocity[2]);
velocity_ef = Vector3f(state.velocity.world_linear_velocity[0],
state.velocity.world_linear_velocity[1],
state.velocity.world_linear_velocity[2]);
accel_body = state.imu.linear_acceleration;
gyro = state.imu.angular_velocity;
velocity_ef = state.velocity.world_linear_velocity;
location.lat = state.gps.lat * 1.0e7;
location.lng = state.gps.lon * 1.0e7;
@ -369,19 +389,13 @@ void AirSim::recv_fdm()
/*
update the AirSim simulation by one time step
*/
void AirSim::update(const struct sitl_input &input)
void AirSim::update(const sitl_input& input)
{
switch (output_type) {
case OutputType::Copter:
output_copter(input);
break;
// Send servos to AirSim
output_servos(input);
case OutputType::Rover:
output_rover(input);
break;
}
recv_fdm();
// Receive sensor data
recv_fdm(input);
// update magnetic field
update_mag_field_bf();

13
libraries/SITL/SIM_AirSim.h
View File

@ -12,7 +12,7 @@
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
/*
Simulator connector for Airsim: https://github.com/Microsoft/AirSim
*/
@ -23,7 +23,7 @@
namespace SITL {
/*
/*
Airsim Simulator
*/
@ -77,9 +77,12 @@ private:
uint64_t last_frame_count;
uint64_t last_timestamp;
void output_copter(const struct sitl_input &input);
void output_rover(const struct sitl_input &input);
void recv_fdm();
void output_copter(const sitl_input& input);
void output_rover(const sitl_input& input);
// Wrapper function over the above 2 output methods
void output_servos(const sitl_input& input);
void recv_fdm(const sitl_input& input);
void report_FPS(void);
bool parse_sensors(const char *json);