diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 12daba1634..4e0e028bb1 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -33,6 +33,7 @@ #include #include #include +#include using namespace SITL; @@ -455,6 +456,29 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) set_speedup(sitl->speedup); last_speedup = sitl->speedup; } + + // for EKF comparison log relhome pos and velocity at loop rate + static uint16_t last_ticks; + uint16_t ticks = AP::scheduler().ticks(); + if (last_ticks != ticks) { + last_ticks = ticks; +// @LoggerMessage: SIM2 +// @Description: Additional simulator state +// @Field: TimeUS: Time since system startup +// @Field: PN: North position from home +// @Field: PE: East position from home +// @Field: PD: Down position from home +// @Field: VN: Velocity north +// @Field: VE: Velocity east +// @Field: VD: Velocity down + Vector3d pos = get_position_relhome(); + Vector3f vel = get_velocity_ef(); + AP::logger().Write("SIM2", "TimeUS,PN,PE,PD,VN,VE,VD", + "Qdddfff", + AP_HAL::micros64(), + pos.x, pos.y, pos.z, + vel.x, vel.y, vel.z); + } } float Aircraft::rangefinder_range() const