diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index bc786eca5e..6f7423773f 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -23,6 +23,21 @@ void LR_MsgHandler_RFRH::process_message(uint8_t *msgbytes) void LR_MsgHandler_RFRF::process_message(uint8_t *msgbytes) { MSG_CREATE(RFRF, msgbytes); +#define MAP_FLAG(flag1, flag2) if (msg.frame_types & uint8_t(flag1)) msg.frame_types |= uint8_t(flag2) + /* + when we force an EKF we map the trigger flags over + */ + if (replay_force_ekf2) { + MAP_FLAG(AP_DAL::FrameType::InitialiseFilterEKF3, AP_DAL::FrameType::InitialiseFilterEKF2); + MAP_FLAG(AP_DAL::FrameType::UpdateFilterEKF3, AP_DAL::FrameType::UpdateFilterEKF2); + MAP_FLAG(AP_DAL::FrameType::LogWriteEKF3, AP_DAL::FrameType::LogWriteEKF2); + } + if (replay_force_ekf3) { + MAP_FLAG(AP_DAL::FrameType::InitialiseFilterEKF2, AP_DAL::FrameType::InitialiseFilterEKF3); + MAP_FLAG(AP_DAL::FrameType::UpdateFilterEKF2, AP_DAL::FrameType::UpdateFilterEKF3); + MAP_FLAG(AP_DAL::FrameType::LogWriteEKF2, AP_DAL::FrameType::LogWriteEKF3); + } +#undef MAP_FLAG AP::dal().handle_message(msg, ekf2, ekf3); } @@ -36,48 +51,52 @@ void LR_MsgHandler_REV2::process_message(uint8_t *msgbytes) { MSG_CREATE(REV2, msgbytes); - switch ((AP_DAL::Event2)msg.event) { + switch ((AP_DAL::Event)msg.event) { - case AP_DAL::Event2::resetGyroBias: + case AP_DAL::Event::resetGyroBias: ekf2.resetGyroBias(); break; - case AP_DAL::Event2::resetHeightDatum: + case AP_DAL::Event::resetHeightDatum: ekf2.resetHeightDatum(); break; - case AP_DAL::Event2::setInhibitGPS: + case AP_DAL::Event::setInhibitGPS: ekf2.setInhibitGPS(); break; - case AP_DAL::Event2::setTakeoffExpected: + case AP_DAL::Event::setTakeoffExpected: ekf2.setTakeoffExpected(true); break; - case AP_DAL::Event2::unsetTakeoffExpected: + case AP_DAL::Event::unsetTakeoffExpected: ekf2.setTakeoffExpected(false); break; - case AP_DAL::Event2::setTouchdownExpected: + case AP_DAL::Event::setTouchdownExpected: ekf2.setTouchdownExpected(true); break; - case AP_DAL::Event2::unsetTouchdownExpected: + case AP_DAL::Event::unsetTouchdownExpected: ekf2.setTouchdownExpected(false); break; - case AP_DAL::Event2::setInhibitGpsVertVelUse: + case AP_DAL::Event::setInhibitGpsVertVelUse: ekf2.setInhibitGpsVertVelUse(true); break; - case AP_DAL::Event2::unsetInhibitGpsVertVelUse: + case AP_DAL::Event::unsetInhibitGpsVertVelUse: ekf2.setInhibitGpsVertVelUse(false); break; - case AP_DAL::Event2::setTerrainHgtStable: + case AP_DAL::Event::setTerrainHgtStable: ekf2.setTerrainHgtStable(true); break; - case AP_DAL::Event2::unsetTerrainHgtStable: + case AP_DAL::Event::unsetTerrainHgtStable: ekf2.setTerrainHgtStable(false); break; - case AP_DAL::Event2::requestYawReset: + case AP_DAL::Event::requestYawReset: ekf2.requestYawReset(); break; - case AP_DAL::Event2::checkLaneSwitch: + case AP_DAL::Event::checkLaneSwitch: ekf2.checkLaneSwitch(); break; } + if (replay_force_ekf3) { + LR_MsgHandler_REV3 h{f, ekf2, ekf3}; + h.process_message(msgbytes); + } } void LR_MsgHandler_RSO2::process_message(uint8_t *msgbytes) @@ -88,12 +107,21 @@ void LR_MsgHandler_RSO2::process_message(uint8_t *msgbytes) loc.lng = msg.lng; loc.alt = msg.alt; ekf2.setOriginLLH(loc); + + if (replay_force_ekf3) { + LR_MsgHandler_RSO2 h{f, ekf2, ekf3}; + h.process_message(msgbytes); + } } 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}; + h.process_message(msgbytes); + } } @@ -101,48 +129,53 @@ void LR_MsgHandler_REV3::process_message(uint8_t *msgbytes) { MSG_CREATE(REV3, msgbytes); - switch ((AP_DAL::Event3)msg.event) { + switch ((AP_DAL::Event)msg.event) { - case AP_DAL::Event3::resetGyroBias: + case AP_DAL::Event::resetGyroBias: ekf3.resetGyroBias(); break; - case AP_DAL::Event3::resetHeightDatum: + case AP_DAL::Event::resetHeightDatum: ekf3.resetHeightDatum(); break; - case AP_DAL::Event3::setInhibitGPS: + case AP_DAL::Event::setInhibitGPS: ekf3.setInhibitGPS(); break; - case AP_DAL::Event3::setTakeoffExpected: + case AP_DAL::Event::setTakeoffExpected: ekf3.setTakeoffExpected(true); break; - case AP_DAL::Event3::unsetTakeoffExpected: + case AP_DAL::Event::unsetTakeoffExpected: ekf3.setTakeoffExpected(false); break; - case AP_DAL::Event3::setTouchdownExpected: + case AP_DAL::Event::setTouchdownExpected: ekf3.setTouchdownExpected(true); break; - case AP_DAL::Event3::unsetTouchdownExpected: + case AP_DAL::Event::unsetTouchdownExpected: ekf3.setTouchdownExpected(false); break; - case AP_DAL::Event3::setInhibitGpsVertVelUse: + case AP_DAL::Event::setInhibitGpsVertVelUse: ekf3.setInhibitGpsVertVelUse(true); break; - case AP_DAL::Event3::unsetInhibitGpsVertVelUse: + case AP_DAL::Event::unsetInhibitGpsVertVelUse: ekf3.setInhibitGpsVertVelUse(false); break; - case AP_DAL::Event3::setTerrainHgtStable: + case AP_DAL::Event::setTerrainHgtStable: ekf3.setTerrainHgtStable(true); break; - case AP_DAL::Event3::unsetTerrainHgtStable: + case AP_DAL::Event::unsetTerrainHgtStable: ekf3.setTerrainHgtStable(false); break; - case AP_DAL::Event3::requestYawReset: + case AP_DAL::Event::requestYawReset: ekf3.requestYawReset(); break; - case AP_DAL::Event3::checkLaneSwitch: + case AP_DAL::Event::checkLaneSwitch: ekf3.checkLaneSwitch(); break; } + + if (replay_force_ekf2) { + LR_MsgHandler_REV2 h{f, ekf2, ekf3}; + h.process_message(msgbytes); + } } void LR_MsgHandler_RSO3::process_message(uint8_t *msgbytes) @@ -153,12 +186,20 @@ void LR_MsgHandler_RSO3::process_message(uint8_t *msgbytes) loc.lng = msg.lng; loc.alt = msg.alt; ekf3.setOriginLLH(loc); + if (replay_force_ekf2) { + LR_MsgHandler_RSO2 h{f, ekf2, ekf3}; + h.process_message(msgbytes); + } } void LR_MsgHandler_RWA3::process_message(uint8_t *msgbytes) { MSG_CREATE(RWA3, msgbytes); ekf3.writeDefaultAirSpeed(msg.airspeed); + if (replay_force_ekf2) { + LR_MsgHandler_RWA2 h{f, ekf2, ekf3}; + h.process_message(msgbytes); + } } void LR_MsgHandler_REY3::process_message(uint8_t *msgbytes) diff --git a/Tools/Replay/LR_MsgHandler.h b/Tools/Replay/LR_MsgHandler.h index 9c9fdde695..b258f496e7 100644 --- a/Tools/Replay/LR_MsgHandler.h +++ b/Tools/Replay/LR_MsgHandler.h @@ -39,30 +39,6 @@ protected: NavEKF3 &ekf3; }; -class LR_MsgHandler_EKF2 : public LR_MsgHandler -{ -public: - LR_MsgHandler_EKF2(struct log_Format &_f, NavEKF2 &_ekf2) : - LR_MsgHandler(_f), - ekf2(_ekf2) {} - using LR_MsgHandler::LR_MsgHandler; - virtual void process_message(uint8_t *msg) override = 0; -protected: - NavEKF2 &ekf2; -}; - -class LR_MsgHandler_EKF3 : public LR_MsgHandler -{ -public: - LR_MsgHandler_EKF3(struct log_Format &_f, NavEKF3 &_ekf3) : - LR_MsgHandler(_f), - ekf3(_ekf3) {} - using LR_MsgHandler::LR_MsgHandler; - virtual void process_message(uint8_t *msg) override = 0; -protected: - NavEKF3 &ekf3; -}; - class LR_MsgHandler_RFRF : public LR_MsgHandler_EKF { using LR_MsgHandler_EKF::LR_MsgHandler_EKF; @@ -106,52 +82,53 @@ public: void process_message(uint8_t *msg) override; }; -class LR_MsgHandler_REV2 : public LR_MsgHandler_EKF2 +class LR_MsgHandler_REV2 : public LR_MsgHandler_EKF { public: - using LR_MsgHandler_EKF2::LR_MsgHandler_EKF2; + using LR_MsgHandler_EKF::LR_MsgHandler_EKF; void process_message(uint8_t *msg) override; }; -class LR_MsgHandler_RSO2 : public LR_MsgHandler_EKF2 +class LR_MsgHandler_RSO2 : public LR_MsgHandler_EKF { - using LR_MsgHandler_EKF2::LR_MsgHandler_EKF2; +public: + using LR_MsgHandler_EKF::LR_MsgHandler_EKF; void process_message(uint8_t *msg) override; }; -class LR_MsgHandler_RWA2 : public LR_MsgHandler_EKF2 +class LR_MsgHandler_RWA2 : public LR_MsgHandler_EKF { public: - using LR_MsgHandler_EKF2::LR_MsgHandler_EKF2; + using LR_MsgHandler_EKF::LR_MsgHandler_EKF; void process_message(uint8_t *msg) override; }; -class LR_MsgHandler_REV3 : public LR_MsgHandler_EKF3 +class LR_MsgHandler_REV3 : public LR_MsgHandler_EKF { public: - using LR_MsgHandler_EKF3::LR_MsgHandler_EKF3; + using LR_MsgHandler_EKF::LR_MsgHandler_EKF; void process_message(uint8_t *msg) override; }; -class LR_MsgHandler_RSO3 : public LR_MsgHandler_EKF3 +class LR_MsgHandler_RSO3 : public LR_MsgHandler_EKF { public: - using LR_MsgHandler_EKF3::LR_MsgHandler_EKF3; + using LR_MsgHandler_EKF::LR_MsgHandler_EKF; void process_message(uint8_t *msg) override; }; -class LR_MsgHandler_RWA3 : public LR_MsgHandler_EKF3 +class LR_MsgHandler_RWA3 : public LR_MsgHandler_EKF { public: - using LR_MsgHandler_EKF3::LR_MsgHandler_EKF3; + using LR_MsgHandler_EKF::LR_MsgHandler_EKF; void process_message(uint8_t *msg) override; }; -class LR_MsgHandler_REY3 : public LR_MsgHandler_EKF3 +class LR_MsgHandler_REY3 : public LR_MsgHandler_EKF { public: - using LR_MsgHandler_EKF3::LR_MsgHandler_EKF3; + using LR_MsgHandler_EKF::LR_MsgHandler_EKF; void process_message(uint8_t *msg) override; }; diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index d3256af64e..d36c3da3fe 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -69,19 +69,19 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) } else if (streq(name, "RFRN")) { msgparser[f.type] = new LR_MsgHandler_RFRN(formats[f.type]); } else if (streq(name, "REV2")) { - msgparser[f.type] = new LR_MsgHandler_REV2(formats[f.type], ekf2); + msgparser[f.type] = new LR_MsgHandler_REV2(formats[f.type], ekf2, ekf3); } else if (streq(name, "RSO2")) { - msgparser[f.type] = new LR_MsgHandler_RSO2(formats[f.type], ekf2); + msgparser[f.type] = new LR_MsgHandler_RSO2(formats[f.type], ekf2, ekf3); } else if (streq(name, "RWA2")) { - msgparser[f.type] = new LR_MsgHandler_RWA2(formats[f.type], ekf2); + msgparser[f.type] = new LR_MsgHandler_RWA2(formats[f.type], ekf2, ekf3); } else if (streq(name, "REV3")) { - msgparser[f.type] = new LR_MsgHandler_REV3(formats[f.type], ekf3); + msgparser[f.type] = new LR_MsgHandler_REV3(formats[f.type], ekf2, ekf3); } else if (streq(name, "RSO3")) { - msgparser[f.type] = new LR_MsgHandler_RSO3(formats[f.type], ekf3); + msgparser[f.type] = new LR_MsgHandler_RSO3(formats[f.type], ekf2, ekf3); } else if (streq(name, "RWA3")) { - msgparser[f.type] = new LR_MsgHandler_RWA3(formats[f.type], ekf3); + msgparser[f.type] = new LR_MsgHandler_RWA3(formats[f.type], ekf2, ekf3); } else if (streq(name, "REY3")) { - msgparser[f.type] = new LR_MsgHandler_REY3(formats[f.type], ekf3); + msgparser[f.type] = new LR_MsgHandler_REY3(formats[f.type], ekf2, ekf3); } else if (streq(name, "RISH")) { msgparser[f.type] = new LR_MsgHandler_RISH(formats[f.type]); } else if (streq(name, "RISI")) { diff --git a/Tools/Replay/MsgHandler.cpp b/Tools/Replay/MsgHandler.cpp index 79ae0f19c6..22651b1597 100644 --- a/Tools/Replay/MsgHandler.cpp +++ b/Tools/Replay/MsgHandler.cpp @@ -180,15 +180,6 @@ void MsgHandler::string_for_labels(char *buffer, uint32_t bufferlen) } } -MsgHandler::~MsgHandler() -{ - for (uint8_t k=0; k 0) { filename = argv[0]; @@ -197,6 +216,18 @@ void Replay::setup() set_user_parameters(); + if (replay_force_ekf2) { + reader.set_parameter("EK2_ENABLE", 1, true); + } + if (replay_force_ekf3) { + reader.set_parameter("EK3_ENABLE", 1, true); + } + + if (replay_force_ekf2 && replay_force_ekf3) { + ::printf("Cannot force both EKF types\n"); + exit(1); + } + if (filename == nullptr) { #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS // allow replay on stm32 diff --git a/Tools/Replay/Replay.h b/Tools/Replay/Replay.h index aed5e1a354..6fef0ef4af 100644 --- a/Tools/Replay/Replay.h +++ b/Tools/Replay/Replay.h @@ -24,6 +24,8 @@ struct user_parameter { }; extern user_parameter *user_parameters; +extern bool replay_force_ekf2; +extern bool replay_force_ekf3; class ReplayVehicle : public AP_Vehicle { public: