From 52fb663b09612a66c5a5ee51e82d32d5d4f6ac47 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 10 May 2024 18:18:22 +1000 Subject: [PATCH] Tools: support ExternalAHRS for Replay --- Tools/Replay/LR_MsgHandler.cpp | 18 ++++++++++++------ Tools/Replay/LR_MsgHandler.h | 10 ++++++++-- Tools/Replay/LogReader.cpp | 33 +++++++++++++++++---------------- Tools/Replay/LogReader.h | 3 ++- Tools/Replay/Parameters.h | 1 + Tools/Replay/Replay.cpp | 7 +++++++ Tools/Replay/Replay.h | 3 ++- 7 files changed, 49 insertions(+), 26 deletions(-) diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index cbdca233afaf68..ea2db67cdfb787 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -39,6 +39,12 @@ void LR_MsgHandler_RFRF::process_message(uint8_t *msgbytes) } #undef MAP_FLAG AP::dal().handle_message(msg, ekf2, ekf3); + if (eahrs.get_name() == nullptr) { + eahrs.init(); + } + if (eahrs.get_name() != nullptr) { + eahrs.update(); + } } void LR_MsgHandler_RFRN::process_message(uint8_t *msgbytes) @@ -75,7 +81,7 @@ void LR_MsgHandler_REV2::process_message(uint8_t *msgbytes) break; } if (replay_force_ekf3) { - LR_MsgHandler_REV3 h{f, ekf2, ekf3}; + LR_MsgHandler_REV3 h{f, ekf2, ekf3, eahrs}; h.process_message(msgbytes); } } @@ -90,7 +96,7 @@ void LR_MsgHandler_RSO2::process_message(uint8_t *msgbytes) ekf2.setOriginLLH(loc); if (replay_force_ekf3) { - LR_MsgHandler_RSO2 h{f, ekf2, ekf3}; + LR_MsgHandler_RSO2 h{f, ekf2, ekf3, eahrs}; h.process_message(msgbytes); } } @@ -100,7 +106,7 @@ void LR_MsgHandler_RWA2::process_message(uint8_t *msgbytes) MSG_CREATE(RWA2, msgbytes); ekf2.writeDefaultAirSpeed(msg.airspeed); if (replay_force_ekf3) { - LR_MsgHandler_RWA2 h{f, ekf2, ekf3}; + LR_MsgHandler_RWA2 h{f, ekf2, ekf3, eahrs}; h.process_message(msgbytes); } } @@ -136,7 +142,7 @@ void LR_MsgHandler_REV3::process_message(uint8_t *msgbytes) } if (replay_force_ekf2) { - LR_MsgHandler_REV2 h{f, ekf2, ekf3}; + LR_MsgHandler_REV2 h{f, ekf2, ekf3, eahrs}; h.process_message(msgbytes); } } @@ -150,7 +156,7 @@ void LR_MsgHandler_RSO3::process_message(uint8_t *msgbytes) loc.alt = msg.alt; ekf3.setOriginLLH(loc); if (replay_force_ekf2) { - LR_MsgHandler_RSO2 h{f, ekf2, ekf3}; + LR_MsgHandler_RSO2 h{f, ekf2, ekf3, eahrs}; h.process_message(msgbytes); } } @@ -160,7 +166,7 @@ void LR_MsgHandler_RWA3::process_message(uint8_t *msgbytes) MSG_CREATE(RWA3, msgbytes); ekf3.writeDefaultAirSpeed(msg.airspeed, msg.uncertainty); if (replay_force_ekf2) { - LR_MsgHandler_RWA2 h{f, ekf2, ekf3}; + LR_MsgHandler_RWA2 h{f, ekf2, ekf3, eahrs}; h.process_message(msgbytes); } } diff --git a/Tools/Replay/LR_MsgHandler.h b/Tools/Replay/LR_MsgHandler.h index 4e30a6571d35ce..a5632ba58c9bcf 100644 --- a/Tools/Replay/LR_MsgHandler.h +++ b/Tools/Replay/LR_MsgHandler.h @@ -5,6 +5,7 @@ #include #include #include +#include class LR_MsgHandler : public MsgHandler { public: @@ -28,15 +29,20 @@ class LR_MsgHandler_RFRH : public LR_MsgHandler class LR_MsgHandler_EKF : public LR_MsgHandler { public: - LR_MsgHandler_EKF(struct log_Format &_f, NavEKF2 &_ekf2, NavEKF3 &_ekf3) : + LR_MsgHandler_EKF(struct log_Format &_f, + NavEKF2 &_ekf2, + NavEKF3 &_ekf3, + AP_ExternalAHRS &_eahrs) : LR_MsgHandler(_f), ekf2(_ekf2), - ekf3(_ekf3) {} + ekf3(_ekf3), + eahrs(_eahrs){} using LR_MsgHandler::LR_MsgHandler; virtual void process_message(uint8_t *msg) override = 0; protected: NavEKF2 &ekf2; NavEKF3 &ekf3; + AP_ExternalAHRS &eahrs; }; class LR_MsgHandler_RFRF : public LR_MsgHandler_EKF diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index 7c1b1a43948695..957e24d7106ae7 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -22,11 +22,12 @@ extern struct user_parameter *user_parameters; -LogReader::LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf2, NavEKF3 &_ekf3) : +LogReader::LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf2, NavEKF3 &_ekf3, AP_ExternalAHRS &_eahrs) : AP_LoggerFileReader(), _log_structure(log_structure), ekf2(_ekf2), - ekf3(_ekf3) + ekf3(_ekf3), + eahrs(_eahrs) { } @@ -65,23 +66,23 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) } else if (streq(name, "RFRH")) { msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RFRH(formats[f.type]); } else if (streq(name, "RFRF")) { - msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RFRF(formats[f.type], ekf2, ekf3); + msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RFRF(formats[f.type], ekf2, ekf3, eahrs); } else if (streq(name, "RFRN")) { msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RFRN(formats[f.type]); } else if (streq(name, "REV2")) { - msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REV2(formats[f.type], ekf2, ekf3); + msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REV2(formats[f.type], ekf2, ekf3, eahrs); } else if (streq(name, "RSO2")) { - msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSO2(formats[f.type], ekf2, ekf3); + msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSO2(formats[f.type], ekf2, ekf3, eahrs); } else if (streq(name, "RWA2")) { - msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWA2(formats[f.type], ekf2, ekf3); + msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWA2(formats[f.type], ekf2, ekf3, eahrs); } else if (streq(name, "REV3")) { - msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REV3(formats[f.type], ekf2, ekf3); + msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REV3(formats[f.type], ekf2, ekf3, eahrs); } else if (streq(name, "RSO3")) { - msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSO3(formats[f.type], ekf2, ekf3); + msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSO3(formats[f.type], ekf2, ekf3, eahrs); } else if (streq(name, "RWA3")) { - msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWA3(formats[f.type], ekf2, ekf3); + msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWA3(formats[f.type], ekf2, ekf3, eahrs); } else if (streq(name, "REY3")) { - msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REY3(formats[f.type], ekf2, ekf3); + msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REY3(formats[f.type], ekf2, ekf3, eahrs); } else if (streq(name, "RISH")) { msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RISH(formats[f.type]); } else if (streq(name, "RISI")) { @@ -115,17 +116,17 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) } else if (streq(name, "RVOH")) { msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RVOH(formats[f.type]); } else if (streq(name, "ROFH")) { - msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_ROFH(formats[f.type], ekf2, ekf3); + msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_ROFH(formats[f.type], ekf2, ekf3, eahrs); } else if (streq(name, "REPH")) { - msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REPH(formats[f.type], ekf2, ekf3); + msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REPH(formats[f.type], ekf2, ekf3, eahrs); } else if (streq(name, "RSLL")) { - msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSLL(formats[f.type], ekf2, ekf3); + msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSLL(formats[f.type], ekf2, ekf3, eahrs); } else if (streq(name, "REVH")) { - msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REVH(formats[f.type], ekf2, ekf3); + msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REVH(formats[f.type], ekf2, ekf3, eahrs); } else if (streq(name, "RWOH")) { - msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWOH(formats[f.type], ekf2, ekf3); + msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWOH(formats[f.type], ekf2, ekf3, eahrs); } else if (streq(name, "RBOH")) { - msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RBOH(formats[f.type], ekf2, ekf3); + msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RBOH(formats[f.type], ekf2, ekf3, eahrs); } else { // debug(" No parser for (%s)\n", name); } diff --git a/Tools/Replay/LogReader.h b/Tools/Replay/LogReader.h index 340828c9670b23..e1ddf9316722b8 100644 --- a/Tools/Replay/LogReader.h +++ b/Tools/Replay/LogReader.h @@ -8,7 +8,7 @@ class LogReader : public AP_LoggerFileReader { public: - LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf, NavEKF3 &_ekf3); + LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf, NavEKF3 &_ekf3, AP_ExternalAHRS &_eahrs); VehicleType::vehicle_type vehicle; @@ -26,6 +26,7 @@ class LogReader : public AP_LoggerFileReader NavEKF2 &ekf2; NavEKF3 &ekf3; + AP_ExternalAHRS &eahrs; struct LogStructure *_log_structure; uint8_t _log_structure_count; diff --git a/Tools/Replay/Parameters.h b/Tools/Replay/Parameters.h index af17de6997d4f1..ef288c42fe1236 100644 --- a/Tools/Replay/Parameters.h +++ b/Tools/Replay/Parameters.h @@ -17,6 +17,7 @@ class Parameters { k_param_logger, k_param_NavEKF3, k_param_gps, + k_param_eahrs, }; AP_Int8 dummy; }; diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index d705933b701805..d445b3e5075852 100644 --- a/Tools/Replay/Replay.cpp +++ b/Tools/Replay/Replay.cpp @@ -76,12 +76,18 @@ const AP_Param::Info ReplayVehicle::var_info[] = { // @Group: GPS // @Path: ../libraries/AP_GPS/AP_GPS.cpp GOBJECT(gps, "GPS", AP_GPS), + + // @Group: EAHRS + // @Path: ../libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp + GOBJECTPTR(eahrs, "EAHRS", AP_ExternalAHRS), AP_VAREND }; void ReplayVehicle::load_parameters(void) { + eahrs = &AP::externalAHRS(); + AP_Param::check_var_info(); StorageManager::erase(); @@ -239,6 +245,7 @@ void Replay::setup() ::printf("open(%s): %m\n", filename); exit(1); } + _vehicle.eahrs = &AP::externalAHRS(); } void Replay::loop() diff --git a/Tools/Replay/Replay.h b/Tools/Replay/Replay.h index 6e4a6c0429e82e..27ed8605a763ca 100644 --- a/Tools/Replay/Replay.h +++ b/Tools/Replay/Replay.h @@ -57,6 +57,7 @@ class ReplayVehicle : public AP_Vehicle { NavEKF2 ekf2; NavEKF3 ekf3; + AP_ExternalAHRS *eahrs; SRV_Channels servo_channels; @@ -99,7 +100,7 @@ class Replay : public AP_HAL::HAL::Callbacks { const char *filename; ReplayVehicle &_vehicle; - LogReader reader{_vehicle.log_structure, _vehicle.ekf2, _vehicle.ekf3}; + LogReader reader{_vehicle.log_structure, _vehicle.ekf2, _vehicle.ekf3, AP::externalAHRS()}; void _parse_command_line(uint8_t argc, char * const argv[]);