Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Replay: added --force-ekf2 and --force-ekf3 #15820

Merged
merged 7 commits into from Nov 17, 2020
Merged

Replay: added --force-ekf2 and --force-ekf3 #15820

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
966b060
AP_Logger: allow for replay with both EKF enabled
tridge Nov 16, 2020
4d31ae6
HAL_SITL: avoid sitl command line processing for replay
tridge Nov 16, 2020
0d7cb59
AP_DAL: use common events and structures for EKF2 and EKF3
tridge Nov 16, 2020
f44f90d
AP_NavEKF2: use common structure names
tridge Nov 16, 2020
b4b05c2
AP_NavEKF3: use common structure names
tridge Nov 16, 2020
0545f43
Replay: use common events and structures
tridge Nov 16, 2020
df4b829
AP_Filesystem: allow for replay on any file in local filesystem
tridge Nov 16, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
97 changes: 69 additions & 28 deletions Tools/Replay/LR_MsgHandler.cpp
View file
Open in desktop
Expand Up @@ -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);
}

Expand All @@ -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)
Expand All @@ -88,61 +107,75 @@ 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);
}
}


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)
Expand All @@ -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)
Expand Down
53 changes: 15 additions & 38 deletions Tools/Replay/LR_MsgHandler.h
View file
Open in desktop
Expand Up @@ -39,30 +39,6 @@ class LR_MsgHandler_EKF : public LR_MsgHandler
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;
Expand Down Expand Up @@ -106,52 +82,53 @@ class LR_MsgHandler_RFRN : public LR_MsgHandler
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;
};

Expand Down
14 changes: 7 additions & 7 deletions Tools/Replay/LogReader.cpp
View file
Open in desktop
Expand Up @@ -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")) {
Expand Down
9 changes: 0 additions & 9 deletions Tools/Replay/MsgHandler.cpp
View file
Open in desktop
Expand Up @@ -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,
Expand Down
1 change: 0 additions & 1 deletion Tools/Replay/MsgHandler.h
View file
Open in desktop
Expand Up @@ -73,7 +73,6 @@ class MsgHandler {

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);
Expand Down