diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index fe074e9b45..6669643f1a 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -32,11 +32,17 @@ #include #include #include +#include using namespace SITL; extern const AP_HAL::HAL& hal; +// the SITL HAL can add information about pausing the simulation and its effect on the UART. Not present when we're compiling for simulation-on-hardware +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +extern const HAL_SITL& hal_sitl; +#endif + /* parent class for all simulator types */ @@ -452,6 +458,14 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) } #if HAL_LOGGING_ENABLED +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + // the SITL HAL can add information about pausing the simulation + // and its effect on the UART. Not present when we're compiling + // for simulation-on-hardware + const uint32_t full_count = hal_sitl.get_uart_output_full_queue_count(); +#else + const uint32_t full_count = 0; +#endif // for EKF comparison log relhome pos and velocity at loop rate static uint16_t last_ticks; uint16_t ticks = AP::scheduler().ticks(); @@ -468,15 +482,20 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) // @Field: VD: Velocity down // @Field: As: Airspeed // @Field: ASpdU: Achieved simulation speedup value +// @Field: UFC: Number of times simulation paused for serial0 output Vector3d pos = get_position_relhome(); Vector3f vel = get_velocity_ef(); - 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, - achieved_rate_hz/rate_hz); + AP::logger().WriteStreaming( + "SIM2", + "TimeUS,PN,PE,PD,VN,VE,VD,As,ASpdU,UFC", + "QdddfffffI", + AP_HAL::micros64(), + pos.x, pos.y, pos.z, + vel.x, vel.y, vel.z, + airspeed_pitot, + achieved_rate_hz/rate_hz, + full_count + ); } #endif }