Replay: added checking of solution

Tools/Replay/LR_MsgHandler.cpp

@@ -83,6 +83,18 @@ void LR_MsgHandler_ATT::process_message(uint8_t *msg)
     attitude_from_msg(msg, attitude, "Roll", "Pitch", "Yaw");
 }
 
+void LR_MsgHandler_CHEK::process_message(uint8_t *msg)
+{
+    wait_timestamp_from_msg(msg);
+    check_state.time_us = hal.scheduler->micros64();
+    attitude_from_msg(msg, check_state.euler, "Roll", "Pitch", "Yaw");
+    check_state.euler *= radians(1);
+    location_from_msg(msg, check_state.pos, "Lat", "Lng", "Alt");
+    require_field(msg, "VN", check_state.velocity.x);
+    require_field(msg, "VE", check_state.velocity.y);
+    require_field(msg, "VD", check_state.velocity.z);
+}
+
 
 void LR_MsgHandler_BARO::process_message(uint8_t *msg)
 {

Tools/Replay/LR_MsgHandler.h

@@ -11,6 +11,14 @@ public:
     virtual void process_message(uint8_t *msg) = 0;
     bool set_parameter(const char *name, float value);
 
+    // state for CHEK message
+    struct CheckState {
+        uint64_t time_us;
+        Vector3f euler;
+        Location pos;
+        Vector3f velocity;
+    };
+
 protected:
     DataFlash_Class &dataflash;
     void wait_timestamp(uint32_t timestamp);
@@ -87,6 +95,20 @@ private:
 };
 
 
+class LR_MsgHandler_CHEK : public LR_MsgHandler
+{
+public:
+    LR_MsgHandler_CHEK(log_Format &_f, DataFlash_Class &_dataflash,
+                       uint64_t &_last_timestamp_usec, CheckState &_check_state)
+        : LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), 
+          check_state(_check_state)
+        { };
+    virtual void process_message(uint8_t *msg);
+
+private:
+    CheckState &check_state;
+};
+
 class LR_MsgHandler_BARO : public LR_MsgHandler
 {
 public:

Tools/Replay/LogReader.cpp

@@ -187,6 +187,10 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) {
 	} else if (streq(name, "FRAM")) {
 	    msgparser[f.type] = new LR_MsgHandler_FRAM(formats[f.type], dataflash,
                                                     last_timestamp_usec);
+	} else if (streq(name, "CHEK")) {
+	  msgparser[f.type] = new LR_MsgHandler_CHEK(formats[f.type], dataflash,
+                                                     last_timestamp_usec,
+                                                     check_state);
 	} else {
             ::printf("  No parser for (%s)\n", name);
 	}

Tools/Replay/LogReader.h

@@ -13,6 +13,7 @@ public:
     const Vector3f &get_inavpos(void) const { return inavpos; }
     const Vector3f &get_sim_attitude(void) const { return sim_attitude; }
     const float &get_relalt(void) const { return rel_altitude; }
+    const LR_MsgHandler::CheckState &get_check_state(void) const { return check_state; }
 
     VehicleType::vehicle_type vehicle;
 
@@ -50,6 +51,8 @@ private:
     float rel_altitude;
     uint64_t last_timestamp_usec;
 
+    LR_MsgHandler::CheckState check_state;
+
     bool installed_vehicle_specific_parsers;
     void maybe_install_vehicle_specific_parsers();