From 977ad4bbf67b7d8ecf50ea2584d495928a2ffbb6 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sun, 29 Dec 2013 22:56:54 +1100
Subject: [PATCH] AP_NavEKF: added dataflash log reader

this gives log playback into HIL
---
 .../examples/AP_NavEKF/LogReader.cpp          | 177 ++++++++++++++++++
 .../AP_NavEKF/examples/AP_NavEKF/LogReader.h  |  31 +++
 2 files changed, 208 insertions(+)
 create mode 100644 libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.cpp
 create mode 100644 libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.h

diff --git a/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.cpp b/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.cpp
new file mode 100644
index 0000000000..e5ff21f086
--- /dev/null
+++ b/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.cpp
@@ -0,0 +1,177 @@
+#include <AP_HAL.h>
+#include <AP_Common.h>
+#include <AP_Math.h>
+#include <AP_Airspeed.h>
+#include <AP_Compass.h>
+#include <AP_GPS.h>
+#include <AP_Compass.h>
+#include <AP_Baro.h>
+#include <AP_InertialSensor.h>
+#include <DataFlash.h>
+
+#include "LogReader.h"
+#include <stdio.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+
+extern const AP_HAL::HAL& hal;
+
+LogReader::LogReader(AP_InertialSensor &_ins, AP_Baro_HIL &_baro, AP_Compass_HIL &_compass, GPS *&_gps) :
+    fd(-1),
+    ins(_ins),
+    baro(_baro),
+    compass(_compass),
+    gps(_gps)
+{}
+
+bool LogReader::open_log(const char *logfile)
+{
+    fd = ::open(logfile, O_RDONLY);
+    if (fd == -1) {
+        return false;
+    }
+    return true;
+}
+
+
+struct PACKED log_Compass {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    int16_t mag_x;
+    int16_t mag_y;
+    int16_t mag_z;
+    int16_t offset_x;
+    int16_t offset_y;
+    int16_t offset_z;
+};
+
+struct PACKED log_Nav_Tuning {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    uint16_t yaw;
+    uint32_t wp_distance;
+    uint16_t target_bearing_cd;
+    uint16_t nav_bearing_cd;
+    int16_t altitude_error_cm;
+    int16_t airspeed_cm;
+    float   altitude;
+    uint32_t groundspeed_cm;
+};
+
+struct PACKED log_Attitude {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    int16_t roll;
+    int16_t pitch;
+    uint16_t yaw;
+    uint16_t error_rp;
+    uint16_t error_yaw;
+};
+
+bool LogReader::update(uint8_t &type)
+{
+    uint8_t hdr[3];
+    if (::read(fd, hdr, 3) != 3) {
+        return false;
+    }
+    if (hdr[0] != HEAD_BYTE1 || hdr[1] != HEAD_BYTE2) {
+        return false;
+    }
+
+    if (hdr[2] == LOG_FORMAT_MSG) {
+        struct log_Format &f = formats[num_formats];
+        memcpy(&f, hdr, 3);
+        if (::read(fd, &f.type, sizeof(f)-3) != sizeof(f)-3) {
+            return false;
+        }
+        num_formats++;
+        type = f.type;
+        return true;
+    }
+
+    uint8_t i;
+    for (i=0; i<num_formats; i++) {
+        if (formats[i].type == hdr[2]) break;
+    }
+    if (i == num_formats) {
+        return false;
+    }
+    const struct log_Format &f = formats[i];
+    
+    uint8_t data[f.length];
+    memcpy(data, hdr, 3);
+    if (::read(fd, &data[3], f.length-3) != f.length-3) {
+        return false;
+    }
+
+    switch (f.type) {
+    case LOG_IMU_MSG: {
+        struct log_IMU msg;
+        memcpy(&msg, data, sizeof(msg));
+        wait_timestamp(msg.timestamp);
+        ins.set_gyro(Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
+        ins.set_accel(Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
+        break;
+    }
+
+    case LOG_GPS_MSG: {
+        struct log_GPS msg;
+        memcpy(&msg, data, sizeof(msg));
+        wait_timestamp(msg.apm_time);
+        gps->setHIL(msg.apm_time,
+                    msg.latitude*1.0e-7f, 
+                    msg.longitude*1.0e-7f, 
+                    msg.altitude*1.0e-2f,
+                    msg.ground_speed*1.0e-2f, 
+                    msg.ground_course*1.0e-2f, 
+                    0, 
+                    msg.num_sats);
+        baro.setHIL(600.0 + msg.rel_altitude*1.0e-2f);
+        break;
+    }
+
+    case LOG_COMPASS_MSG: {
+        struct log_Compass msg;
+        memcpy(&msg, data, sizeof(msg));
+        wait_timestamp(msg.time_ms);
+        //compass.setHIL(Vector3i(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z));
+        compass.setHIL(Vector3i(msg.mag_x, msg.mag_y, msg.mag_z));
+        break;
+    }
+
+    case LOG_ATTITUDE_MSG: {
+        struct log_Attitude msg;
+        memcpy(&msg, data, sizeof(msg));
+        wait_timestamp(msg.time_ms);
+        attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
+        break;
+    }
+    }
+
+    type = f.type;
+
+    return true;
+}
+
+void LogReader::wait_timestamp(uint32_t timestamp)
+{
+    while (hal.scheduler->millis() < timestamp) {
+        hal.scheduler->delay(1);
+    }
+}
+
+bool LogReader::wait_type(uint8_t wtype)
+{
+    while (true) {
+        uint8_t type;
+        if (!update(type)) {
+            return false;
+        }
+        if (wtype == type) {
+            break;
+        }
+    }
+    return true;
+}
diff --git a/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.h b/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.h
new file mode 100644
index 0000000000..dde45f334b
--- /dev/null
+++ b/libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.h
@@ -0,0 +1,31 @@
+
+enum log_messages {
+    LOG_NTUN_MSG     = 2,
+    LOG_ATTITUDE_MSG = 10,
+    LOG_COMPASS_MSG  = 12
+};
+
+
+class LogReader 
+{
+public:
+    LogReader(AP_InertialSensor &_ins, AP_Baro_HIL &_baro, AP_Compass_HIL &_compass, GPS *&_gps);
+    bool open_log(const char *logfile);
+    bool update(uint8_t &type);
+    bool wait_type(uint8_t type);
+    const Vector3f &get_attitude(void) const { return attitude; }
+
+private:
+    int fd;
+    AP_InertialSensor &ins;
+    AP_Baro_HIL &baro;
+    AP_Compass_HIL &compass;
+    GPS *&gps;
+
+    uint8_t num_formats;
+    struct log_Format formats[32];
+
+    Vector3f attitude;
+
+    void wait_timestamp(uint32_t timestamp);
+};