diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index cbdca233afaf68..f88f55c0c6d621 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -205,8 +205,50 @@ void LR_MsgHandler_RBRI::process_message(uint8_t *msgbytes) AP::dal().handle_message(msg); } +// RRNH - Replay RaNgefinder Header constructor +LR_MsgHandler_RRNH::LR_MsgHandler_RRNH(struct log_Format &_f) : + LR_MsgHandler(_f) +{ + // the checksum here is the checksum of the old "struct log_Format + // RRNH" before the type was changed from int16_t to float. It + // corresponds to log_RRNH_with_int16_max_distance, below. + if (crc_crc32(0, (uint8_t*)&_f, sizeof(_f)) == 0xDD71C118) { + max_distance_int16_to_float_transform_required = true; + } +} + void LR_MsgHandler_RRNH::process_message(uint8_t *msgbytes) { + if (max_distance_int16_to_float_transform_required) { + // this structure was copied out of AP_DAL.cpp before being + // modified there. _end has been trimmed out + + struct log_RRNH_with_int16_t_distance { + // this is rotation-pitch-270! + int16_t ground_clearance_cm; + int16_t max_distance_cm; + uint8_t num_sensors; + }; + + // this is the same structure as found in + // AP_DAL/LogStructure.h, sans __end + struct log_RRNH_with_float_distance { + float max_distance; + int16_t ground_clearance_cm; + uint8_t num_sensors; + }; + // note that if fields are added to RRNH message then this + // will be incorrect! + const log_RRNH_with_int16_t_distance &tmp = *((log_RRNH_with_int16_t_distance*)&msgbytes[3]); + const log_RRNH msg { + max_distance : tmp.max_distance_cm * 0.01, + ground_clearance_cm : tmp.ground_clearance_cm, + num_sensors : tmp.num_sensors, + }; + AP::dal().handle_message(msg); + return; + } + MSG_CREATE(RRNH, msgbytes); AP::dal().handle_message(msg); } diff --git a/Tools/Replay/LR_MsgHandler.h b/Tools/Replay/LR_MsgHandler.h index 4e30a6571d35ce..b3a17f3991eb37 100644 --- a/Tools/Replay/LR_MsgHandler.h +++ b/Tools/Replay/LR_MsgHandler.h @@ -176,11 +176,14 @@ class LR_MsgHandler_RBRI : public LR_MsgHandler void process_message(uint8_t *msg) override; }; +// RRNH - Replay RaNgefinder Header class LR_MsgHandler_RRNH : public LR_MsgHandler { public: - using LR_MsgHandler::LR_MsgHandler; + LR_MsgHandler_RRNH(struct log_Format &_f); void process_message(uint8_t *msg) override; +private: + bool max_distance_int16_to_float_transform_required; }; class LR_MsgHandler_RRNI : public LR_MsgHandler {