Skip to content

Commit

Permalink
Tools: support ExternalAHRS for Replay
Browse files Browse the repository at this point in the history
  • Loading branch information
@tridge
tridge committed Sep 11, 2024
1 parent 39ebe61 commit 52fb663
Show file tree
Hide file tree
Showing 7 changed files with 49 additions and 26 deletions.
18 changes: 12 additions & 6 deletions Tools/Replay/LR_MsgHandler.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,12 @@ void LR_MsgHandler_RFRF::process_message(uint8_t *msgbytes)
}
#undef MAP_FLAG
AP::dal().handle_message(msg, ekf2, ekf3);
if (eahrs.get_name() == nullptr) {
eahrs.init();
}
if (eahrs.get_name() != nullptr) {
eahrs.update();
}
}

void LR_MsgHandler_RFRN::process_message(uint8_t *msgbytes)
Expand Down Expand Up @@ -75,7 +81,7 @@ void LR_MsgHandler_REV2::process_message(uint8_t *msgbytes)
break;
}
if (replay_force_ekf3) {
LR_MsgHandler_REV3 h{f, ekf2, ekf3};
LR_MsgHandler_REV3 h{f, ekf2, ekf3, eahrs};
h.process_message(msgbytes);
}
}
Expand All @@ -90,7 +96,7 @@ void LR_MsgHandler_RSO2::process_message(uint8_t *msgbytes)
ekf2.setOriginLLH(loc);

if (replay_force_ekf3) {
LR_MsgHandler_RSO2 h{f, ekf2, ekf3};
LR_MsgHandler_RSO2 h{f, ekf2, ekf3, eahrs};
h.process_message(msgbytes);
}
}
Expand All @@ -100,7 +106,7 @@ 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};
LR_MsgHandler_RWA2 h{f, ekf2, ekf3, eahrs};
h.process_message(msgbytes);
}
}
Expand Down Expand Up @@ -136,7 +142,7 @@ void LR_MsgHandler_REV3::process_message(uint8_t *msgbytes)
}

if (replay_force_ekf2) {
LR_MsgHandler_REV2 h{f, ekf2, ekf3};
LR_MsgHandler_REV2 h{f, ekf2, ekf3, eahrs};
h.process_message(msgbytes);
}
}
Expand All @@ -150,7 +156,7 @@ void LR_MsgHandler_RSO3::process_message(uint8_t *msgbytes)
loc.alt = msg.alt;
ekf3.setOriginLLH(loc);
if (replay_force_ekf2) {
LR_MsgHandler_RSO2 h{f, ekf2, ekf3};
LR_MsgHandler_RSO2 h{f, ekf2, ekf3, eahrs};
h.process_message(msgbytes);
}
}
Expand All @@ -160,7 +166,7 @@ void LR_MsgHandler_RWA3::process_message(uint8_t *msgbytes)
MSG_CREATE(RWA3, msgbytes);
ekf3.writeDefaultAirSpeed(msg.airspeed, msg.uncertainty);
if (replay_force_ekf2) {
LR_MsgHandler_RWA2 h{f, ekf2, ekf3};
LR_MsgHandler_RWA2 h{f, ekf2, ekf3, eahrs};
h.process_message(msgbytes);
}
}
Expand Down
10 changes: 8 additions & 2 deletions Tools/Replay/LR_MsgHandler.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
#include <AP_GPS/AP_GPS.h>
#include <AP_NavEKF2/AP_NavEKF2.h>
#include <AP_NavEKF3/AP_NavEKF3.h>
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>

class LR_MsgHandler : public MsgHandler {
public:
Expand All @@ -28,15 +29,20 @@ class LR_MsgHandler_RFRH : public LR_MsgHandler
class LR_MsgHandler_EKF : public LR_MsgHandler
{
public:
LR_MsgHandler_EKF(struct log_Format &_f, NavEKF2 &_ekf2, NavEKF3 &_ekf3) :
LR_MsgHandler_EKF(struct log_Format &_f,
NavEKF2 &_ekf2,
NavEKF3 &_ekf3,
AP_ExternalAHRS &_eahrs) :
LR_MsgHandler(_f),
ekf2(_ekf2),
ekf3(_ekf3) {}
ekf3(_ekf3),
eahrs(_eahrs){}
using LR_MsgHandler::LR_MsgHandler;
virtual void process_message(uint8_t *msg) override = 0;
protected:
NavEKF2 &ekf2;
NavEKF3 &ekf3;
AP_ExternalAHRS &eahrs;
};

class LR_MsgHandler_RFRF : public LR_MsgHandler_EKF
Expand Down
33 changes: 17 additions & 16 deletions Tools/Replay/LogReader.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,11 +22,12 @@

extern struct user_parameter *user_parameters;

LogReader::LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf2, NavEKF3 &_ekf3) :
LogReader::LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf2, NavEKF3 &_ekf3, AP_ExternalAHRS &_eahrs) :
AP_LoggerFileReader(),
_log_structure(log_structure),
ekf2(_ekf2),
ekf3(_ekf3)
ekf3(_ekf3),
eahrs(_eahrs)
{
}

Expand Down Expand Up @@ -65,23 +66,23 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
} else if (streq(name, "RFRH")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RFRH(formats[f.type]);
} else if (streq(name, "RFRF")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RFRF(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RFRF(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "RFRN")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RFRN(formats[f.type]);
} else if (streq(name, "REV2")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REV2(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REV2(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "RSO2")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSO2(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSO2(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "RWA2")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWA2(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWA2(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "REV3")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REV3(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REV3(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "RSO3")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSO3(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSO3(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "RWA3")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWA3(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWA3(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "REY3")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REY3(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REY3(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "RISH")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RISH(formats[f.type]);
} else if (streq(name, "RISI")) {
Expand Down Expand Up @@ -115,17 +116,17 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
} else if (streq(name, "RVOH")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RVOH(formats[f.type]);
} else if (streq(name, "ROFH")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_ROFH(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_ROFH(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "REPH")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REPH(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REPH(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "RSLL")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSLL(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSLL(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "REVH")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REVH(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REVH(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "RWOH")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWOH(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWOH(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "RBOH")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RBOH(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RBOH(formats[f.type], ekf2, ekf3, eahrs);
} else {
// debug(" No parser for (%s)\n", name);
}
Expand Down
3 changes: 2 additions & 1 deletion Tools/Replay/LogReader.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@
class LogReader : public AP_LoggerFileReader
{
public:
LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf, NavEKF3 &_ekf3);
LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf, NavEKF3 &_ekf3, AP_ExternalAHRS &_eahrs);

VehicleType::vehicle_type vehicle;

Expand All @@ -26,6 +26,7 @@ class LogReader : public AP_LoggerFileReader

NavEKF2 &ekf2;
NavEKF3 &ekf3;
AP_ExternalAHRS &eahrs;

struct LogStructure *_log_structure;
uint8_t _log_structure_count;
Expand Down
1 change: 1 addition & 0 deletions Tools/Replay/Parameters.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@ class Parameters {
k_param_logger,
k_param_NavEKF3,
k_param_gps,
k_param_eahrs,
};
AP_Int8 dummy;
};
Expand Down
7 changes: 7 additions & 0 deletions Tools/Replay/Replay.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,12 +76,18 @@ const AP_Param::Info ReplayVehicle::var_info[] = {
// @Group: GPS
// @Path: ../libraries/AP_GPS/AP_GPS.cpp
GOBJECT(gps, "GPS", AP_GPS),

// @Group: EAHRS
// @Path: ../libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp
GOBJECTPTR(eahrs, "EAHRS", AP_ExternalAHRS),

AP_VAREND
};

void ReplayVehicle::load_parameters(void)
{
eahrs = &AP::externalAHRS();

AP_Param::check_var_info();

StorageManager::erase();
Expand Down Expand Up @@ -239,6 +245,7 @@ void Replay::setup()
::printf("open(%s): %m\n", filename);
exit(1);
}
_vehicle.eahrs = &AP::externalAHRS();
}

void Replay::loop()
Expand Down
3 changes: 2 additions & 1 deletion Tools/Replay/Replay.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,7 @@ class ReplayVehicle : public AP_Vehicle {

NavEKF2 ekf2;
NavEKF3 ekf3;
AP_ExternalAHRS *eahrs;

SRV_Channels servo_channels;

Expand Down Expand Up @@ -99,7 +100,7 @@ class Replay : public AP_HAL::HAL::Callbacks {
const char *filename;
ReplayVehicle &_vehicle;

LogReader reader{_vehicle.log_structure, _vehicle.ekf2, _vehicle.ekf3};
LogReader reader{_vehicle.log_structure, _vehicle.ekf2, _vehicle.ekf3, AP::externalAHRS()};

void _parse_command_line(uint8_t argc, char * const argv[]);

Expand Down

0 comments on commit 52fb663

Please sign in to comment.