SITL: use thread for FlightAxis comms

this allows us to run SITL at a much higher framerate than RF can
provide, and results in the EKF being much happier

libraries/SITL/SIM_Aircraft.cpp

@@ -32,7 +32,7 @@
 #include <DataFlash/DataFlash.h>
 #include <AP_Param/AP_Param.h>
 
-namespace SITL {
+using namespace SITL;
 
 /*
   parent class for all simulator types
@@ -680,6 +680,9 @@ void Aircraft::smooth_sensors(void)
         return;
     }
     const float delta_time = (now - smoothing.last_update_us) * 1.0e-6f;
+    if (delta_time < 0 || delta_time > 0.1) {
+        return;
+    }
 
     // calculate required accel to get us to desired position and velocity in the time_constant
     const float time_constant = 0.1f;
@@ -773,4 +776,24 @@ float Aircraft::filtered_servo_range(const struct sitl_input &input, uint8_t idx
     return filtered_idx(v, idx);
 }
 
-}  // namespace SITL
+// extrapolate sensors by a given delta time in seconds
+void Aircraft::extrapolate_sensors(float delta_time)
+{
+    Vector3f accel_earth = dcm * accel_body;
+    accel_earth.z += GRAVITY_MSS;
+
+    dcm.rotate(gyro * delta_time);
+    dcm.normalize();
+
+    // work out acceleration as seen by the accelerometers. It sees the kinematic
+    // acceleration (ie. real movement), plus gravity
+    accel_body = dcm.transposed() * (accel_earth + Vector3f(0,0,-GRAVITY_MSS));
+
+    // new velocity and position vectors
+    velocity_ef += accel_earth * delta_time;
+    position += velocity_ef * delta_time;
+    velocity_air_ef = velocity_ef + wind_ef;
+    velocity_air_bf = dcm.transposed() * velocity_air_ef;
+}
+
+

libraries/SITL/SIM_Aircraft.h

@@ -232,6 +232,9 @@ protected:
     float filtered_servo_angle(const struct sitl_input &input, uint8_t idx);
     float filtered_servo_range(const struct sitl_input &input, uint8_t idx);
 
+    // extrapolate sensors by a given delta time in seconds
+    void extrapolate_sensors(float delta_time);
+    
 private:
     uint64_t last_time_us = 0;
     uint32_t frame_counter = 0;

libraries/SITL/SIM_FlightAxis.cpp

@@ -28,10 +28,11 @@
 
 #include <AP_HAL/AP_HAL.h>
 #include <DataFlash/DataFlash.h>
+#include "pthread.h"
 
 extern const AP_HAL::HAL& hal;
 
-namespace SITL {
+using namespace SITL;
 
 // the asprintf() calls are not worth checking for SITL
 #pragma GCC diagnostic ignored "-Wunused-result"
@@ -72,6 +73,41 @@ FlightAxis::FlightAxis(const char *home_str, const char *frame_str) :
     default:
         AP_HAL::panic("Unsupported flightaxis rotation %u\n", (unsigned)rotation);
     }
+
+    /* Create the thread that will be waiting for data from FlightAxis */
+    mutex = hal.util->new_semaphore();
+
+    int ret = pthread_create(&thread, NULL, update_thread, this);
+    if (ret != 0) {
+        AP_HAL::panic("SIM_FlightAxis: failed to create thread");
+    }
+}
+    
+/*
+  update thread trampoline
+ */
+void *FlightAxis::update_thread(void *arg)
+{
+    FlightAxis *flightaxis = (FlightAxis *)arg;
+
+    pthread_setname_np(pthread_self(), "ardupilot-flightaxis");
+    
+    flightaxis->update_loop();
+    return nullptr;
+}
+
+/*
+  main update loop
+ */
+void FlightAxis::update_loop(void)
+{
+    while (true) {
+        struct sitl_input new_input;
+        mutex->take(HAL_SEMAPHORE_BLOCK_FOREVER);
+        new_input = last_input;
+        mutex->give();
+        exchange_data(new_input);
+    }
 }
 
 /*
@@ -267,7 +303,18 @@ void FlightAxis::exchange_data(const struct sitl_input &input)
                                scaled_servos[7]);
 
     if (reply) {
+        mutex->take(HAL_SEMAPHORE_BLOCK_FOREVER);
+        double lastt_s = state.m_currentPhysicsTime_SEC;
         parse_reply(reply);
+        double dt = state.m_currentPhysicsTime_SEC - lastt_s;
+        if (dt > 0 && dt < 0.1) {
+            if (average_frame_time_s < 1.0e-6) {
+                average_frame_time_s = dt;
+            }
+            average_frame_time_s = average_frame_time_s * 0.98 + dt * 0.02;
+        }
+        socket_frame_counter++;
+        mutex->give();
         free(reply);
     }
 }
@@ -278,21 +325,43 @@ void FlightAxis::exchange_data(const struct sitl_input &input)
  */
 void FlightAxis::update(const struct sitl_input &input)
 {
-    exchange_data(input);
-
+    mutex->take(HAL_SEMAPHORE_BLOCK_FOREVER);
+    
+    last_input = input;
+    
     double dt_seconds = state.m_currentPhysicsTime_SEC - last_time_s;
     if (dt_seconds < 0) {
         // cope with restarting RealFlight while connected
         initial_time_s = time_now_us * 1.0e-6f;
         last_time_s = state.m_currentPhysicsTime_SEC;
         position_offset.zero();
+        mutex->give();
         return;
     }
-    if (dt_seconds < 0.0001f) {
-        // we probably got a repeated frame
-        time_now_us += 1;
+    if (dt_seconds < 0.00001f) {
+        float delta_time = 0.001;
+        // don't go past the next expected frame
+        if (delta_time + extrapolated_s > average_frame_time_s) {
+            delta_time = average_frame_time_s - extrapolated_s;
+        }
+        if (delta_time <= 0) {
+            usleep(1000);
+            mutex->give();
+            return;
+        }
+        time_now_us += delta_time * 1.0e6;
+        extrapolate_sensors(delta_time);
+        update_position();
+        update_mag_field_bf();
+        mutex->give();
+        usleep(delta_time*1.0e6);
+        extrapolated_s += delta_time;
+        report_FPS();
         return;
     }
+
+    extrapolated_s = 0;
+    
     if (initial_time_s <= 0) {
         dt_seconds = 0.001f;
         initial_time_s = state.m_currentPhysicsTime_SEC - dt_seconds;
@@ -367,16 +436,15 @@ void FlightAxis::update(const struct sitl_input &input)
 
     update_position();
     time_advance();
-    time_now_us = (state.m_currentPhysicsTime_SEC - initial_time_s)*1.0e6;
-
-    if (frame_counter++ % 1000 == 0) {
-        if (last_frame_count_s != 0) {
-            printf("%.2f FPS\n",
-                   1000 / (state.m_currentPhysicsTime_SEC - last_frame_count_s));
-        } else {
-            printf("Initial position %f %f %f\n", position.x, position.y, position.z);
+    uint64_t new_time_us = (state.m_currentPhysicsTime_SEC - initial_time_s)*1.0e6;
+    if (new_time_us < time_now_us) {
+        uint64_t dt_us = time_now_us - new_time_us;
+        if (dt_us > 500000) {
+            // time going backwards
+            time_now_us = new_time_us;
         }
-        last_frame_count_s = state.m_currentPhysicsTime_SEC;
+    } else {
+        time_now_us = new_time_us;
     }
 
     last_time_s = state.m_currentPhysicsTime_SEC;
@@ -385,6 +453,26 @@ void FlightAxis::update(const struct sitl_input &input)
 
     // update magnetic field
     update_mag_field_bf();
+    mutex->give();
+
+    report_FPS();
 }
 
-} // namespace SITL
+/*
+  report frame rates
+ */
+void FlightAxis::report_FPS(void)
+{
+    if (frame_counter++ % 1000 == 0) {
+        if (last_frame_count_s != 0) {
+            uint64_t frames = socket_frame_counter - last_socket_frame_counter;
+            last_socket_frame_counter = socket_frame_counter;
+            double dt = state.m_currentPhysicsTime_SEC - last_frame_count_s;
+            printf("%.2f/%.2f FPS avg=%.2f\n",
+                   frames / dt, 1000 / dt, 1.0/average_frame_time_s);
+        } else {
+            printf("Initial position %f %f %f\n", position.x, position.y, position.z);
+        }
+        last_frame_count_s = state.m_currentPhysicsTime_SEC;
+    }
+}

libraries/SITL/SIM_FlightAxis.h

@@ -156,14 +156,24 @@ private:
     void exchange_data(const struct sitl_input &input);
     void parse_reply(const char *reply);
 
-    double initial_time_s = 0;
-    double last_time_s = 0;
-    bool heli_demix = false;
-    bool rev4_servos = false;
-    bool controller_started = false;
-    uint64_t frame_counter = 0;
-    uint64_t activation_frame_counter = 0;
-    double last_frame_count_s = 0;
+    static void *update_thread(void *arg);
+    void update_loop(void);
+    void report_FPS(void);
+
+    struct sitl_input last_input;
+
+    double average_frame_time_s;
+    double extrapolated_s;
+    double initial_time_s;
+    double last_time_s;
+    bool heli_demix;
+    bool rev4_servos;
+    bool controller_started;
+    uint64_t frame_counter;
+    uint64_t activation_frame_counter;
+    uint64_t socket_frame_counter;
+    uint64_t last_socket_frame_counter;
+    double last_frame_count_s;
     Vector3f position_offset;
     Vector3f last_velocity_ef;
     Matrix3f att_rotation;
@@ -171,6 +181,9 @@ private:
 
     const char *controller_ip = "127.0.0.1";
     uint16_t controller_port = 18083;
+
+    pthread_t thread;
+    AP_HAL::Semaphore *mutex;
 };
 
 

libraries/SITL/SIM_XPlane.cpp

@@ -336,26 +336,13 @@ failed:
     }
 
     // advance time by 1ms
-    Vector3f rot_accel;
     frame_time_us = 1000;
     float delta_time = frame_time_us * 1e-6f;
 
     time_now_us += frame_time_us;
 
-    // extrapolate sensors
-    dcm.rotate(gyro * delta_time);
-    dcm.normalize();
-
-    // work out acceleration as seen by the accelerometers. It sees the kinematic
-    // acceleration (ie. real movement), plus gravity
-    accel_body = dcm.transposed() * (accel_earth + Vector3f(0,0,-GRAVITY_MSS));
-
-    // new velocity and position vectors
-    velocity_ef += accel_earth * delta_time;
-    position += velocity_ef * delta_time;
-    velocity_air_ef = velocity_ef + wind_ef;
-    velocity_air_bf = dcm.transposed() * velocity_air_ef;
-
+    extrapolate_sensors(delta_time);
+    
     update_position();
     time_advance();
     update_mag_field_bf();