Skip to content

Commit

Permalink
SITL: log number of times sim paused on serial0 buffer
Browse files Browse the repository at this point in the history
SITL pauses the simulation if we do not have a minimum amount of space in its out queue.

Log the number of times we do this.
  • Loading branch information
peterbarker committed Jun 8, 2024
1 parent 3341f10 commit 6a58cf5
Showing 1 changed file with 13 additions and 7 deletions.
20 changes: 13 additions & 7 deletions libraries/SITL/SIM_Aircraft.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,10 +32,12 @@
#include <AP_JSON/AP_JSON.h>
#include <AP_Filesystem/AP_Filesystem.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_HAL_SITL/HAL_SITL_Class.h>

using namespace SITL;

extern const AP_HAL::HAL& hal;
extern const HAL_SITL& hal_sitl;

/*
parent class for all simulator types
Expand Down Expand Up @@ -470,13 +472,17 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
// @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,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,
hal_sitl.get_uart_output_full_queue_count()
);
}
#endif
}
Expand Down

0 comments on commit 6a58cf5

Please sign in to comment.