Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Replay: support external position and velocity data

This commit is contained in:
Andrew Tridgell 2020-11-07 10:57:07 +11:00
parent 5260d677ef
commit 0ee8b415f2
3 changed files with 72 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Tools/Replay/LR_MsgHandler.cpp
View File

@ -242,6 +242,16 @@ void LR_MsgHandler_ROFH::process_message(uint8_t *msg)
AP::dal().handle_message(MSG_CAST(ROFH,msg), ekf2, ekf3);
}
void LR_MsgHandler_REPH::process_message(uint8_t *msg)
{
AP::dal().handle_message(MSG_CAST(REPH,msg), ekf2, ekf3);
}
void LR_MsgHandler_REVH::process_message(uint8_t *msg)
{
AP::dal().handle_message(MSG_CAST(REVH,msg), ekf2, ekf3);
}
#include <AP_AHRS/AP_AHRS.h>
#include "VehicleType.h"

118
Tools/Replay/LR_MsgHandler.h
View File

@ -38,34 +38,68 @@ public:
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_RFRF : public LR_MsgHandler
class LR_MsgHandler_EKF : public LR_MsgHandler
{
public:
LR_MsgHandler_RFRF(struct log_Format &_f, NavEKF2 &_ekf2, NavEKF3 &_ekf3) :
LR_MsgHandler_EKF(struct log_Format &_f, NavEKF2 &_ekf2, NavEKF3 &_ekf3) :
LR_MsgHandler(_f),
ekf2(_ekf2),
ekf3(_ekf3) {}
using LR_MsgHandler::LR_MsgHandler;
void process_message(uint8_t *msg) override;
private:
virtual void process_message(uint8_t *msg) override = 0;
protected:
NavEKF2 &ekf2;
NavEKF3 &ekf3;
};
class LR_MsgHandler_ROFH : public LR_MsgHandler
class LR_MsgHandler_EKF2 : public LR_MsgHandler
{
public:
LR_MsgHandler_ROFH(struct log_Format &_f, NavEKF2 &_ekf2, NavEKF3 &_ekf3) :
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),
ekf2(_ekf2),
ekf3(_ekf3) {}
using LR_MsgHandler::LR_MsgHandler;
void process_message(uint8_t *msg) override;
private:
NavEKF2 &ekf2;
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;
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_ROFH : public LR_MsgHandler_EKF
{
using LR_MsgHandler_EKF::LR_MsgHandler_EKF;
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_REPH : public LR_MsgHandler_EKF
{
using LR_MsgHandler_EKF::LR_MsgHandler_EKF;
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_REVH : public LR_MsgHandler_EKF
{
using LR_MsgHandler_EKF::LR_MsgHandler_EKF;
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_RFRN : public LR_MsgHandler
{
public:
@ -73,89 +107,53 @@ public:
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_REV2 : public LR_MsgHandler
class LR_MsgHandler_REV2 : public LR_MsgHandler_EKF2
{
public:
LR_MsgHandler_REV2(struct log_Format &_f, NavEKF2 &_ekf2) :
LR_MsgHandler(_f),
ekf2(_ekf2) {}
using LR_MsgHandler_EKF2::LR_MsgHandler_EKF2;
void process_message(uint8_t *msg) override;
private:
NavEKF2 &ekf2;
};
class LR_MsgHandler_RSO2 : public LR_MsgHandler
class LR_MsgHandler_RSO2 : public LR_MsgHandler_EKF2
{
public:
LR_MsgHandler_RSO2(struct log_Format &_f, NavEKF2 &_ekf2) :
LR_MsgHandler(_f),
ekf2(_ekf2) {}
using LR_MsgHandler_EKF2::LR_MsgHandler_EKF2;
void process_message(uint8_t *msg) override;
private:
NavEKF2 &ekf2;
};
class LR_MsgHandler_RWA2 : public LR_MsgHandler
class LR_MsgHandler_RWA2 : public LR_MsgHandler_EKF2
{
public:
LR_MsgHandler_RWA2(struct log_Format &_f, NavEKF2 &_ekf2) :
LR_MsgHandler(_f),
ekf2(_ekf2) {}
using LR_MsgHandler_EKF2::LR_MsgHandler_EKF2;
void process_message(uint8_t *msg) override;
private:
NavEKF2 &ekf2;
};
class LR_MsgHandler_REV3 : public LR_MsgHandler
class LR_MsgHandler_REV3 : public LR_MsgHandler_EKF3
{
public:
LR_MsgHandler_REV3(struct log_Format &_f, NavEKF3 &_ekf3) :
LR_MsgHandler(_f),
ekf3(_ekf3) {}
using LR_MsgHandler_EKF3::LR_MsgHandler_EKF3;
void process_message(uint8_t *msg) override;
private:
NavEKF3 &ekf3;
};
class LR_MsgHandler_RSO3 : public LR_MsgHandler
class LR_MsgHandler_RSO3 : public LR_MsgHandler_EKF3
{
public:
LR_MsgHandler_RSO3(struct log_Format &_f, NavEKF3 &_ekf3) :
LR_MsgHandler(_f),
ekf3(_ekf3) {}
using LR_MsgHandler_EKF3::LR_MsgHandler_EKF3;
void process_message(uint8_t *msg) override;
private:
NavEKF3 &ekf3;
};
class LR_MsgHandler_RWA3 : public LR_MsgHandler
class LR_MsgHandler_RWA3 : public LR_MsgHandler_EKF3
{
public:
LR_MsgHandler_RWA3(struct log_Format &_f, NavEKF3 &_ekf3) :
LR_MsgHandler(_f),
ekf3(_ekf3) {}
using LR_MsgHandler_EKF3::LR_MsgHandler_EKF3;
void process_message(uint8_t *msg) override;
private:
NavEKF3 &ekf3;
};
class LR_MsgHandler_REY3 : public LR_MsgHandler
class LR_MsgHandler_REY3 : public LR_MsgHandler_EKF3
{
public:
LR_MsgHandler_REY3(struct log_Format &_f, NavEKF3 &_ekf3) :
LR_MsgHandler(_f),
ekf3(_ekf3) {}
using LR_MsgHandler_EKF3::LR_MsgHandler_EKF3;
void process_message(uint8_t *msg) override;
private:
NavEKF3 &ekf3;
};
class LR_MsgHandler_RISH : public LR_MsgHandler

4
Tools/Replay/LogReader.cpp
View File

@ -222,6 +222,10 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
msgparser[f.type] = new LR_MsgHandler_RVOH(formats[f.type]);
} else if (streq(name, "ROFH")) {
msgparser[f.type] = new LR_MsgHandler_ROFH(formats[f.type], ekf2, ekf3);
} else if (streq(name, "REPH")) {
msgparser[f.type] = new LR_MsgHandler_REPH(formats[f.type], ekf2, ekf3);
} else if (streq(name, "REVH")) {
msgparser[f.type] = new LR_MsgHandler_REVH(formats[f.type], ekf2, ekf3);
} else {
// debug(" No parser for (%s)\n", name);
}