From 95f51123d7aa7c23b94209a3b3afa727b1e891a8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 30 Dec 2013 09:47:50 +1100 Subject: [PATCH] AP_NavEKF: added plot.dat output --- .../examples/AP_NavEKF/AP_NavEKF.pde | 26 ++++++++++++++++--- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde b/libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde index 454e030d28..267e05e27b 100644 --- a/libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde +++ b/libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde @@ -65,6 +65,8 @@ static void delay_cb() LogReader.wait_type(LOG_GPS_MSG); } +static FILE *plotf; + void setup() { ::printf("Starting\n"); @@ -99,6 +101,9 @@ void setup() compass.set_initial_location(g_gps->latitude, g_gps->longitude); NavEKF.InitialiseFilter(); + + plotf = fopen("plot.dat", "w"); + fprintf(plotf, "time ATT.Roll ATT.Pitch ATT.Yaw AHRS.Roll AHRS.Pitch AHRS.Yaw EKF.Roll EKF.Pitch EKF.Yaw\n"); } void loop() @@ -107,6 +112,7 @@ void loop() uint8_t type; if (!LogReader.update(type)) { ::printf("End of log\n"); + fclose(plotf); exit(0); } switch (type) { @@ -118,14 +124,26 @@ void loop() case LOG_ATTITUDE_MSG: { Vector3f ekf_euler; NavEKF.getEulerAngles(ekf_euler); - ::printf("t=%.3f AHRS: (%.1f %.1f %.1f) ATT: (%.1f %.1f %.1f) EKF: (%.1f %.1f %.1f) ALT: %.1f GPS: %u %f %f\n", + fprintf(plotf, "%.3f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f\n", + hal.scheduler->millis() * 0.001f, + LogReader.get_attitude().x, + LogReader.get_attitude().y, + LogReader.get_attitude().z, + ahrs.roll_sensor*0.01f, + ahrs.pitch_sensor*0.01f, + ahrs.yaw_sensor*0.01f, + degrees(ekf_euler.x), + degrees(ekf_euler.y), + degrees(ekf_euler.z)); + + ::printf("t=%.3f ATT: (%.1f %.1f %.1f) AHRS: (%.1f %.1f %.1f) EKF: (%.1f %.1f %.1f) ALT: %.1f GPS: %u %f %f\n", hal.scheduler->millis() * 0.001f, - ahrs.roll_sensor*0.01f, - ahrs.pitch_sensor*0.01f, - ahrs.yaw_sensor*0.01f, LogReader.get_attitude().x, LogReader.get_attitude().y, LogReader.get_attitude().z, + ahrs.roll_sensor*0.01f, + ahrs.pitch_sensor*0.01f, + ahrs.yaw_sensor*0.01f, degrees(ekf_euler.x), degrees(ekf_euler.y), degrees(ekf_euler.z),