SITL: Airsim: Resend servo output after timeout

Plus some cleanup

libraries/SITL/SIM_AirSim.cpp

@@ -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;
-
-        case OutputType::Rover:
-            output_rover(input);
-            break;
-    }
+    // Send servos to AirSim
+    output_servos(input);
 
-    recv_fdm();
+    // Receive sensor data
+    recv_fdm(input);
 
     // update magnetic field
     update_mag_field_bf();

libraries/SITL/SIM_AirSim.h

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