Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: added SIM2 message 

useful for EKF debugging

# Conflicts:
#	libraries/SITL/SIM_Aircraft.cpp
This commit is contained in:
Andrew Tridgell 2021-07-22 13:50:33 +10:00 committed by Randy Mackay
parent 34eb9f7328
commit 1f5d16bff8
1 changed files with 24 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/SITL/SIM_Aircraft.cpp
View File

@ -33,6 +33,7 @@
#include <AP_Logger/AP_Logger.h>
#include <AP_Param/AP_Param.h>
#include <AP_Declination/AP_Declination.h>
#include <AP_Scheduler/AP_Scheduler.h>
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