diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index f6ef67f386..9b096e02ab 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -128,7 +128,6 @@ void LR_MsgHandler_GPS2::process_message(uint8_t *msg) update_from_msg_gps(1, msg); } - void LR_MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg) { uint64_t time_us; @@ -184,6 +183,34 @@ void LR_MsgHandler_GPS::process_message(uint8_t *msg) } +void LR_MsgHandler_GPA_Base::update_from_msg_gpa(uint8_t gps_offset, uint8_t *msg) +{ + uint64_t time_us; + require_field(msg, "TimeUS", time_us); + wait_timestamp_usec(time_us); + + uint16_t vdop, hacc, vacc, sacc; + require_field(msg, "VDop", vdop); + require_field(msg, "HAcc", hacc); + require_field(msg, "VAcc", vacc); + require_field(msg, "SAcc", sacc); + + gps.setHIL_Accuracy(gps_offset, vdop*0.01f, hacc*0.01f, vacc*0.01f, sacc*0.01f); +} + +void LR_MsgHandler_GPA::process_message(uint8_t *msg) +{ + update_from_msg_gpa(0, msg); +} + + +void LR_MsgHandler_GPA2::process_message(uint8_t *msg) +{ + update_from_msg_gpa(1, msg); +} + + + void LR_MsgHandler_IMU2::process_message(uint8_t *msg) { update_from_msg_imu(1, msg); diff --git a/Tools/Replay/LR_MsgHandler.h b/Tools/Replay/LR_MsgHandler.h index f67037a562..87a8dd2cb4 100644 --- a/Tools/Replay/LR_MsgHandler.h +++ b/Tools/Replay/LR_MsgHandler.h @@ -153,7 +153,6 @@ private: uint32_t &ground_alt_cm; }; - class LR_MsgHandler_GPS : public LR_MsgHandler_GPS_Base { public: @@ -190,6 +189,50 @@ private: uint32_t &ground_alt_cm; }; +class LR_MsgHandler_GPA_Base : public LR_MsgHandler +{ + +public: + LR_MsgHandler_GPA_Base(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, AP_GPS &_gps) + : LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), gps(_gps) { }; + +protected: + void update_from_msg_gpa(uint8_t imu_offset, uint8_t *data); + +private: + AP_GPS &gps; +}; + + +class LR_MsgHandler_GPA : public LR_MsgHandler_GPA_Base +{ +public: + LR_MsgHandler_GPA(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, AP_GPS &_gps) + : LR_MsgHandler_GPA_Base(_f, _dataflash,_last_timestamp_usec, + _gps), gps(_gps) { }; + + void process_message(uint8_t *msg); + +private: + AP_GPS &gps; +}; + +class LR_MsgHandler_GPA2 : public LR_MsgHandler_GPA_Base +{ +public: + LR_MsgHandler_GPA2(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, AP_GPS &_gps) + : LR_MsgHandler_GPA_Base(_f, _dataflash, _last_timestamp_usec, + _gps), gps(_gps) { }; + virtual void process_message(uint8_t *msg); +private: + AP_GPS &gps; +}; + + + class LR_MsgHandler_IMU_Base : public LR_MsgHandler diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index 8897214686..8f66df41e3 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -178,6 +178,15 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) msgparser[f.type] = new LR_MsgHandler_GPS2(formats[f.type], dataflash, last_timestamp_usec, gps, ground_alt_cm); + } else if (streq(name, "GPA")) { + msgparser[f.type] = new LR_MsgHandler_GPA(formats[f.type], + dataflash, + last_timestamp_usec, + gps); + } else if (streq(name, "GPA2")) { + msgparser[f.type] = new LR_MsgHandler_GPA2(formats[f.type], dataflash, + last_timestamp_usec, + gps); } else if (streq(name, "MSG")) { msgparser[f.type] = new LR_MsgHandler_MSG(formats[f.type], dataflash, last_timestamp_usec,