Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-06 16:03:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: report data and frame rates in XPlane

Browse Source 
and avoid time going backwards if possible
This commit is contained in:
Andrew Tridgell 2016-06-20 11:18:14 +10:00
parent 6530c3bd91
commit 51fff00871
2 changed files with 36 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/SITL/SIM_XPlane.cpp
View File

@ -102,7 +102,15 @@ bool XPlane::receive_data(void)
1U << Joystick1 | 1U << ThrottleCommand); 1U << Joystick1 | 1U << ThrottleCommand);
Location loc {}; Location loc {};
Vector3f pos; Vector3f pos;
ssize_t len = socket_in.recv(pkt, sizeof(pkt), 1); uint32_t wait_time_ms = 1;
uint32_t now = AP_HAL::millis();
// if we are about to get another frame from X-Plane then wait longer
if (xplane_frame_time > wait_time_ms &&
now+1 >= last_data_time_ms + xplane_frame_time) {
wait_time_ms = 10;
}
ssize_t len = socket_in.recv(pkt, sizeof(pkt), wait_time_ms);
if (len < pkt_len+5 || memcmp(pkt, "DATA@", 5) != 0) { if (len < pkt_len+5 || memcmp(pkt, "DATA@", 5) != 0) {
// not a data packet we understand // not a data packet we understand
@ -131,7 +139,7 @@ bool XPlane::receive_data(void)
uint64_t tus = data[3] * 1.0e6f; uint64_t tus = data[3] * 1.0e6f;
if (tus + time_base_us <= time_now_us) { if (tus + time_base_us <= time_now_us) {
uint64_t tdiff = time_now_us - (tus + time_base_us); uint64_t tdiff = time_now_us - (tus + time_base_us);
if (tdiff > 1e6) { if (tdiff > 1e6f) {
printf("X-Plane time reset %lu\n", (unsigned long)tdiff); printf("X-Plane time reset %lu\n", (unsigned long)tdiff);
} }
time_base_us = time_now_us - tus; time_base_us = time_now_us - tus;
@ -257,7 +265,13 @@ bool XPlane::receive_data(void)
update_mag_field_bf(); update_mag_field_bf();
if (now > last_data_time_ms && now - last_data_time_ms < 100) {
xplane_frame_time = now - last_data_time_ms;
}
last_data_time_ms = AP_HAL::millis(); last_data_time_ms = AP_HAL::millis();
report.data_count++;
report.frame_count++;
return true; return true;
failed: failed:
@ -289,6 +303,7 @@ failed:
update_position(); update_position();
update_mag_field_bf(); update_mag_field_bf();
report.frame_count++;
return false; return false;
} }
@ -339,6 +354,18 @@ void XPlane::update(const struct sitl_input &input)
send_data(input); send_data(input);
} }
uint32_t now = AP_HAL::millis();
if (report.last_report_ms == 0) {
report.last_report_ms = now;
}
if (now - report.last_report_ms > 5000) {
float dt = (now - report.last_report_ms) * 1.0e-3f;
printf("Data rate: %.1f FPS Frame rate: %.1f FPS\n",
report.data_count/dt, report.frame_count/dt);
report.last_report_ms = now;
report.data_count = 0;
report.frame_count = 0;
}
} }
} // namespace SITL } // namespace SITL

6
libraries/SITL/SIM_XPlane.h
View File

@ -58,6 +58,12 @@ private:
Vector3f accel_earth; Vector3f accel_earth;
float throttle_sent = -1; float throttle_sent = -1;
bool connected = false; bool connected = false;
uint32_t xplane_frame_time;
struct {
uint32_t last_report_ms;
uint32_t data_count;
uint32_t frame_count;
} report;
// throttle joystick input is very weird. See comments in the main code // throttle joystick input is very weird. See comments in the main code
const uint32_t throttle_magic = 123; const uint32_t throttle_magic = 123;