From 093481786edfb4e8f46e465c5727afaf83adb4a2 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 3 Jan 2014 16:07:39 +1100
Subject: [PATCH] AP_NavEKF: added SIM state to playback tool

---
 libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde | 7 +++++--
 libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.cpp | 8 ++++++++
 libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.h   | 2 ++
 libraries/AP_NavEKF/examples/AP_NavEKF/pitch.gnu     | 2 +-
 libraries/AP_NavEKF/examples/AP_NavEKF/roll.gnu      | 2 +-
 5 files changed, 17 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 6e7869a712..bd4e3825c6 100644
--- a/libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde
+++ b/libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde
@@ -102,7 +102,7 @@ void setup()
 
     plotf = fopen("plot.dat", "w");
     plotf2 = fopen("plot2.dat", "w");
-    fprintf(plotf, "time ATT.Roll ATT.Pitch ATT.Yaw AHRS.Roll AHRS.Pitch AHRS.Yaw EKF.Roll EKF.Pitch EKF.Yaw\n");
+    fprintf(plotf, "time SIM.Roll SIM.Pitch SIM.Yaw ATT.Roll ATT.Pitch ATT.Yaw AHRS.Roll AHRS.Pitch AHRS.Yaw EKF.Roll EKF.Pitch EKF.Yaw\n");
     fprintf(plotf2, "time E1 E2 E3 VN VE VD PN PE PD GX GY GZ AX AY AZ MN ME MD MX MY MZ E1ref E2ref E3ref\n");
 }
 
@@ -138,8 +138,11 @@ void loop()
             NavEKF.getMagXYZ(magXYZ);
             float temp = degrees(ekf_euler.z);
             if (temp < 0.0f) temp = temp + 360.0f;
-            fprintf(plotf, "%.3f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f\n",
+            fprintf(plotf, "%.3f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f\n",
                     hal.scheduler->millis() * 0.001f,
+                    LogReader.get_sim_attitude().x,
+                    LogReader.get_sim_attitude().y,
+                    LogReader.get_sim_attitude().z,
                     LogReader.get_attitude().x,
                     LogReader.get_attitude().y,
                     LogReader.get_attitude().z,
diff --git a/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.cpp b/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.cpp
index ef5b455397..c437e0509f 100644
--- a/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.cpp
+++ b/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.cpp
@@ -151,6 +151,14 @@ bool LogReader::update(uint8_t &type)
         attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
         break;
     }
+
+    case LOG_SIMSTATE_MSG: {
+        struct log_AHRS msg;
+        memcpy(&msg, data, sizeof(msg));
+        wait_timestamp(msg.time_ms);
+        sim_attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
+        break;
+    }
     }
 
     type = f.type;
diff --git a/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.h b/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.h
index 3f88d677ba..94aaf6b20a 100644
--- a/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.h
+++ b/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.h
@@ -14,6 +14,7 @@ public:
     bool update(uint8_t &type);
     bool wait_type(uint8_t type);
     const Vector3f &get_attitude(void) const { return attitude; }
+    const Vector3f &get_sim_attitude(void) const { return sim_attitude; }
 
 private:
     int fd;
@@ -28,6 +29,7 @@ private:
     struct log_Format formats[32];
 
     Vector3f attitude;
+    Vector3f sim_attitude;
 
     void wait_timestamp(uint32_t timestamp);
 };
diff --git a/libraries/AP_NavEKF/examples/AP_NavEKF/pitch.gnu b/libraries/AP_NavEKF/examples/AP_NavEKF/pitch.gnu
index 34d4fe26b8..1bbfd6acd2 100644
--- a/libraries/AP_NavEKF/examples/AP_NavEKF/pitch.gnu
+++ b/libraries/AP_NavEKF/examples/AP_NavEKF/pitch.gnu
@@ -1,3 +1,3 @@
 set style data lines
-plot "plot.dat" using "ATT.Pitch", "" using "AHRS.Pitch", "" using "EKF.Pitch"
+plot "plot.dat" using "SIM.Pitch", "" using "AHRS.Pitch", "" using "EKF.Pitch"
 pause -1 "hit return to exit"
diff --git a/libraries/AP_NavEKF/examples/AP_NavEKF/roll.gnu b/libraries/AP_NavEKF/examples/AP_NavEKF/roll.gnu
index 5f2573ab13..a9622852ef 100644
--- a/libraries/AP_NavEKF/examples/AP_NavEKF/roll.gnu
+++ b/libraries/AP_NavEKF/examples/AP_NavEKF/roll.gnu
@@ -1,3 +1,3 @@
 set style data lines
-plot "plot.dat" using "ATT.Roll", "" using "AHRS.Roll", "" using "EKF.Roll"
+plot "plot.dat" using "SIM.Roll", "" using "AHRS.Roll", "" using "EKF.Roll"
 pause -1 "hit return to exit"