Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

SITL: debug code for logging raw SITL data

Browse Source
This commit is contained in:
Andrew Tridgell 2016-06-04 14:21:21 +10:00
parent f6a7c1839a
commit 7c227ac96c
1 changed files with 13 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/SITL/SIM_Aircraft.cpp
View File

@ -29,6 +29,8 @@
#include <Mmsystem.h>
#endif
#include <DataFlash/DataFlash.h>
namespace SITL {
/*
@ -123,11 +125,8 @@ bool Aircraft::on_ground(const Vector3f &pos) const
*/
void Aircraft::update_position(void)
{
float bearing = degrees(atan2f(position.y, position.x));
float distance = sqrtf(sq(position.x) + sq(position.y));
location = home;
location_update(location, bearing, distance);
location_offset(location, position.x, position.y);
location.alt = home.alt - position.z*100.0f;
@ -140,6 +139,16 @@ void Aircraft::update_position(void)
if (use_time_sync) {
sync_frame_time();
}
#if 0
// logging of raw sitl data
Vector3f accel_ef = dcm * accel_body;
DataFlash_Class::instance()->Log_Write("SITL", "TimeUS,VN,VE,VD,AN,AE,AD,PN,PE,PD", "Qfffffffff",
AP_HAL::micros64(),
velocity_ef.x, velocity_ef.y, velocity_ef.z,
accel_ef.x, accel_ef.y, accel_ef.z,
position.x, position.y, position.z);
#endif
}
/* advance time by deltat in seconds */