Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

Replay: use common events and structures

Browse Source 
allows for --force-ekf2 and --force-ekf3 to run the other EKFs events
when needed
This commit is contained in:
Andrew Tridgell 2020-11-16 14:34:44 +11:00 committed by Peter Barker
parent 4c606a30bb
commit 32bead6013
7 changed files with 126 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
Tools/Replay/LR_MsgHandler.cpp
View File

@ -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)

53
Tools/Replay/LR_MsgHandler.h
View File

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

14
Tools/Replay/LogReader.cpp
View File

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

9
Tools/Replay/MsgHandler.cpp
View File

@ -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,

1
Tools/Replay/MsgHandler.h
View File

@ -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);

35
Tools/Replay/Replay.cpp
View File

@ -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;
argc -= gopt.optind;
argv += 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

2
Tools/Replay/Replay.h
View File

@ -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: