diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 22b09af2e7..a283f50a5d 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -32,7 +32,7 @@ #include #include -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; +} + + diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 805c6980f9..b3dc8d761e 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/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; diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index 0d98e54e2e..87fa87ff27 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/libraries/SITL/SIM_FlightAxis.cpp @@ -28,10 +28,11 @@ #include #include +#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; + } +} diff --git a/libraries/SITL/SIM_FlightAxis.h b/libraries/SITL/SIM_FlightAxis.h index 28644095ee..0a9defbbf2 100644 --- a/libraries/SITL/SIM_FlightAxis.h +++ b/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; }; diff --git a/libraries/SITL/SIM_XPlane.cpp b/libraries/SITL/SIM_XPlane.cpp index 2b4dc8f02b..053cb74668 100644 --- a/libraries/SITL/SIM_XPlane.cpp +++ b/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();