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

Replay: new replay implemention 

this uses log msgs from AP_DAL to replay more accurately

Co-authored-by: Peter Barker <pbarker@barker.dropbear.id.au>
This commit is contained in:
Andrew Tridgell 2020-11-06 10:25:17 +11:00
parent e116b1ff0a
commit 81f96aedd2
12 changed files with 676 additions and 2175 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
Tools/Replay/DataFlashFileReader.cpp
View File

@ -61,7 +61,7 @@ void AP_LoggerFileReader::get_packet_counts(uint64_t dest[])
memcpy(dest, packet_counts, sizeof(packet_counts)); memcpy(dest, packet_counts, sizeof(packet_counts));
} }
bool AP_LoggerFileReader::update(char type[5], uint8_t &core) bool AP_LoggerFileReader::update()
{ {
uint8_t hdr[3]; uint8_t hdr[3];
if (read_input(hdr, 3) != 3) { if (read_input(hdr, 3) != 3) {
@ -81,8 +81,6 @@ bool AP_LoggerFileReader::update(char type[5], uint8_t &core)
return false; return false;
} }
memcpy(&formats[f.type], &f, sizeof(formats[f.type])); memcpy(&formats[f.type], &f, sizeof(formats[f.type]));
strncpy(type, "FMT", 3);
type[3] = 0;
message_count++; message_count++;
return handle_log_format_msg(f); return handle_log_format_msg(f);
@ -103,9 +101,6 @@ bool AP_LoggerFileReader::update(char type[5], uint8_t &core)
return false; return false;
} }
strncpy(type, f.name, 4);
type[4] = 0;
message_count++; message_count++;
return handle_msg(f, msg, core); return handle_msg(f, msg);
} }

4
Tools/Replay/DataFlashFileReader.h
View File

@ -12,10 +12,10 @@ public:
~AP_LoggerFileReader(); ~AP_LoggerFileReader();
bool open_log(const char *logfile); bool open_log(const char *logfile);
bool update(char type[5], uint8_t &core); bool update();
virtual bool handle_log_format_msg(const struct log_Format &f) = 0; virtual bool handle_log_format_msg(const struct log_Format &f) = 0;
virtual bool handle_msg(const struct log_Format &f, uint8_t *msg, uint8_t &core) = 0; virtual bool handle_msg(const struct log_Format &f, uint8_t *msg) = 0;
void format_type(uint16_t type, char dest[5]); void format_type(uint16_t type, char dest[5]);
void get_packet_counts(uint64_t dest[]); void get_packet_counts(uint64_t dest[]);

643
Tools/Replay/LR_MsgHandler.cpp
View File

@ -3,418 +3,285 @@
#include "Replay.h" #include "Replay.h"
#include <AP_HAL_Linux/Scheduler.h> #include <AP_HAL_Linux/Scheduler.h>
#include <AP_DAL/AP_DAL.h>
#include <cinttypes> #include <cinttypes>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
LR_MsgHandler::LR_MsgHandler(struct log_Format &_f, #define MSG_CAST(sname,msg) *((log_ ##sname *)(msg+3))
AP_Logger &_logger,
uint64_t &_last_timestamp_usec) : LR_MsgHandler::LR_MsgHandler(struct log_Format &_f) :
logger(_logger), last_timestamp_usec(_last_timestamp_usec),
MsgHandler(_f) { MsgHandler(_f) {
} }
void LR_MsgHandler::wait_timestamp_usec(uint64_t timestamp) void LR_MsgHandler_RFRH::process_message(uint8_t *msg)
{ {
last_timestamp_usec = timestamp; AP::dal().handle_message(MSG_CAST(RFRH,msg));
hal.scheduler->stop_clock(timestamp);
} }
void LR_MsgHandler::wait_timestamp(uint32_t timestamp) void LR_MsgHandler_RFRF::process_message(uint8_t *msg)
{ {
uint64_t usecs = timestamp*1000UL; const log_RFRF &RFRF = MSG_CAST(RFRF,msg);
wait_timestamp_usec(usecs); uint8_t frame_types = RFRF.frame_types;
} if (frame_types & uint8_t(AP_DAL::FrameType::InitialiseFilterEKF2)) {
ekf2_init_done = ekf2.InitialiseFilter();
void LR_MsgHandler::wait_timestamp_from_msg(uint8_t *msg)
{
uint64_t time_us;
uint32_t time_ms;
if (field_value(msg, "TimeUS", time_us)) {
// 64-bit timestamp present - great!
wait_timestamp_usec(time_us);
} else if (field_value(msg, "TimeMS", time_ms)) {
// there is special rounding code that needs to be crossed in
// wait_timestamp:
wait_timestamp(time_ms);
} else {
::printf("No timestamp on message");
} }
} if (frame_types & uint8_t(AP_DAL::FrameType::UpdateFilterEKF2)) {
if (!ekf2_init_done) {
ekf2_init_done = ekf2.InitialiseFilter();
}
/* if (ekf2_init_done) {
* subclasses to handle specific messages below here ekf2.UpdateFilter();
*/
void LR_MsgHandler_AHR2::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
attitude_from_msg(msg, ahr2_attitude, "Roll", "Pitch", "Yaw");
}
void LR_MsgHandler_ARM::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
uint8_t ArmState = require_field_uint8_t(msg, "ArmState");
hal.util->set_soft_armed(ArmState);
printf("Armed state: %u at %lu\n",
(unsigned)ArmState,
(unsigned long)AP_HAL::millis());
}
void LR_MsgHandler_ARSP::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
airspeed.setHIL(require_field_float(msg, "Airspeed"),
require_field_float(msg, "DiffPress"),
require_field_float(msg, "Temp"));
}
void LR_MsgHandler_NKF1::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
}
void LR_MsgHandler_NKF1::process_message(uint8_t *msg, uint8_t &core)
{
wait_timestamp_from_msg(msg);
if (!field_value(msg, "C", core)) {
// 255 here is a special marker for "no core present in log".
// This may give us a hope of backwards-compatability.
core = 255;
}
}
void LR_MsgHandler_XKF1::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
}
void LR_MsgHandler_XKF1::process_message(uint8_t *msg, uint8_t &core)
{
wait_timestamp_from_msg(msg);
if (!field_value(msg, "C", core)) {
// 255 here is a special marker for "no core present in log".
// This may give us a hope of backwards-compatability.
core = 255;
}
}
void LR_MsgHandler_ATT::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
attitude_from_msg(msg, attitude, "Roll", "Pitch", "Yaw");
}
void LR_MsgHandler_CHEK::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
check_state.time_us = AP_HAL::micros64();
attitude_from_msg(msg, check_state.euler, "Roll", "Pitch", "Yaw");
check_state.euler *= radians(1);
location_from_msg(msg, check_state.pos, "Lat", "Lng", "Alt");
require_field(msg, "VN", check_state.velocity.x);
require_field(msg, "VE", check_state.velocity.y);
require_field(msg, "VD", check_state.velocity.z);
}
void LR_MsgHandler_BARO::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
uint32_t last_update_ms;
if (!field_value(msg, "SMS", last_update_ms)) {
last_update_ms = 0;
}
AP::baro().setHIL(0,
require_field_float(msg, "Press"),
require_field_int16_t(msg, "Temp") * 0.01f,
require_field_float(msg, "Alt"),
require_field_float(msg, "CRt"),
last_update_ms);
}
void LR_MsgHandler_Event::process_message(uint8_t *msg)
{
uint8_t id = require_field_uint8_t(msg, "Id");
if ((LogEvent)id == LogEvent::ARMED) {
hal.util->set_soft_armed(true);
printf("Armed at %lu\n",
(unsigned long)AP_HAL::millis());
} else if ((LogEvent)id == LogEvent::DISARMED) {
hal.util->set_soft_armed(false);
printf("Disarmed at %lu\n",
(unsigned long)AP_HAL::millis());
}
}
void LR_MsgHandler_GPS2::process_message(uint8_t *msg)
{
update_from_msg_gps(1, msg);
}
void LR_MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg)
{
uint64_t time_us;
if (! field_value(msg, "TimeUS", time_us)) {
uint32_t timestamp;
require_field(msg, "T", timestamp);
time_us = timestamp * 1000;
}
wait_timestamp_usec(time_us);
Location loc;
location_from_msg(msg, loc, "Lat", "Lng", "Alt");
Vector3f vel;
ground_vel_from_msg(msg, vel, "Spd", "GCrs", "VZ");
uint8_t status = require_field_uint8_t(msg, "Status");
uint8_t hdop = 0;
if (! field_value(msg, "HDop", hdop) &&
! field_value(msg, "HDp", hdop)) {
hdop = 20;
}
uint8_t nsats = 0;
if (! field_value(msg, "NSats", nsats) &&
! field_value(msg, "numSV", nsats)) {
field_not_found(msg, "NSats");
}
uint16_t GWk;
uint32_t GMS;
if (! field_value(msg, "GWk", GWk)) {
field_not_found(msg, "GWk");
}
if (! field_value(msg, "GMS", GMS)) {
field_not_found(msg, "GMS");
}
gps.setHIL(gps_offset,
(AP_GPS::GPS_Status)status,
AP_GPS::time_epoch_convert(GWk, GMS),
loc,
vel,
nsats,
hdop);
if (status == AP_GPS::GPS_OK_FIX_3D && ground_alt_cm == 0) {
ground_alt_cm = require_field_int32_t(msg, "Alt");
}
// we don't call GPS update_instance which would ordinarily write
// these...
AP::logger().Write_GPS(gps_offset);
}
void LR_MsgHandler_GPS::process_message(uint8_t *msg)
{
update_from_msg_gps(0, msg);
}
void LR_MsgHandler_GPA_Base::update_from_msg_gpa(uint8_t gps_offset, uint8_t *msg)
{
uint64_t time_us;
require_field(msg, "TimeUS", time_us);
wait_timestamp_usec(time_us);
uint16_t vdop, hacc, vacc, sacc;
require_field(msg, "VDop", vdop);
require_field(msg, "HAcc", hacc);
require_field(msg, "VAcc", vacc);
require_field(msg, "SAcc", sacc);
uint8_t have_vertical_velocity;
if (! field_value(msg, "VV", have_vertical_velocity)) {
have_vertical_velocity = !is_zero(gps.velocity(gps_offset).z);
}
uint32_t sample_ms;
if (! field_value(msg, "SMS", sample_ms)) {
sample_ms = 0;
}
gps.setHIL_Accuracy(gps_offset, vdop*0.01f, hacc*0.01f, vacc*0.01f, sacc*0.01f, have_vertical_velocity, sample_ms);
}
void LR_MsgHandler_GPA::process_message(uint8_t *msg)
{
update_from_msg_gpa(0, msg);
}
void LR_MsgHandler_GPA2::process_message(uint8_t *msg)
{
update_from_msg_gpa(1, msg);
}
void LR_MsgHandler_IMU2::process_message(uint8_t *msg)
{
update_from_msg_imu(1, msg);
}
void LR_MsgHandler_IMU3::process_message(uint8_t *msg)
{
update_from_msg_imu(2, msg);
}
void LR_MsgHandler_IMU_Base::update_from_msg_imu(uint8_t imu_offset, uint8_t *msg)
{
wait_timestamp_from_msg(msg);
uint8_t this_imu_mask = 1 << imu_offset;
if (gyro_mask & this_imu_mask) {
Vector3f gyro;
require_field(msg, "Gyr", gyro);
ins.set_gyro(imu_offset, gyro);
}
if (accel_mask & this_imu_mask) {
Vector3f accel2;
require_field(msg, "Acc", accel2);
ins.set_accel(imu_offset, accel2);
}
}
void LR_MsgHandler_IMU::process_message(uint8_t *msg)
{
update_from_msg_imu(0, msg);
}
void LR_MsgHandler_IMT_Base::update_from_msg_imt(uint8_t imu_offset, uint8_t *msg)
{
wait_timestamp_from_msg(msg);
if (!use_imt) {
return;
}
uint8_t this_imu_mask = 1 << imu_offset;
float delta_time = 0;
require_field(msg, "DelT", delta_time);
ins.set_delta_time(delta_time);
if (gyro_mask & this_imu_mask) {
Vector3f d_angle;
require_field(msg, "DelA", d_angle);
float d_angle_dt;
if (!field_value(msg, "DelaT", d_angle_dt)) {
d_angle_dt = 0;
} }
ins.set_delta_angle(imu_offset, d_angle, d_angle_dt);
} }
if (accel_mask & this_imu_mask) { if (frame_types & uint8_t(AP_DAL::FrameType::InitialiseFilterEKF3)) {
float dvt = 0; ekf3_init_done = ekf3.InitialiseFilter();
require_field(msg, "DelvT", dvt); }
Vector3f d_velocity; if (frame_types & uint8_t(AP_DAL::FrameType::UpdateFilterEKF3)) {
require_field(msg, "DelV", d_velocity); if (!ekf3_init_done) {
ins.set_delta_velocity(imu_offset, dvt, d_velocity); ekf3_init_done = ekf3.InitialiseFilter();
}
if (ekf3_init_done) {
ekf3.UpdateFilter();
}
}
if (frame_types & uint8_t(AP_DAL::FrameType::LogWriteEKF2)) {
ekf2.Log_Write();
}
if (frame_types & uint8_t(AP_DAL::FrameType::LogWriteEKF3)) {
ekf3.Log_Write();
} }
} }
void LR_MsgHandler_IMT::process_message(uint8_t *msg) void LR_MsgHandler_RFRN::process_message(uint8_t *msg)
{ {
update_from_msg_imt(0, msg); AP::dal().handle_message(MSG_CAST(RFRN,msg));
} }
void LR_MsgHandler_IMT2::process_message(uint8_t *msg) void LR_MsgHandler_REV2::process_message(uint8_t *msg)
{ {
update_from_msg_imt(1, msg); const log_REV2 &rev2 = MSG_CAST(REV2,msg);
}
void LR_MsgHandler_IMT3::process_message(uint8_t *msg) switch ((AP_DAL::Event2)rev2.event) {
{
update_from_msg_imt(2, msg);
}
void LR_MsgHandler_MAG2::process_message(uint8_t *msg) case AP_DAL::Event2::resetGyroBias:
{ ekf2.resetGyroBias();
update_from_msg_compass(1, msg); break;
} case AP_DAL::Event2::resetHeightDatum:
ekf2.resetHeightDatum();
break;
void LR_MsgHandler_MAG_Base::update_from_msg_compass(uint8_t compass_offset, uint8_t *msg) case AP_DAL::Event2::setInhibitGPS:
{ ekf2.setInhibitGPS();
wait_timestamp_from_msg(msg); break;
case AP_DAL::Event2::setTakeoffExpected:
Vector3f mag; ekf2.setTakeoffExpected(true);
require_field(msg, "Mag", mag); break;
Vector3f mag_offset; case AP_DAL::Event2::unsetTakeoffExpected:
require_field(msg, "Ofs", mag_offset); ekf2.setTakeoffExpected(false);
uint32_t last_update_usec; break;
if (!field_value(msg, "S", last_update_usec)) { case AP_DAL::Event2::setTouchdownExpected:
last_update_usec = AP_HAL::micros(); ekf2.setTouchdownExpected(true);
break;
case AP_DAL::Event2::unsetTouchdownExpected:
ekf2.setTouchdownExpected(false);
break;
case AP_DAL::Event2::setInhibitGpsVertVelUse:
ekf2.setInhibitGpsVertVelUse(true);
break;
case AP_DAL::Event2::unsetInhibitGpsVertVelUse:
ekf2.setInhibitGpsVertVelUse(false);
break;
case AP_DAL::Event2::setTerrainHgtStable:
ekf2.setTerrainHgtStable(true);
break;
case AP_DAL::Event2::unsetTerrainHgtStable:
ekf2.setTerrainHgtStable(false);
break;
case AP_DAL::Event2::requestYawReset:
ekf2.requestYawReset();
break;
case AP_DAL::Event2::checkLaneSwitch:
ekf2.checkLaneSwitch();
break;
} }
}
compass.setHIL(compass_offset, mag - mag_offset, last_update_usec); void LR_MsgHandler_RSO2::process_message(uint8_t *msg)
// compass_offset is which compass we are setting info for; {
// mag_offset is a vector indicating the compass' calibration... const log_RSO2 &rso2 = MSG_CAST(RSO2,msg);
compass.set_offsets(compass_offset, mag_offset); Location loc;
loc.lat = rso2.lat;
loc.lng = rso2.lng;
loc.alt = rso2.alt;
ekf2.setOriginLLH(loc);
}
void LR_MsgHandler_RWA2::process_message(uint8_t *msg)
{
const log_RWA2 &rwa2 = MSG_CAST(RWA2,msg);
ekf2.writeDefaultAirSpeed(rwa2.airspeed);
} }
void LR_MsgHandler_REV3::process_message(uint8_t *msg)
void LR_MsgHandler_MAG::process_message(uint8_t *msg)
{ {
update_from_msg_compass(0, msg); const log_REV3 &rev3 = MSG_CAST(REV3,msg);
switch ((AP_DAL::Event3)rev3.event) {
case AP_DAL::Event3::resetGyroBias:
ekf3.resetGyroBias();
break;
case AP_DAL::Event3::resetHeightDatum:
ekf3.resetHeightDatum();
break;
case AP_DAL::Event3::setInhibitGPS:
ekf3.setInhibitGPS();
break;
case AP_DAL::Event3::setTakeoffExpected:
ekf3.setTakeoffExpected(true);
break;
case AP_DAL::Event3::unsetTakeoffExpected:
ekf3.setTakeoffExpected(false);
break;
case AP_DAL::Event3::setTouchdownExpected:
ekf3.setTouchdownExpected(true);
break;
case AP_DAL::Event3::unsetTouchdownExpected:
ekf3.setTouchdownExpected(false);
break;
case AP_DAL::Event3::setInhibitGpsVertVelUse:
ekf3.setInhibitGpsVertVelUse(true);
break;
case AP_DAL::Event3::unsetInhibitGpsVertVelUse:
ekf3.setInhibitGpsVertVelUse(false);
break;
case AP_DAL::Event3::setTerrainHgtStable:
ekf3.setTerrainHgtStable(true);
break;
case AP_DAL::Event3::unsetTerrainHgtStable:
ekf3.setTerrainHgtStable(false);
break;
case AP_DAL::Event3::requestYawReset:
ekf3.requestYawReset();
break;
case AP_DAL::Event3::checkLaneSwitch:
ekf3.checkLaneSwitch();
break;
}
}
void LR_MsgHandler_RSO3::process_message(uint8_t *msg)
{
const log_RSO3 &rso3 = MSG_CAST(RSO3,msg);
Location loc;
loc.lat = rso3.lat;
loc.lng = rso3.lng;
loc.alt = rso3.alt;
ekf3.setOriginLLH(loc);
}
void LR_MsgHandler_RWA3::process_message(uint8_t *msg)
{
const log_RWA3 &rwa3 = MSG_CAST(RWA3,msg);
ekf3.writeDefaultAirSpeed(rwa3.airspeed);
}
void LR_MsgHandler_REY3::process_message(uint8_t *msg)
{
const log_RWA3 &rwa3 = MSG_CAST(RWA3,msg);
ekf3.writeDefaultAirSpeed(rwa3.airspeed);
}
void LR_MsgHandler_RISH::process_message(uint8_t *msg)
{
AP::dal().handle_message(MSG_CAST(RISH,msg));
}
void LR_MsgHandler_RISI::process_message(uint8_t *msg)
{
AP::dal().handle_message(MSG_CAST(RISI,msg));
}
void LR_MsgHandler_RASH::process_message(uint8_t *msg)
{
AP::dal().handle_message(MSG_CAST(RASH,msg));
}
void LR_MsgHandler_RASI::process_message(uint8_t *msg)
{
AP::dal().handle_message(MSG_CAST(RASI,msg));
}
void LR_MsgHandler_RBRH::process_message(uint8_t *msg)
{
AP::dal().handle_message(MSG_CAST(RBRH,msg));
}
void LR_MsgHandler_RBRI::process_message(uint8_t *msg)
{
AP::dal().handle_message(MSG_CAST(RBRI,msg));
}
void LR_MsgHandler_RRNH::process_message(uint8_t *msg)
{
AP::dal().handle_message(MSG_CAST(RRNH,msg));
}
void LR_MsgHandler_RRNI::process_message(uint8_t *msg)
{
AP::dal().handle_message(MSG_CAST(RRNI,msg));
}
void LR_MsgHandler_RGPH::process_message(uint8_t *msg)
{
AP::dal().handle_message(MSG_CAST(RGPH,msg));
}
void LR_MsgHandler_RGPI::process_message(uint8_t *msg)
{
AP::dal().handle_message(MSG_CAST(RGPI,msg));
}
void LR_MsgHandler_RGPJ::process_message(uint8_t *msg)
{
AP::dal().handle_message(MSG_CAST(RGPJ,msg));
}
void LR_MsgHandler_RMGH::process_message(uint8_t *msg)
{
AP::dal().handle_message(MSG_CAST(RMGH,msg));
}
void LR_MsgHandler_RMGI::process_message(uint8_t *msg)
{
AP::dal().handle_message(MSG_CAST(RMGI,msg));
}
void LR_MsgHandler_RBCH::process_message(uint8_t *msg)
{
AP::dal().handle_message(MSG_CAST(RBCH,msg));
}
void LR_MsgHandler_RBCI::process_message(uint8_t *msg)
{
AP::dal().handle_message(MSG_CAST(RBCI,msg));
}
void LR_MsgHandler_RVOH::process_message(uint8_t *msg)
{
AP::dal().handle_message(MSG_CAST(RVOH,msg));
} }
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#include "VehicleType.h" #include "VehicleType.h"
void LR_MsgHandler_MSG::process_message(uint8_t *msg)
{
const uint8_t msg_text_len = 64;
char msg_text[msg_text_len];
require_field(msg, "Message", msg_text, msg_text_len);
if (strncmp(msg_text, "ArduPlane", strlen("ArduPlane")) == 0) {
vehicle = VehicleType::VEHICLE_PLANE;
::printf("Detected Plane\n");
ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING);
ahrs.set_fly_forward(true);
} else if (strncmp(msg_text, "ArduCopter", strlen("ArduCopter")) == 0 ||
strncmp(msg_text, "APM:Copter", strlen("APM:Copter")) == 0) {
vehicle = VehicleType::VEHICLE_COPTER;
::printf("Detected Copter\n");
ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
ahrs.set_fly_forward(false);
} else if (strncmp(msg_text, "ArduRover", strlen("ArduRover")) == 0) {
vehicle = VehicleType::VEHICLE_ROVER;
::printf("Detected Rover\n");
ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
ahrs.set_fly_forward(true);
}
}
void LR_MsgHandler_NTUN_Copter::process_message(uint8_t *msg)
{
inavpos = Vector3f(require_field_float(msg, "PosX") * 0.01f,
require_field_float(msg, "PosY") * 0.01f,
0);
}
bool LR_MsgHandler_PARM::set_parameter(const char *name, const float value) bool LR_MsgHandler_PARM::set_parameter(const char *name, const float value)
{ {
const char *ignore_parms[] = { "GPS_TYPE", "AHRS_EKF_TYPE", "EK2_ENABLE", "EK3_ENABLE" const char *ignore_parms[] = {
"COMPASS_ORIENT", "COMPASS_ORIENT2", // "GPS_TYPE",
"COMPASS_ORIENT3", "LOG_FILE_BUFSIZE", // "AHRS_EKF_TYPE",
"LOG_DISARMED"}; // "EK2_ENABLE",
// "EK3_ENABLE",
// "COMPASS_ORIENT",
// "COMPASS_ORIENT2",
// "COMPASS_ORIENT3",
"LOG_FILE_BUFSIZE",
"LOG_DISARMED"
};
for (uint8_t i=0; i < ARRAY_SIZE(ignore_parms); i++) { for (uint8_t i=0; i < ARRAY_SIZE(ignore_parms); i++) {
if (strncmp(name, ignore_parms[i], AP_MAX_NAME_SIZE) == 0) { if (strncmp(name, ignore_parms[i], AP_MAX_NAME_SIZE) == 0) {
::printf("Ignoring set of %s to %f\n", name, value); ::printf("Ignoring set of %s to %f\n", name, value);
@ -432,7 +299,6 @@ void LR_MsgHandler_PARM::process_message(uint8_t *msg)
uint64_t time_us; uint64_t time_us;
if (field_value(msg, "TimeUS", time_us)) { if (field_value(msg, "TimeUS", time_us)) {
wait_timestamp_usec(time_us);
} else { } else {
// older logs can have a lot of FMT and PARM messages up the // older logs can have a lot of FMT and PARM messages up the
// front which don't have timestamps. Since in Replay we run // front which don't have timestamps. Since in Replay we run
@ -445,24 +311,9 @@ void LR_MsgHandler_PARM::process_message(uint8_t *msg)
require_field(msg, "Name", parameter_name, parameter_name_len); require_field(msg, "Name", parameter_name, parameter_name_len);
float value = require_field_float(msg, "Value"); float value = require_field_float(msg, "Value");
if (globals.no_params || replay.check_user_param(parameter_name)) { // if (globals.no_params || replay.check_user_param(parameter_name)) {
printf("Not changing %s to %f\n", parameter_name, value); // printf("Not changing %s to %f\n", parameter_name, value);
} else { // } else {
set_parameter(parameter_name, value); set_parameter(parameter_name, value);
} // }
}
void LR_MsgHandler_PM::process_message(uint8_t *msg)
{
uint32_t new_logdrop;
if (field_value(msg, "LogDrop", new_logdrop) &&
new_logdrop != 0) {
printf("PM.LogDrop: %u dropped at timestamp %" PRIu64 "\n", new_logdrop, last_timestamp_usec);
}
}
void LR_MsgHandler_SIM::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
attitude_from_msg(msg, sim_attitude, "Roll", "Pitch", "Yaw");
} }

557
Tools/Replay/LR_MsgHandler.h
View File

@ -3,14 +3,14 @@
#include "MsgHandler.h" #include "MsgHandler.h"
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#include <AP_GPS/AP_GPS.h> #include <AP_GPS/AP_GPS.h>
#include <AP_NavEKF2/AP_NavEKF2.h>
#include <AP_NavEKF3/AP_NavEKF3.h>
#include <functional> #include <functional>
class LR_MsgHandler : public MsgHandler { class LR_MsgHandler : public MsgHandler {
public: public:
LR_MsgHandler(struct log_Format &f, LR_MsgHandler(struct log_Format &f);
AP_Logger &_logger,
uint64_t &last_timestamp_usec);
virtual void process_message(uint8_t *msg) = 0; virtual void process_message(uint8_t *msg) = 0;
virtual void process_message(uint8_t *msg, uint8_t &core) { virtual void process_message(uint8_t *msg, uint8_t &core) {
// base implementation just ignores the core parameter; // base implementation just ignores the core parameter;
@ -28,431 +28,233 @@ public:
}; };
protected: protected:
AP_Logger &logger;
void wait_timestamp(uint32_t timestamp);
void wait_timestamp_usec(uint64_t timestamp);
void wait_timestamp_from_msg(uint8_t *msg);
uint64_t &last_timestamp_usec;
}; };
/* subclasses below this point */ class LR_MsgHandler_RFRH : public LR_MsgHandler
class LR_MsgHandler_AHR2 : public LR_MsgHandler
{ {
public: public:
LR_MsgHandler_AHR2(log_Format &_f, AP_Logger &_logger, using LR_MsgHandler::LR_MsgHandler;
uint64_t &_last_timestamp_usec, Vector3f &_ahr2_attitude) void process_message(uint8_t *msg) override;
: LR_MsgHandler(_f, _logger,_last_timestamp_usec), };
ahr2_attitude(_ahr2_attitude) { };
class LR_MsgHandler_RFRF : public LR_MsgHandler
{
public:
LR_MsgHandler_RFRF(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:
NavEKF2 &ekf2;
NavEKF3 &ekf3;
bool ekf2_init_done;
bool ekf3_init_done;
};
class LR_MsgHandler_RFRN : public LR_MsgHandler
{
public:
using LR_MsgHandler::LR_MsgHandler;
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_REV2 : public LR_MsgHandler
{
public:
LR_MsgHandler_REV2(struct log_Format &_f, NavEKF2 &_ekf2) :
LR_MsgHandler(_f),
ekf2(_ekf2) {}
void process_message(uint8_t *msg) override; void process_message(uint8_t *msg) override;
private: private:
Vector3f &ahr2_attitude; NavEKF2 &ekf2;
}; };
class LR_MsgHandler_RSO2 : public LR_MsgHandler
class LR_MsgHandler_ARM : public LR_MsgHandler
{ {
public: public:
LR_MsgHandler_ARM(log_Format &_f, AP_Logger &_logger, LR_MsgHandler_RSO2(struct log_Format &_f, NavEKF2 &_ekf2) :
uint64_t &_last_timestamp_usec) LR_MsgHandler(_f),
: LR_MsgHandler(_f, _logger, _last_timestamp_usec) { }; ekf2(_ekf2) {}
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_ARSP : public LR_MsgHandler
{
public:
LR_MsgHandler_ARSP(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec, AP_Airspeed &_airspeed) :
LR_MsgHandler(_f, _logger, _last_timestamp_usec), airspeed(_airspeed) { };
void process_message(uint8_t *msg) override; void process_message(uint8_t *msg) override;
private: private:
AP_Airspeed &airspeed; NavEKF2 &ekf2;
}; };
class LR_MsgHandler_NKF1 : public LR_MsgHandler class LR_MsgHandler_RWA2 : public LR_MsgHandler
{ {
public: public:
LR_MsgHandler_NKF1(log_Format &_f, AP_Logger &_logger, LR_MsgHandler_RWA2(struct log_Format &_f, NavEKF2 &_ekf2) :
uint64_t &_last_timestamp_usec) : LR_MsgHandler(_f),
LR_MsgHandler(_f, _logger, _last_timestamp_usec) { }; ekf2(_ekf2) {}
void process_message(uint8_t *msg) override;
void process_message(uint8_t *msg, uint8_t &core) override;
};
class LR_MsgHandler_XKF1 : public LR_MsgHandler
{
public:
LR_MsgHandler_XKF1(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec) :
LR_MsgHandler(_f, _logger, _last_timestamp_usec) { };
void process_message(uint8_t *msg) override;
void process_message(uint8_t *msg, uint8_t &core) override;
};
class LR_MsgHandler_ATT : public LR_MsgHandler
{
public:
LR_MsgHandler_ATT(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec, Vector3f &_attitude)
: LR_MsgHandler(_f, _logger, _last_timestamp_usec), attitude(_attitude)
{ };
void process_message(uint8_t *msg) override; void process_message(uint8_t *msg) override;
private: private:
Vector3f &attitude; NavEKF2 &ekf2;
}; };
class LR_MsgHandler_CHEK : public LR_MsgHandler class LR_MsgHandler_REV3 : public LR_MsgHandler
{ {
public: public:
LR_MsgHandler_CHEK(log_Format &_f, AP_Logger &_logger, LR_MsgHandler_REV3(struct log_Format &_f, NavEKF3 &_ekf3) :
uint64_t &_last_timestamp_usec, CheckState &_check_state) LR_MsgHandler(_f),
: LR_MsgHandler(_f, _logger, _last_timestamp_usec), ekf3(_ekf3) {}
check_state(_check_state)
{ };
void process_message(uint8_t *msg) override; void process_message(uint8_t *msg) override;
private: private:
CheckState &check_state; NavEKF3 &ekf3;
}; };
class LR_MsgHandler_BARO : public LR_MsgHandler class LR_MsgHandler_RSO3 : public LR_MsgHandler
{ {
public: public:
LR_MsgHandler_BARO(log_Format &_f, AP_Logger &_logger, LR_MsgHandler_RSO3(struct log_Format &_f, NavEKF3 &_ekf3) :
uint64_t &_last_timestamp_usec) LR_MsgHandler(_f),
: LR_MsgHandler(_f, _logger, _last_timestamp_usec) ekf3(_ekf3) {}
{ };
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_Event : public LR_MsgHandler
{
public:
LR_MsgHandler_Event(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec)
: LR_MsgHandler(_f, _logger, _last_timestamp_usec) { };
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_GPS_Base : public LR_MsgHandler
{
public:
LR_MsgHandler_GPS_Base(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec, AP_GPS &_gps,
uint32_t &_ground_alt_cm)
: LR_MsgHandler(_f, _logger, _last_timestamp_usec),
gps(_gps), ground_alt_cm(_ground_alt_cm) { };
protected:
void update_from_msg_gps(uint8_t imu_offset, uint8_t *data);
private:
AP_GPS &gps;
uint32_t &ground_alt_cm;
};
class LR_MsgHandler_GPS : public LR_MsgHandler_GPS_Base
{
public:
LR_MsgHandler_GPS(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec, AP_GPS &_gps,
uint32_t &_ground_alt_cm)
: LR_MsgHandler_GPS_Base(_f, _logger,_last_timestamp_usec,
_gps, _ground_alt_cm),
gps(_gps), ground_alt_cm(_ground_alt_cm) { };
void process_message(uint8_t *msg) override; void process_message(uint8_t *msg) override;
private: private:
AP_GPS &gps; NavEKF3 &ekf3;
uint32_t &ground_alt_cm;
}; };
// it would be nice to use the same parser for both GPS message types class LR_MsgHandler_RWA3 : public LR_MsgHandler
// (and other packets, too...). I*think* the contructor can simply
// take e.g. &gps[1]... problems are going to arise if we don't
// actually have that many gps' compiled in!
class LR_MsgHandler_GPS2 : public LR_MsgHandler_GPS_Base
{ {
public: public:
LR_MsgHandler_GPS2(log_Format &_f, AP_Logger &_logger, LR_MsgHandler_RWA3(struct log_Format &_f, NavEKF3 &_ekf3) :
uint64_t &_last_timestamp_usec, AP_GPS &_gps, LR_MsgHandler(_f),
uint32_t &_ground_alt_cm) ekf3(_ekf3) {}
: LR_MsgHandler_GPS_Base(_f, _logger, _last_timestamp_usec,
_gps, _ground_alt_cm), gps(_gps),
ground_alt_cm(_ground_alt_cm) { };
void process_message(uint8_t *msg) override;
private:
AP_GPS &gps;
uint32_t &ground_alt_cm;
};
class LR_MsgHandler_GPA_Base : public LR_MsgHandler
{
public:
LR_MsgHandler_GPA_Base(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec, AP_GPS &_gps)
: LR_MsgHandler(_f, _logger, _last_timestamp_usec), gps(_gps) { };
protected:
void update_from_msg_gpa(uint8_t imu_offset, uint8_t *data);
private:
AP_GPS &gps;
};
class LR_MsgHandler_GPA : public LR_MsgHandler_GPA_Base
{
public:
LR_MsgHandler_GPA(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec, AP_GPS &_gps)
: LR_MsgHandler_GPA_Base(_f, _logger,_last_timestamp_usec,
_gps), gps(_gps) { };
void process_message(uint8_t *msg) override; void process_message(uint8_t *msg) override;
private: private:
AP_GPS &gps; NavEKF3 &ekf3;
}; };
class LR_MsgHandler_GPA2 : public LR_MsgHandler_GPA_Base class LR_MsgHandler_REY3 : public LR_MsgHandler
{ {
public: public:
LR_MsgHandler_GPA2(log_Format &_f, AP_Logger &_logger, LR_MsgHandler_REY3(struct log_Format &_f, NavEKF3 &_ekf3) :
uint64_t &_last_timestamp_usec, AP_GPS &_gps) LR_MsgHandler(_f),
: LR_MsgHandler_GPA_Base(_f, _logger, _last_timestamp_usec, ekf3(_ekf3) {}
_gps), gps(_gps) { };
void process_message(uint8_t *msg) override;
private:
AP_GPS &gps;
};
class LR_MsgHandler_IMU_Base : public LR_MsgHandler
{
public:
LR_MsgHandler_IMU_Base(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
AP_InertialSensor &_ins) :
LR_MsgHandler(_f, _logger, _last_timestamp_usec),
accel_mask(_accel_mask),
gyro_mask(_gyro_mask),
ins(_ins) { };
void update_from_msg_imu(uint8_t imu_offset, uint8_t *msg);
private:
uint8_t &accel_mask;
uint8_t &gyro_mask;
AP_InertialSensor &ins;
};
class LR_MsgHandler_IMU : public LR_MsgHandler_IMU_Base
{
public:
LR_MsgHandler_IMU(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
AP_InertialSensor &_ins)
: LR_MsgHandler_IMU_Base(_f, _logger, _last_timestamp_usec,
_accel_mask, _gyro_mask, _ins) { };
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_IMU2 : public LR_MsgHandler_IMU_Base
{
public:
LR_MsgHandler_IMU2(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
AP_InertialSensor &_ins)
: LR_MsgHandler_IMU_Base(_f, _logger, _last_timestamp_usec,
_accel_mask, _gyro_mask, _ins) {};
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_IMU3 : public LR_MsgHandler_IMU_Base
{
public:
LR_MsgHandler_IMU3(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
AP_InertialSensor &_ins)
: LR_MsgHandler_IMU_Base(_f, _logger, _last_timestamp_usec,
_accel_mask, _gyro_mask, _ins) {};
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_IMT_Base : public LR_MsgHandler
{
public:
LR_MsgHandler_IMT_Base(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
bool &_use_imt,
AP_InertialSensor &_ins) :
LR_MsgHandler(_f, _logger, _last_timestamp_usec),
accel_mask(_accel_mask),
gyro_mask(_gyro_mask),
use_imt(_use_imt),
ins(_ins) { };
void update_from_msg_imt(uint8_t imu_offset, uint8_t *msg);
private:
uint8_t &accel_mask;
uint8_t &gyro_mask;
bool &use_imt;
AP_InertialSensor &ins;
};
class LR_MsgHandler_IMT : public LR_MsgHandler_IMT_Base
{
public:
LR_MsgHandler_IMT(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
bool &_use_imt,
AP_InertialSensor &_ins)
: LR_MsgHandler_IMT_Base(_f, _logger, _last_timestamp_usec,
_accel_mask, _gyro_mask, _use_imt, _ins) { };
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_IMT2 : public LR_MsgHandler_IMT_Base
{
public:
LR_MsgHandler_IMT2(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
bool &_use_imt,
AP_InertialSensor &_ins)
: LR_MsgHandler_IMT_Base(_f, _logger, _last_timestamp_usec,
_accel_mask, _gyro_mask, _use_imt, _ins) { };
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_IMT3 : public LR_MsgHandler_IMT_Base
{
public:
LR_MsgHandler_IMT3(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
bool &_use_imt,
AP_InertialSensor &_ins)
: LR_MsgHandler_IMT_Base(_f, _logger, _last_timestamp_usec,
_accel_mask, _gyro_mask, _use_imt, _ins) { };
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_MAG_Base : public LR_MsgHandler
{
public:
LR_MsgHandler_MAG_Base(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec, Compass &_compass)
: LR_MsgHandler(_f, _logger, _last_timestamp_usec), compass(_compass) { };
protected:
void update_from_msg_compass(uint8_t compass_offset, uint8_t *msg);
private:
Compass &compass;
};
class LR_MsgHandler_MAG : public LR_MsgHandler_MAG_Base
{
public:
LR_MsgHandler_MAG(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec, Compass &_compass)
: LR_MsgHandler_MAG_Base(_f, _logger, _last_timestamp_usec,_compass) {};
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_MAG2 : public LR_MsgHandler_MAG_Base
{
public:
LR_MsgHandler_MAG2(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec, Compass &_compass)
: LR_MsgHandler_MAG_Base(_f, _logger, _last_timestamp_usec,_compass) {};
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_MSG : public LR_MsgHandler
{
public:
LR_MsgHandler_MSG(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec,
VehicleType::vehicle_type &_vehicle, AP_AHRS &_ahrs) :
LR_MsgHandler(_f, _logger, _last_timestamp_usec),
vehicle(_vehicle), ahrs(_ahrs) { }
void process_message(uint8_t *msg) override; void process_message(uint8_t *msg) override;
private: private:
VehicleType::vehicle_type &vehicle; NavEKF3 &ekf3;
AP_AHRS &ahrs;
}; };
class LR_MsgHandler_RISH : public LR_MsgHandler
class LR_MsgHandler_NTUN_Copter : public LR_MsgHandler
{ {
public: public:
LR_MsgHandler_NTUN_Copter(log_Format &_f, AP_Logger &_logger, using LR_MsgHandler::LR_MsgHandler;
uint64_t &_last_timestamp_usec, Vector3f &_inavpos) void process_message(uint8_t *msg) override;
: LR_MsgHandler(_f, _logger, _last_timestamp_usec), inavpos(_inavpos) {}; };
class LR_MsgHandler_RISI : public LR_MsgHandler
{
public:
using LR_MsgHandler::LR_MsgHandler;
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_RASH : public LR_MsgHandler
{
public:
using LR_MsgHandler::LR_MsgHandler;
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_RASI : public LR_MsgHandler
{
public:
using LR_MsgHandler::LR_MsgHandler;
void process_message(uint8_t *msg) override; void process_message(uint8_t *msg) override;
private:
Vector3f &inavpos;
}; };
class LR_MsgHandler_RBRH : public LR_MsgHandler
{
public:
using LR_MsgHandler::LR_MsgHandler;
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_RBRI : public LR_MsgHandler
{
public:
using LR_MsgHandler::LR_MsgHandler;
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_RRNH : public LR_MsgHandler
{
public:
using LR_MsgHandler::LR_MsgHandler;
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_RRNI : public LR_MsgHandler
{
public:
using LR_MsgHandler::LR_MsgHandler;
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_RGPH : public LR_MsgHandler
{
public:
using LR_MsgHandler::LR_MsgHandler;
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_RGPI : public LR_MsgHandler
{
public:
using LR_MsgHandler::LR_MsgHandler;
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_RGPJ : public LR_MsgHandler
{
public:
using LR_MsgHandler::LR_MsgHandler;
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_RMGH : public LR_MsgHandler
{
public:
using LR_MsgHandler::LR_MsgHandler;
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_RMGI : public LR_MsgHandler
{
public:
using LR_MsgHandler::LR_MsgHandler;
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_RBCH : public LR_MsgHandler
{
public:
using LR_MsgHandler::LR_MsgHandler;
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_RBCI : public LR_MsgHandler
{
public:
using LR_MsgHandler::LR_MsgHandler;
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_RVOH : public LR_MsgHandler
{
public:
using LR_MsgHandler::LR_MsgHandler;
void process_message(uint8_t *msg) override;
};
class LR_MsgHandler_PARM : public LR_MsgHandler class LR_MsgHandler_PARM : public LR_MsgHandler
{ {
public: public:
LR_MsgHandler_PARM(log_Format &_f, AP_Logger &_logger, LR_MsgHandler_PARM(log_Format &_f,
uint64_t &_last_timestamp_usec, uint64_t &_last_timestamp_usec,
const std::function<bool(const char *name, const float)>&set_parameter_callback) : const std::function<bool(const char *name, const float)>&set_parameter_callback) :
LR_MsgHandler(_f, _logger, _last_timestamp_usec), LR_MsgHandler(_f),
_set_parameter_callback(set_parameter_callback) _set_parameter_callback(set_parameter_callback)
{}; {};
@ -462,32 +264,3 @@ private:
bool set_parameter(const char *name, const float value); bool set_parameter(const char *name, const float value);
const std::function<bool(const char *name, const float)>_set_parameter_callback; const std::function<bool(const char *name, const float)>_set_parameter_callback;
}; };
class LR_MsgHandler_PM : public LR_MsgHandler
{
public:
LR_MsgHandler_PM(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec)
: LR_MsgHandler(_f, _logger, _last_timestamp_usec) { };
void process_message(uint8_t *msg) override;
private:
};
class LR_MsgHandler_SIM : public LR_MsgHandler
{
public:
LR_MsgHandler_SIM(log_Format &_f, AP_Logger &_logger,
uint64_t &_last_timestamp_usec,
Vector3f &_sim_attitude)
: LR_MsgHandler(_f, _logger, _last_timestamp_usec),
sim_attitude(_sim_attitude)
{ };
void process_message(uint8_t *msg) override;
private:
Vector3f &sim_attitude;
};

438
Tools/Replay/LogReader.cpp
View File

@ -1,25 +1,12 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h>
#include <AP_Airspeed/AP_Airspeed.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_HAL_Linux/Scheduler.h>
#include "LogReader.h" #include "LogReader.h"
#include "MsgHandler.h"
#include <stdio.h> #include <stdio.h>
#include <unistd.h> #include <unistd.h>
#include <sys/types.h> #include <sys/types.h>
#include <sys/stat.h> #include <sys/stat.h>
#include "MsgHandler.h"
#include "Replay.h"
#define DEBUG 1 #define DEBUG 1
#if DEBUG #if DEBUG
# define debug(fmt, args...) printf(fmt "\n", ##args) # define debug(fmt, args...) printf(fmt "\n", ##args)
@ -29,109 +16,17 @@
#define streq(x, y) (!strcmp(x, y)) #define streq(x, y) (!strcmp(x, y))
extern const AP_HAL::HAL& hal; // const struct LogStructure running_codes_log_structure[] = {
// LOG_COMMON_STRUCTURES,
// };
const struct LogStructure running_codes_log_structure[] = { LogReader::LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf2, NavEKF3 &_ekf3) :
LOG_COMMON_STRUCTURES,
};
LogReader::LogReader(AP_AHRS &_ahrs,
AP_InertialSensor &_ins,
Compass &_compass,
AP_GPS &_gps,
AP_Airspeed &_airspeed,
AP_Logger &_logger,
struct LogStructure *log_structure,
uint8_t log_structure_count,
const char **&_nottypes):
AP_LoggerFileReader(), AP_LoggerFileReader(),
vehicle(VehicleType::VEHICLE_UNKNOWN),
ahrs(_ahrs),
ins(_ins),
compass(_compass),
gps(_gps),
airspeed(_airspeed),
logger(_logger),
accel_mask(7),
gyro_mask(7),
last_timestamp_usec(0),
installed_vehicle_specific_parsers(false),
_log_structure(log_structure), _log_structure(log_structure),
nottypes(_nottypes) ekf2(_ekf2),
ekf3(_ekf3)
{ {
if (log_structure_count != 0) {
::fprintf(stderr, "Do NOT put anything in the log_structure before passing it in here");
abort(); // so there.
}
initialise_fmt_map();
} }
struct log_Format deferred_formats[LOGREADER_MAX_FORMATS];
// some log entries (e.g. "NTUN") are used by the different vehicle
// types with wildy varying payloads. We thus can't use the same
// parser for just any e.g. NTUN message. We defer the registration
// of a parser for these messages until we know what model we're
// dealing with.
void LogReader::maybe_install_vehicle_specific_parsers() {
if (! installed_vehicle_specific_parsers &&
vehicle != VehicleType::VEHICLE_UNKNOWN) {
switch(vehicle) {
case VehicleType::VEHICLE_COPTER:
for (uint8_t i = 0; i<LOGREADER_MAX_FORMATS; i++) {
if (deferred_formats[i].type != 0) {
msgparser[i] = new LR_MsgHandler_NTUN_Copter
(deferred_formats[i], logger, last_timestamp_usec,
inavpos);
}
}
break;
case VehicleType::VEHICLE_PLANE:
break;
case VehicleType::VEHICLE_ROVER:
break;
case VehicleType::VEHICLE_UNKNOWN:
break;
}
installed_vehicle_specific_parsers = true;
}
}
/*
messages which we will be generating, so should be discarded.
Additionally, FMT messages for messages NOT in this list will be
passed straight through to the output file, whereas FMT messages for
messages IN this list must come from LOG_BASE_STRUCTURES in
LogStructure.h
Note that there is an existing with FMTU messages, as these are
emitted both from the common structures at log startup and also when
Log_Write(...) is called for the first time for a particular format
name. Since we include it in generated_names FMTU messages will not
be passed through from the source logs - but since the Replay code
does call Log_Write(...) you will end up with a small selection of
FMTU messages from that.
*/
static const char *generated_names[] = {
"FMT",
"FMTU",
"NKF1", "NKF2", "NKF3", "NKF4", "NKF5", "NKF6", "NKF7", "NKF8", "NKF9", "NKF0",
"NKQ", "NKQ1", "NKQ2",
"XKF1", "XKF2", "XKF3", "XKF4", "XKF5", "XKF6", "XKF7", "XKF8", "XKF9", "XKF0",
"XKQ", "XKQ1", "XKQ2", "XKFD", "XKV1", "XKV2",
"AHR2",
"ORGN",
"POS",
"CHEK",
"IMT", "IMT2", "IMT3",
"MAG", "MAG2",
"BARO", "BAR2",
"GPS","GPA",
NULL,
};
// these names we emit from the code as normal, but are emitted using // these names we emit from the code as normal, but are emitted using
// Log_Write. Thus they are not present in LOG_COMMON_STRUCTURES. A // Log_Write. Thus they are not present in LOG_COMMON_STRUCTURES. A
// format will be written for this by the code itself the first time // format will be written for this by the code itself the first time
@ -139,14 +34,13 @@ static const char *generated_names[] = {
// messages from the old log to the new log, so we need to keep a map // messages from the old log to the new log, so we need to keep a map
// of IDs to prune out... // of IDs to prune out...
static const char *log_write_names[] = { static const char *log_write_names[] = {
"NKA",
"NKV",
"NKT1",
"NKT2",
nullptr nullptr
}; };
static const char *generated_names[] = {
NULL
};
/* /*
see if a type is in a list of types see if a type is in a list of types
*/ */
@ -163,39 +57,6 @@ bool LogReader::in_list(const char *type, const char *list[])
return false; return false;
} }
void LogReader::initialise_fmt_map()
{
for (const char **name = generated_names;
*name !=nullptr;
name++) {
bool found = false;
for (uint8_t n=0; n<ARRAY_SIZE(running_codes_log_structure); n++) {
if (streq(*name, running_codes_log_structure[n].name)) {
const uint8_t t = running_codes_log_structure[n].msg_type;
mapped_msgid[t] = t;
found = true;
break;
}
}
if (!found) {
if (streq(*name, "CHEK")) {
// HACK: CHEK is emitted using Log_Write, so doesn't
// have a fixed address to pre-populate the fmt-map
// with....
continue;
}
if (strncmp(*name, "NK", 2)==0 || strncmp(*name, "XK", 2) == 0) {
// cope with older logs. Really old logs only had EKF
// messages, newer logs have an instance number rather
// than having NKF1 and NKF6.
continue;
}
::fprintf(stderr, "Failed to find apparently-generated-name (%s) in COMMON_LOG_STRUCTURES\n", *name);
abort();
}
}
}
/* /*
map from an incoming format type to an outgoing format type map from an incoming format type to an outgoing format type
*/ */
@ -210,7 +71,7 @@ uint8_t LogReader::map_fmt_type(const char *name, uint8_t intype)
return mapped_msgid[intype]; return mapped_msgid[intype];
} }
for (uint8_t n=next_msgid; n<255; n++) { for (uint8_t n=next_msgid; n<255; n++) {
::fprintf(stderr, "next_msgid=%u\n", n); // ::fprintf(stderr, "next_msgid=%u\n", n);
bool already_mapped = false; bool already_mapped = false;
for (uint16_t i=0; i<sizeof(mapped_msgid); i++) { for (uint16_t i=0; i<sizeof(mapped_msgid); i++) {
if (mapped_msgid[i] == n) { if (mapped_msgid[i] == n) {
@ -227,6 +88,10 @@ uint8_t LogReader::map_fmt_type(const char *name, uint8_t intype)
} }
mapped_msgid[intype] = n; mapped_msgid[intype] = n;
next_msgid = n+1; next_msgid = n+1;
if (next_msgid == 128) {
// don't. Just... don't...
next_msgid += 1;
}
break; break;
} }
if (mapped_msgid[intype] == 0) { if (mapped_msgid[intype] == 0) {
@ -237,46 +102,39 @@ uint8_t LogReader::map_fmt_type(const char *name, uint8_t intype)
return mapped_msgid[intype]; return mapped_msgid[intype];
} }
bool LogReader::save_message_type(const char *name)
{
bool save_message = !in_list(name, generated_names);
save_message = save_message && !in_list(name, log_write_names);
if (save_chek_messages && strcmp(name, "CHEK") == 0) {
save_message = true;
}
return save_message;
}
bool LogReader::handle_log_format_msg(const struct log_Format &f) bool LogReader::handle_log_format_msg(const struct log_Format &f)
{ {
// emit the output as we receive it:
AP::logger().WriteBlock((void*)&f, sizeof(f));
char name[5]; char name[5];
memset(name, '\0', 5); memset(name, '\0', 5);
memcpy(name, f.name, 4); memcpy(name, f.name, 4);
debug("Defining log format for type (%d) (%s)\n", f.type, name); // debug("Defining log format for type (%d) (%s)\n", f.type, name);
struct LogStructure s = _log_structure[_log_structure_count++]; struct LogStructure s = _log_structure[_log_structure_count++];
logger.set_num_types(_log_structure_count); // logger.set_num_types(_log_structure_count);
if (in_list(name, log_write_names)) { if (in_list(name, log_write_names)) {
debug("%s is a Log_Write-written message\n", name); // debug("%s is a Log_Write-written message\n", name);
} else { } else {
if (in_list(name, generated_names)) { if (in_list(name, generated_names)) {
debug("Log format for type (%d) (%s) taken from running code\n", // debug("Log format for type (%d) (%s) taken from running code\n",
f.type, name); // f.type, name);
bool found = false; // bool found = false;
for (uint8_t n=0; n<ARRAY_SIZE(running_codes_log_structure); n++) { // for (uint8_t n=0; n<ARRAY_SIZE(running_codes_log_structure); n++) {
if (streq(name, running_codes_log_structure[n].name)) { // if (streq(name, running_codes_log_structure[n].name)) {
found = true; // found = true;
memcpy(&s, &running_codes_log_structure[n], sizeof(LogStructure)); // memcpy(&s, &running_codes_log_structure[n], sizeof(LogStructure));
break; // break;
} // }
} // }
if (!found) { // if (!found) {
::fprintf(stderr, "Expected to be able to emit an FMT for (%s), but no FMT message found in running code\n", name); // ::fprintf(stderr, "Expected to be able to emit an FMT for (%s), but no FMT message found in running code\n", name);
abort(); // abort();
} // }
} else { } else {
debug("Log format for type (%d) (%s) taken from log\n", f.type, name); // debug("Log format for type (%d) (%s) taken from log\n", f.type, name);
// generate a LogStructure entry for this FMT // generate a LogStructure entry for this FMT
s.msg_type = map_fmt_type(name, f.type); s.msg_type = map_fmt_type(name, f.type);
s.msg_len = f.length; s.msg_len = f.length;
@ -295,7 +153,7 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
memcpy(pkt.name, s.name, sizeof(pkt.name)); memcpy(pkt.name, s.name, sizeof(pkt.name));
memcpy(pkt.format, s.format, sizeof(pkt.format)); memcpy(pkt.format, s.format, sizeof(pkt.format));
memcpy(pkt.labels, s.labels, sizeof(pkt.labels)); memcpy(pkt.labels, s.labels, sizeof(pkt.labels));
logger.WriteCriticalBlock(&pkt, sizeof(pkt)); // AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt));
} }
if (msgparser[f.type] != NULL) { if (msgparser[f.type] != NULL) {
@ -305,154 +163,99 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
// map from format name to a parser subclass: // map from format name to a parser subclass:
if (streq(name, "PARM")) { if (streq(name, "PARM")) {
msgparser[f.type] = new LR_MsgHandler_PARM msgparser[f.type] = new LR_MsgHandler_PARM
(formats[f.type], logger, (formats[f.type],
last_timestamp_usec, last_timestamp_usec,
[this](const char *xname, const float xvalue) { [this](const char *xname, const float xvalue) {
return set_parameter(xname, xvalue); return set_parameter(xname, xvalue);
}); });
} else if (streq(name, "GPS")) { } else if (streq(name, "RFRH")) {
msgparser[f.type] = new LR_MsgHandler_GPS(formats[f.type], msgparser[f.type] = new LR_MsgHandler_RFRH(formats[f.type]);
logger, } else if (streq(name, "RFRF")) {
last_timestamp_usec, msgparser[f.type] = new LR_MsgHandler_RFRF(formats[f.type], ekf2, ekf3);
gps, ground_alt_cm); } else if (streq(name, "RFRN")) {
} else if (streq(name, "GPS2")) { msgparser[f.type] = new LR_MsgHandler_RFRN(formats[f.type]);
msgparser[f.type] = new LR_MsgHandler_GPS2(formats[f.type], logger, } else if (streq(name, "REV2")) {
last_timestamp_usec, msgparser[f.type] = new LR_MsgHandler_REV2(formats[f.type], ekf2);
gps, ground_alt_cm); } else if (streq(name, "RSO2")) {
} else if (streq(name, "GPA")) { msgparser[f.type] = new LR_MsgHandler_RSO2(formats[f.type], ekf2);
msgparser[f.type] = new LR_MsgHandler_GPA(formats[f.type], } else if (streq(name, "RWA2")) {
logger, msgparser[f.type] = new LR_MsgHandler_RWA2(formats[f.type], ekf2);
last_timestamp_usec, } else if (streq(name, "REV3")) {
gps); msgparser[f.type] = new LR_MsgHandler_REV3(formats[f.type], ekf3);
} else if (streq(name, "GPA2")) { } else if (streq(name, "RSO3")) {
msgparser[f.type] = new LR_MsgHandler_GPA2(formats[f.type], logger, msgparser[f.type] = new LR_MsgHandler_RSO3(formats[f.type], ekf3);
last_timestamp_usec, } else if (streq(name, "RWA3")) {
gps); msgparser[f.type] = new LR_MsgHandler_RWA3(formats[f.type], ekf3);
} else if (streq(name, "MSG")) { } else if (streq(name, "REY3")) {
msgparser[f.type] = new LR_MsgHandler_MSG(formats[f.type], logger, msgparser[f.type] = new LR_MsgHandler_REY3(formats[f.type], ekf3);
last_timestamp_usec, } else if (streq(name, "RISH")) {
vehicle, ahrs); msgparser[f.type] = new LR_MsgHandler_RISH(formats[f.type]);
} else if (streq(name, "IMU")) { } else if (streq(name, "RISI")) {
msgparser[f.type] = new LR_MsgHandler_IMU(formats[f.type], logger, msgparser[f.type] = new LR_MsgHandler_RISI(formats[f.type]);
last_timestamp_usec, } else if (streq(name, "RASH")) {
accel_mask, gyro_mask, ins); msgparser[f.type] = new LR_MsgHandler_RASH(formats[f.type]);
} else if (streq(name, "IMU2")) { } else if (streq(name, "RASI")) {
msgparser[f.type] = new LR_MsgHandler_IMU2(formats[f.type], logger, msgparser[f.type] = new LR_MsgHandler_RASI(formats[f.type]);
last_timestamp_usec, } else if (streq(name, "RBRH")) {
accel_mask, gyro_mask, ins); msgparser[f.type] = new LR_MsgHandler_RBRH(formats[f.type]);
} else if (streq(name, "IMU3")) { } else if (streq(name, "RBRI")) {
msgparser[f.type] = new LR_MsgHandler_IMU3(formats[f.type], logger, msgparser[f.type] = new LR_MsgHandler_RBRI(formats[f.type]);
last_timestamp_usec, } else if (streq(name, "RRNH")) {
accel_mask, gyro_mask, ins); msgparser[f.type] = new LR_MsgHandler_RRNH(formats[f.type]);
} else if (streq(name, "IMT")) { } else if (streq(name, "RRNI")) {
msgparser[f.type] = new LR_MsgHandler_IMT(formats[f.type], logger, msgparser[f.type] = new LR_MsgHandler_RRNI(formats[f.type]);
last_timestamp_usec, } else if (streq(name, "RGPH")) {
accel_mask, gyro_mask, use_imt, ins); msgparser[f.type] = new LR_MsgHandler_RGPH(formats[f.type]);
} else if (streq(name, "IMT2")) { } else if (streq(name, "RGPI")) {
msgparser[f.type] = new LR_MsgHandler_IMT2(formats[f.type], logger, msgparser[f.type] = new LR_MsgHandler_RGPI(formats[f.type]);
last_timestamp_usec, } else if (streq(name, "RGPJ")) {
accel_mask, gyro_mask, use_imt, ins); msgparser[f.type] = new LR_MsgHandler_RGPJ(formats[f.type]);
} else if (streq(name, "IMT3")) { } else if (streq(name, "RMGH")) {
msgparser[f.type] = new LR_MsgHandler_IMT3(formats[f.type], logger, msgparser[f.type] = new LR_MsgHandler_RMGH(formats[f.type]);
last_timestamp_usec, } else if (streq(name, "RMGI")) {
accel_mask, gyro_mask, use_imt, ins); msgparser[f.type] = new LR_MsgHandler_RMGI(formats[f.type]);
} else if (streq(name, "SIM")) { } else if (streq(name, "RBCH")) {
msgparser[f.type] = new LR_MsgHandler_SIM(formats[f.type], logger, msgparser[f.type] = new LR_MsgHandler_RBCH(formats[f.type]);
last_timestamp_usec, } else if (streq(name, "RBCI")) {
sim_attitude); msgparser[f.type] = new LR_MsgHandler_RBCI(formats[f.type]);
} else if (streq(name, "BARO")) {
msgparser[f.type] = new LR_MsgHandler_BARO(formats[f.type], logger,
last_timestamp_usec);
} else if (streq(name, "ARM")) {
msgparser[f.type] = new LR_MsgHandler_ARM(formats[f.type], logger,
last_timestamp_usec);
} else if (streq(name, "EV")) {
msgparser[f.type] = new LR_MsgHandler_Event(formats[f.type], logger,
last_timestamp_usec);
} else if (streq(name, "AHR2")) {
msgparser[f.type] = new LR_MsgHandler_AHR2(formats[f.type], logger,
last_timestamp_usec,
ahr2_attitude);
} else if (streq(name, "ATT")) {
// this parser handles *all* attitude messages - the common one,
// and also the rover/copter/plane-specific (old) messages
msgparser[f.type] = new LR_MsgHandler_ATT(formats[f.type], logger,
last_timestamp_usec,
attitude);
} else if (streq(name, "MAG")) {
msgparser[f.type] = new LR_MsgHandler_MAG(formats[f.type], logger,
last_timestamp_usec, compass);
} else if (streq(name, "MAG2")) {
msgparser[f.type] = new LR_MsgHandler_MAG2(formats[f.type], logger,
last_timestamp_usec, compass);
} else if (streq(name, "NTUN")) {
// the label "NTUN" is used by rover, copter and plane -
// and they all look different! creation of a parser is
// deferred until we receive a MSG log entry telling us
// which vehicle type to use. Sucks.
memcpy(&deferred_formats[f.type], &formats[f.type],
sizeof(struct log_Format));
} else if (streq(name, "ARSP")) { // plane-specific(?!)
msgparser[f.type] = new LR_MsgHandler_ARSP(formats[f.type], logger,
last_timestamp_usec,
airspeed);
} else if (streq(name, "NKF1")) {
msgparser[f.type] = new LR_MsgHandler_NKF1(formats[f.type], logger,
last_timestamp_usec);
} else if (streq(name, "CHEK")) {
msgparser[f.type] = new LR_MsgHandler_CHEK(formats[f.type], logger,
last_timestamp_usec,
check_state);
} else if (streq(name, "PM")) {
msgparser[f.type] = new LR_MsgHandler_PM(formats[f.type], logger,
last_timestamp_usec);
} else { } else {
debug(" No parser for (%s)\n", name); // debug(" No parser for (%s)\n", name);
}
return true;
}
bool LogReader::handle_msg(const struct log_Format &f, uint8_t *msg, uint8_t &core) {
char name[5];
memset(name, '\0', 5);
memcpy(name, f.name, 4);
if (save_message_type(name)) {
// write this message through to output log, changing the ID
// present in the input log to that used for the same message
// name in the output log
if (mapped_msgid[msg[2]] == 0) {
printf("Unknown msgid %u\n", (unsigned)msg[2]);
exit(1);
}
msg[2] = mapped_msgid[msg[2]];
if (!in_list(name, nottypes)) {
logger.WriteBlock(msg, f.length);
}
// a MsgHandler would probably have found a timestamp and
// caled stop_clock. This runs IO, clearing logger's
// buffer.
hal.scheduler->stop_clock(((Linux::Scheduler*)hal.scheduler)->stopped_clock_usec());
} }
LR_MsgHandler *p = msgparser[f.type];
if (p == NULL) {
return true;
}
p->process_message(msg, core);
maybe_install_vehicle_specific_parsers();
return true; return true;
} }
bool LogReader::handle_msg(const struct log_Format &f, uint8_t *msg) {
// emit the output as we receive it:
AP::logger().WriteBlock(msg, f.length);
// char name[5];
// memset(name, '\0', 5);
// memcpy(name, f.name, 4);
LR_MsgHandler *p = msgparser[f.type];
if (p == NULL) {
return true;
}
p->process_message(msg);
return true;
}
#include <sys/types.h>
#include <signal.h>
bool LogReader::set_parameter(const char *name, float value) bool LogReader::set_parameter(const char *name, float value)
{ {
// if (!strcmp(name, "EK3_ENABLE")) {
// kill(0, SIGTRAP);
// }
enum ap_var_type var_type; enum ap_var_type var_type;
AP_Param *vp = AP_Param::find(name, &var_type); AP_Param *vp = AP_Param::find(name, &var_type);
if (vp == NULL) { if (vp == NULL) {
// a lot of parameters will not be found - e.g. FORMAT_VERSION
// and all of the vehicle-specific parameters, ....
// ::fprintf(stderr, "Parameter (%s) not found\n", name);
return false; return false;
} }
float old_value = 0; float old_value = 0;
@ -469,11 +272,10 @@ bool LogReader::set_parameter(const char *name, float value)
old_value = ((AP_Int8 *)vp)->cast_to_float(); old_value = ((AP_Int8 *)vp)->cast_to_float();
((AP_Int8 *)vp)->set(value); ((AP_Int8 *)vp)->set(value);
} else { } else {
// we don't support mavlink set on this parameter AP_HAL::panic("What manner of evil is var_type=%u", var_type);
return false;
} }
if (fabsf(old_value - value) > 1.0e-12) { if (fabsf(old_value - value) > 1.0e-12) {
::printf("Changed %s to %.8f from %.8f\n", name, value, old_value); ::fprintf(stderr, "Changed %s to %.8f from %.8f\n", name, value, old_value);
} }
return true; return true;
} }

25
Tools/Replay/LogReader.h
View File

@ -1,3 +1,5 @@
#pragma once
#include "VehicleType.h" #include "VehicleType.h"
#include "DataFlashFileReader.h" #include "DataFlashFileReader.h"
#include "LR_MsgHandler.h" #include "LR_MsgHandler.h"
@ -6,15 +8,7 @@
class LogReader : public AP_LoggerFileReader class LogReader : public AP_LoggerFileReader
{ {
public: public:
LogReader(AP_AHRS &_ahrs, LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf, NavEKF3 &_ekf3);
AP_InertialSensor &_ins,
Compass &_compass,
AP_GPS &_gps,
AP_Airspeed &_airspeed,
AP_Logger &_logger,
struct LogStructure *log_structure,
uint8_t log_structure_count,
const char **&nottypes);
const Vector3f &get_attitude(void) const { return attitude; } const Vector3f &get_attitude(void) const { return attitude; }
const Vector3f &get_ahr2_attitude(void) const { return ahr2_attitude; } const Vector3f &get_ahr2_attitude(void) const { return ahr2_attitude; }
@ -34,19 +28,17 @@ public:
uint64_t last_timestamp_us(void) const { return last_timestamp_usec; } uint64_t last_timestamp_us(void) const { return last_timestamp_usec; }
bool handle_log_format_msg(const struct log_Format &f) override; bool handle_log_format_msg(const struct log_Format &f) override;
bool handle_msg(const struct log_Format &f, uint8_t *msg, uint8_t &core) override; bool handle_msg(const struct log_Format &f, uint8_t *msg) override;
static bool in_list(const char *type, const char *list[]); static bool in_list(const char *type, const char *list[]);
protected: protected:
private: private:
AP_AHRS &ahrs;
AP_InertialSensor &ins; NavEKF2 &ekf2;
Compass &compass; NavEKF3 &ekf3;
AP_GPS &gps;
AP_Airspeed &airspeed;
AP_Logger &logger;
struct LogStructure *_log_structure; struct LogStructure *_log_structure;
uint8_t _log_structure_count; uint8_t _log_structure_count;
@ -74,7 +66,6 @@ private:
LR_MsgHandler::CheckState check_state; LR_MsgHandler::CheckState check_state;
bool installed_vehicle_specific_parsers; bool installed_vehicle_specific_parsers;
const char **&nottypes;
bool save_chek_messages; bool save_chek_messages;

2
Tools/Replay/Makefile
View File

@ -1,7 +1,7 @@
# this is meant to make existing build instructions work with waf # this is meant to make existing build instructions work with waf
all: all:
@cd ../../ && modules/waf/waf-light configure --board linux --debug @cd ../../ && modules/waf/waf-light configure --board linux --debug --disable-scripting
@cd ../../ && modules/waf/waf-light --target tools/Replay @cd ../../ && modules/waf/waf-light --target tools/Replay
@cp ../../build/linux/tools/Replay Replay.elf @cp ../../build/linux/tools/Replay Replay.elf
@echo Built Replay.elf @echo Built Replay.elf

1
Tools/Replay/MsgHandler.cpp
View File

@ -1,5 +1,4 @@
#include "MsgHandler.h" #include "MsgHandler.h"
#include <AP_AHRS/AP_AHRS.h>
void fatal(const char *msg) { void fatal(const char *msg) {
::printf("%s",msg); ::printf("%s",msg);

3
Tools/Replay/Parameters.h
View File

@ -15,7 +15,8 @@ public:
k_param_NavEKF2, k_param_NavEKF2,
k_param_compass, k_param_compass,
k_param_logger, k_param_logger,
k_param_NavEKF3 k_param_NavEKF3,
k_param_gps,
}; };
AP_Int8 dummy; AP_Int8 dummy;
}; };

936
Tools/Replay/Replay.cpp
View File

File diff suppressed because it is too large Load Diff

154
Tools/Replay/Replay.h
View File

@ -13,41 +13,9 @@
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include <StorageManager/StorageManager.h>
#include <fenv.h>
#include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_AccelCal/AP_AccelCal.h>
#include <AP_Declination/AP_Declination.h>
#include <Filter/Filter.h>
#include <AP_Airspeed/AP_Airspeed.h>
#include <AP_Vehicle/AP_Vehicle.h> #include <AP_Vehicle/AP_Vehicle.h>
#include <AP_Notify/AP_Notify.h>
#include <AP_Logger/AP_Logger.h> #include "LogReader.h"
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_NavEKF2/AP_NavEKF2.h>
#include <AP_NavEKF3/AP_NavEKF3.h>
#include <AP_Mission/AP_Mission.h>
#include <AP_Rally/AP_Rally.h>
#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_Terrain/AP_Terrain.h>
#include <AP_OpticalFlow/AP_OpticalFlow.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <RC_Channel/RC_Channel.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <stdio.h>
#include <errno.h>
#include <signal.h>
#include <unistd.h>
#include <AP_HAL/utility/getopt_cpp.h>
class ReplayVehicle : public AP_Vehicle { class ReplayVehicle : public AP_Vehicle {
public: public:
@ -74,6 +42,9 @@ public:
}; };
AP_Logger logger{unused}; AP_Logger logger{unused};
NavEKF2 ekf2;
NavEKF3 ekf3;
protected: protected:
void init_ardupilot() override; void init_ardupilot() override;
@ -87,133 +58,22 @@ private:
static const AP_Param::Info var_info[]; static const AP_Param::Info var_info[];
}; };
class Replay : public AP_HAL::HAL::Callbacks { class Replay : public AP_HAL::HAL::Callbacks {
public: public:
Replay(ReplayVehicle &vehicle) : Replay(ReplayVehicle &vehicle) :
filename("log.bin"),
_vehicle(vehicle) { } _vehicle(vehicle) { }
// HAL::Callbacks implementation.
void setup() override; void setup() override;
void loop() override; void loop() override;
void flush_logger(void);
void show_packet_counts();
bool check_solution = false;
const char *log_filename = NULL;
bool generate_fpe = true;
/*
information about a log from find_log_info
*/
struct log_information {
uint16_t update_rate;
bool have_imu2:1;
bool have_imt:1;
bool have_imt2:1;
} log_info {};
// return true if a user parameter of name is set
bool check_user_param(const char *name);
private: private:
const char *filename; const char *filename;
ReplayVehicle &_vehicle; ReplayVehicle &_vehicle;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL LogReader reader{_vehicle.log_structure, _vehicle.ekf2, _vehicle.ekf3};
SITL::SITL sitl;
#endif
LogReader logreader{_vehicle.ahrs,
_vehicle.ins,
_vehicle.compass,
_vehicle.gps,
_vehicle.airspeed,
_vehicle.logger,
_vehicle.log_structure,
0,
nottypes};
FILE *ekf1f;
FILE *ekf2f;
FILE *ekf3f;
FILE *ekf4f;
bool done_baro_init;
bool done_home_init;
int32_t arm_time_ms = -1;
bool ahrs_healthy;
bool use_imt = true;
bool check_generate = false;
float tolerance_euler = 3;
float tolerance_pos = 2;
float tolerance_vel = 2;
const char **nottypes = NULL;
uint16_t downsample = 0;
bool logmatch = false;
uint32_t output_counter = 0;
uint64_t last_timestamp = 0;
bool packet_counts = false;
struct {
float max_roll_error;
float max_pitch_error;
float max_yaw_error;
float max_pos_error;
float max_alt_error;
float max_vel_error;
} check_result {};
void _parse_command_line(uint8_t argc, char * const argv[]); void _parse_command_line(uint8_t argc, char * const argv[]);
struct user_parameter {
struct user_parameter *next;
char name[17];
float value;
} *user_parameters;
void set_ins_update_rate(uint16_t update_rate);
void inhibit_gyro_cal();
void force_log_disarmed();
void usage(void);
void set_user_parameters(void);
void read_sensors(const char *type, uint8_t core);
void write_ekf_logs(void);
void log_check_generate();
void log_check_solution();
bool show_error(const char *text, float max_error, float tolerance);
void report_checks();
bool find_log_info(struct log_information &info);
const char **parse_list_from_string(const char *str);
bool parse_param_line(char *line, char **vname, float &value);
void load_param_file(const char *filename);
void set_signal_handlers(void);
void flush_and_exit();
FILE *xfopen(const char *f, const char *mode);
bool seen_non_fmt;
}; };
/*
Replay specific log structures
*/
struct PACKED log_Chek {
LOG_PACKET_HEADER;
uint64_t time_us;
int16_t roll;
int16_t pitch;
uint16_t yaw;
int32_t lat;
int32_t lng;
float alt;
float vnorth;
float veast;
float vdown;
};
extern Replay replay;

79
Tools/Replay/check_replay.py Executable file
View File

@ -0,0 +1,79 @@
#!/usr/bin/env python
'''
check that replay produced identical results
'''
import time
from argparse import ArgumentParser
parser = ArgumentParser(description=__doc__)
parser.add_argument("--condition", default=None, help="condition for packets")
parser.add_argument("--ekf2-only", action='store_true', help="only check EKF2")
parser.add_argument("--ekf3-only", action='store_true', help="only check EKF3")
parser.add_argument("--verbose", action='store_true', help="verbose output")
parser.add_argument("logs", metavar="LOG", nargs="+")
args = parser.parse_args()
from pymavlink import mavutil
def check_log(logfile):
'''check replay log for matching output'''
print("Processing log %s" % filename)
count = 0
errors = 0
counts = {}
mlog = mavutil.mavlink_connection(filename)
ek2_list = ['NKF1','NKF2','NKF3','NKF4','NKF5','NKF0','NKQ']
ek3_list = ['XKF1','XKF2','XKF3','XKF4','XKF0','XKFS','XKQ','XKFD','XKV1','XKV2']
if args.ekf2_only:
mlist = ek2_list
elif args.ekf3_only:
mlist = ek3_list
else:
mlist = ek2_list + ek3_list
base = {}
for m in mlist:
base[m] = {}
while True:
m = mlog.recv_match(type=mlist)
if m is None:
break
if not hasattr(m,'C'):
continue
mtype = m.get_type()
core = m.C
if core < 100:
base[mtype][core] = m
continue
mb = base[mtype][core-100]
count += 1
if not mtype in counts:
counts[mtype] = 0
counts[mtype] += 1
mismatch = False
for f in m._fieldnames:
if f == 'C':
continue
v1 = getattr(m,f)
v2 = getattr(mb,f)
if v1 != v2:
mismatch = True
errors += 1
print("Mismatch in field %s.%s: %s %s" % (mtype, f, str(v1), str(v2)))
if mismatch:
print(mb)
print(m)
print("Processed %u messages, %u errors" % (count, errors))
if args.verbose:
print(counts)
for filename in args.logs:
check_log(filename)