Replay: use common events and structures

allows for --force-ekf2 and --force-ekf3 to run the other EKFs events
when needed

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)

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;
 };
 

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")) {

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<LOGREADER_MAX_FIELDS; k++) {
-        if (field_info[k].label != NULL) {
-            free(field_info[k].label);
-        }
-    }
-}
-
 void MsgHandler::location_from_msg(uint8_t *msg,
                                   Location &loc,
                                   const char *label_lat,

Tools/Replay/MsgHandler.h

@@ -73,7 +73,6 @@ private:
 
 protected:
     struct log_Format f; // the format we are a parser for
-    ~MsgHandler();
 
     void location_from_msg(uint8_t *msg, Location &loc, const char *label_lat,
 			   const char *label_long, const char *label_alt);

Tools/Replay/Replay.cpp

@@ -36,6 +36,8 @@ static ReplayVehicle replayvehicle;
 
 // list of user parameters
 user_parameter *user_parameters;
+bool replay_force_ekf2;
+bool replay_force_ekf3;
 
 #define GSCALAR(v, name, def) { replayvehicle.g.v.vtype, name, Parameters::k_param_ ## v, &replayvehicle.g.v, {def_value : def} }
 #define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &replayvehicle.v, {group_info : class::var_info} }
@@ -129,8 +131,15 @@ void Replay::usage(void)
     ::printf("Options:\n");
     ::printf("\t--parm NAME=VALUE  set parameter NAME to VALUE\n");
     ::printf("\t--param-file FILENAME  load parameters from a file\n");
+    ::printf("\t--force-ekf2 force enable EKF2\n");
+    ::printf("\t--force-ekf3 force enable EKF3\n");
 }
 
+enum param_key : uint8_t {
+    FORCE_EKF2 = 1,
+    FORCE_EKF3,
+};
+
 void Replay::_parse_command_line(uint8_t argc, char * const argv[])
 {
     const struct GetOptLong::option options[] = {
@@ -138,6 +147,8 @@ void Replay::_parse_command_line(uint8_t argc, char * const argv[])
         {"parm",            true,   0, 'p'},
         {"param",           true,   0, 'p'},
         {"param-file",      true,   0, 'F'},
+        {"force-ekf2",      false,  0, param_key::FORCE_EKF2},
+        {"force-ekf3",      false,  0, param_key::FORCE_EKF3},
         {"help",            false,  0, 'h'},
         {0, false, 0, 0}
     };
@@ -165,6 +176,14 @@ void Replay::_parse_command_line(uint8_t argc, char * const argv[])
             load_param_file(gopt.optarg);
             break;
 
+        case param_key::FORCE_EKF2:
+            replay_force_ekf2 = true;
+            break;
+
+        case param_key::FORCE_EKF3:
+            replay_force_ekf3 = true;
+            break;
+
         case 'h':
         default:
             usage();
@@ -172,8 +191,8 @@ void Replay::_parse_command_line(uint8_t argc, char * const argv[])
         }
     }
 
-	argv += gopt.optind;
+    argv += gopt.optind;
-	argc -= gopt.optind;
+    argc -= gopt.optind;
 
     if (argc > 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

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: