diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index f2620a8460..6ad31a1173 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -298,9 +298,9 @@ void Aircraft::sync_frame_time(void) uint32_t now_ms = last_wall_time_us / 1000ULL; float dt_wall = (now_ms - last_fps_report_ms) * 0.001; - if (dt_wall > 2.0) { + if (dt_wall > 0.01) { // 0.01s average + achieved_rate_hz = (frame_counter - last_frame_count) / dt_wall; #if 0 - const float achieved_rate_hz = (frame_counter - last_frame_count) / dt_wall; ::printf("Rate: target:%.1f achieved:%.1f speedup %.1f/%.1f\n", rate_hz*target_speedup, achieved_rate_hz, achieved_rate_hz/rate_hz, target_speedup); @@ -471,14 +471,16 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) // @Field: VE: Velocity east // @Field: VD: Velocity down // @Field: As: Airspeed +// @Field: ASpdU: Achieved simulation speedup value Vector3d pos = get_position_relhome(); Vector3f vel = get_velocity_ef(); - AP::logger().WriteStreaming("SIM2", "TimeUS,PN,PE,PD,VN,VE,VD,As", - "Qdddffff", + AP::logger().WriteStreaming("SIM2", "TimeUS,PN,PE,PD,VN,VE,VD,As,ASpdU", + "Qdddfffff", AP_HAL::micros64(), pos.x, pos.y, pos.z, vel.x, vel.y, vel.z, - airspeed_pitot); + airspeed_pitot, + achieved_rate_hz/rate_hz); } } diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 645f78c7fc..04ef0e946a 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -225,6 +225,7 @@ protected: uint64_t frame_time_us; uint64_t last_wall_time_us; uint32_t last_fps_report_ms; + float achieved_rate_hz; // achieved speedup rate int64_t sleep_debt_us; uint32_t last_frame_count; uint8_t instance;