Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Tools: Replay: make it work again

Browse Source 
Tools: Replay: ignore setting of LOG_DISARMED

Otherwise log files that come in with LOG_DISARMED false don't get any
significant output

Tools: Replay: apply user parameters after any PARM message

Tools: Replay: emit timestamp when EKF is force-started

Tools: Replay: use stderr for what it's good for

Tools: Replay: force log disarmed
This commit is contained in:
Peter Barker 2018-04-22 20:19:11 +10:00 committed by Peter Barker
parent af66d72510
commit 5452730fc9
7 changed files with 229 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
Tools/Replay/DataFlashFileReader.cpp
View File

@ -88,11 +88,6 @@ bool DataFlashFileReader::update(char type[5])
return handle_log_format_msg(f);
}
if (!done_format_msgs) {
done_format_msgs = true;
end_format_msgs();
}
const struct log_Format &f = formats[hdr[2]];
if (f.length == 0) {
// can't just throw these away as the format specifies the

2
Tools/Replay/DataFlashFileReader.h
View File

@ -22,8 +22,6 @@ public:
protected:
int fd = -1;
bool done_format_msgs = false;
virtual void end_format_msgs(void) {}
struct log_Format formats[LOGREADER_MAX_FORMATS] {};

3
Tools/Replay/LR_MsgHandler.cpp
View File

@ -383,7 +383,8 @@ 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"
"COMPASS_ORIENT", "COMPASS_ORIENT2",
"COMPASS_ORIENT3", "LOG_FILE_BUFSIZE"};
"COMPASS_ORIENT3", "LOG_FILE_BUFSIZE",
"LOG_DISARMED"};
for (uint8_t i=0; i < ARRAY_SIZE(ignore_parms); i++) {
if (strncmp(name, ignore_parms[i], AP_MAX_NAME_SIZE) == 0) {
::printf("Ignoring set of %s to %f\n", name, value);

226
Tools/Replay/LogReader.cpp
View File

@ -18,7 +18,7 @@
#include "MsgHandler.h"
#include "Replay.h"
#define DEBUG 0
#define DEBUG 1
#if DEBUG
# define debug(fmt, args...) printf(fmt "\n", ##args)
#else
@ -29,12 +29,19 @@
extern const AP_HAL::HAL& hal;
const struct LogStructure log_structure[] = {
const struct LogStructure running_codes_log_structure[] = {
LOG_COMMON_STRUCTURES,
};
LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, Compass &_compass, AP_GPS &_gps,
AP_Airspeed &_airspeed, DataFlash_Class &_dataflash, const char **&_nottypes):
LogReader::LogReader(AP_AHRS &_ahrs,
AP_InertialSensor &_ins,
Compass &_compass,
AP_GPS &_gps,
AP_Airspeed &_airspeed,
DataFlash_Class &_dataflash,
struct LogStructure *log_structure,
uint8_t log_structure_count,
const char **&_nottypes):
DataFlashFileReader(),
vehicle(VehicleType::VEHICLE_UNKNOWN),
ahrs(_ahrs),
@ -47,8 +54,15 @@ LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, Compass &_compass,
gyro_mask(7),
last_timestamp_usec(0),
installed_vehicle_specific_parsers(false),
_log_structure(log_structure),
nottypes(_nottypes)
{
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];
@ -83,17 +97,53 @@ void LogReader::maybe_install_vehicle_specific_parsers() {
}
/*
messages which we will be generating, so should be discarded
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[] = { "EKF1", "EKF2", "EKF3", "EKF4", "EKF5",
"NKF1", "NKF2", "NKF3", "NKF4", "NKF5",
"NKF6", "NKF7", "NKF8", "NKF9", "NKF10",
"AHR2", "POS", "CHEK",
"IMT", "IMT2",
"MAG", "MAG2",
"BARO", "BAR2",
"GPS","GPA",
"NKA", "NKV", NULL };
static const char *generated_names[] = {
"FMT",
"FMTU",
"NKF1", "NKF2", "NKF3", "NKF4", "NKF5", "NKF6", "NKF7", "NKF8", "NKF9", "NKF0",
"NKQ1", "NKQ2",
"XKF1", "XKF2", "XKF3", "XKF4", "XKF5", "XKF6", "XKF7", "XKF8", "XKF9", "XKF0",
"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
// 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
// the message is emitted to the log. However, we must not write the
// messages from the old log to the new log, so we need to keep a map
// of IDs to prune out...
static const char *log_write_names[] = {
"NKA",
"NKV",
"NKT1",
"NKT2",
nullptr
};
/*
see if a type is in a list of types
@ -111,30 +161,78 @@ bool LogReader::in_list(const char *type, const char *list[])
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;
}
::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
*/
uint8_t LogReader::map_fmt_type(const char *name, uint8_t intype)
{
if (intype == 128) {
// everybody's favourite FMT message...
return 128;
}
if (mapped_msgid[intype] != 0) {
// already mapped
return mapped_msgid[intype];
}
// see if it is in our structure list
for (uint8_t i=0; i<ARRAY_SIZE(log_structure); i++) {
if (strcmp(name, log_structure[i].name) == 0) {
mapped_msgid[intype] = log_structure[i].msg_type;
return mapped_msgid[intype];
for (uint8_t n=next_msgid; n<255; n++) {
::fprintf(stderr, "next_msgid=%u\n", next_msgid);
bool already_mapped = false;
for (uint16_t i=0; i<sizeof(mapped_msgid); i++) {
if (mapped_msgid[i] == n) {
// already mapped - must be one of our generated names
already_mapped = true;
break;
}
}
if (already_mapped) {
continue;
}
if (DataFlash_Class::instance()->msg_type_in_use(n)) {
continue;
}
mapped_msgid[intype] = n;
next_msgid = n+1;
break;
}
// it is a new one, allocate an ID
mapped_msgid[intype] = next_msgid++;
if (mapped_msgid[intype] == 0) {
::fprintf(stderr, "mapping failed\n");
abort();
}
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;
}
@ -148,21 +246,53 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
memcpy(name, f.name, 4);
debug("Defining log format for type (%d) (%s)\n", f.type, name);
if (save_message_type(name)) {
/*
any messages which we won't be generating internally in
replay should get the original FMT header
We need to remap the type in the FMT header to avoid
conflicts with our current table
*/
struct log_Format f_mapped = f;
f_mapped.type = map_fmt_type(name, f.type);
dataflash.WriteBlock(&f_mapped, sizeof(f_mapped));
struct LogStructure s = _log_structure[_log_structure_count++];
dataflash.set_num_types(_log_structure_count);
if (in_list(name, log_write_names)) {
debug("%s is a Log_Write-written message\n", name);
} else {
if (in_list(name, generated_names)) {
debug("Log format for type (%d) (%s) taken from running code\n",
f.type, 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)) {
found = true;
memcpy(&s, &running_codes_log_structure[n], sizeof(LogStructure));
break;
}
}
if (!found) {
::fprintf(stderr, "Expected to be able to emit an FMT for (%s), but no FMT message found in running code\n", name);
abort();
}
} else {
debug("Log format for type (%d) (%s) taken from log\n", f.type, name);
// generate a LogStructure entry for this FMT
s.msg_type = map_fmt_type(name, f.type);
s.msg_len = f.length;
s.name = f.name;
s.format = f.format;
s.labels = f.labels;
}
if (msgparser[f.type] != NULL) {
return true;
}
// emit the FMT to DataFlash:
struct log_Format pkt {};
pkt.head1 = HEAD_BYTE1;
pkt.head2 = HEAD_BYTE2;
pkt.msgid = LOG_FORMAT_MSG;
pkt.type = s.msg_type;
pkt.length = s.msg_len;
strncpy(pkt.name, s.name, sizeof(pkt.name));
strncpy(pkt.format, s.format, sizeof(pkt.format));
strncpy(pkt.labels, s.labels, sizeof(pkt.labels));
dataflash.WriteCriticalBlock(&pkt, sizeof(pkt));
}
if (msgparser[f.type] != NULL) {
return true;
}
// map from format name to a parser subclass:
if (streq(name, "PARM")) {
@ -281,6 +411,9 @@ bool LogReader::handle_msg(const struct log_Format &f, uint8_t *msg) {
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);
@ -350,28 +483,3 @@ bool LogReader::set_parameter(const char *name, float value)
}
return true;
}
/*
called when the last FMT message has been processed
*/
void LogReader::end_format_msgs(void)
{
// write out any formats we will be producing
for (uint8_t i=0; generated_names[i]; i++) {
for (uint8_t n=0; n<ARRAY_SIZE(log_structure); n++) {
if (strcmp(generated_names[i], log_structure[n].name) == 0) {
const struct LogStructure *s = &log_structure[n];
struct log_Format pkt {};
pkt.head1 = HEAD_BYTE1;
pkt.head2 = HEAD_BYTE2;
pkt.msgid = LOG_FORMAT_MSG;
pkt.type = s->msg_type;
pkt.length = s->msg_len;
strncpy(pkt.name, s->name, sizeof(pkt.name));
strncpy(pkt.format, s->format, sizeof(pkt.format));
strncpy(pkt.labels, s->labels, sizeof(pkt.labels));
dataflash.WriteCriticalBlock(&pkt, sizeof(pkt));
}
}
}
}

14
Tools/Replay/LogReader.h
View File

@ -6,7 +6,15 @@
class LogReader : public DataFlashFileReader
{
public:
LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, Compass &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed, DataFlash_Class &_dataflash, const char **&nottypes);
LogReader(AP_AHRS &_ahrs,
AP_InertialSensor &_ins,
Compass &_compass,
AP_GPS &_gps,
AP_Airspeed &_airspeed,
DataFlash_Class &_dataflash,
struct LogStructure *log_structure,
uint8_t log_structure_count,
const char **&nottypes);
bool wait_type(const char *type);
const Vector3f &get_attitude(void) const { return attitude; }
@ -32,7 +40,6 @@ public:
static bool in_list(const char *type, const char *list[]);
protected:
virtual void end_format_msgs(void) override;
private:
AP_AHRS &ahrs;
@ -41,6 +48,8 @@ private:
AP_GPS &gps;
AP_Airspeed &airspeed;
DataFlash_Class &dataflash;
struct LogStructure *_log_structure;
uint8_t _log_structure_count;
uint8_t accel_mask;
uint8_t gyro_mask;
@ -72,6 +81,7 @@ private:
void maybe_install_vehicle_specific_parsers();
void initialise_fmt_map();
uint8_t map_fmt_type(const char *name, uint8_t intype);
bool save_message_type(const char *name);

60
Tools/Replay/Replay.cpp
View File

@ -100,22 +100,16 @@ void ReplayVehicle::load_parameters(void)
AP_Param::set_default_by_name("LOG_FILE_BUFSIZE", 60);
}
static const struct LogStructure min_log_structure[] = {
{ LOG_FORMAT_MSG, sizeof(log_Format),
"FMT", "BBnNZ", "Type,Length,Name,Format,Columns", "-b---", "-----" },
{ LOG_PARAMETER_MSG, sizeof(log_Parameter),
"PARM", "QNf", "TimeUS,Name,Value", "s--", "F--" },
{ LOG_MESSAGE_MSG, sizeof(log_Message),
"MSG", "QZ", "TimeUS,Message", "s-", "F-" },
};
void ReplayVehicle::setup(void)
{
load_parameters();
// we pass a minimal log structure, as we will be outputting the
// log structures we need manually, to prevent FMT duplicates
dataflash.Init(min_log_structure, ARRAY_SIZE(min_log_structure));
// we pass an empty log structure, filling the structure in with
// either the format present in the log (if we do not emit the
// message as a product of Replay), or the format understood in
// the current code (if we do emit the message in the normal
// places in the EKF, for example)
dataflash.Init(log_structure, 0);
ahrs.set_compass(&compass);
ahrs.set_fly_forward(true);
@ -539,6 +533,7 @@ void Replay::setup()
_vehicle.setup();
inhibit_gyro_cal();
force_log_disarmed();
if (log_info.update_rate == 400) {
// assume copter for 400Hz
@ -563,6 +558,12 @@ void Replay::inhibit_gyro_cal() {
abort();
}
}
void Replay::force_log_disarmed() {
if (!logreader.set_parameter("LOG_DISARMED", 1)) {
::fprintf(stderr, "Failed to set LOG_DISARMED parameter\n");
abort();
}
}
/*
setup user -p parameters
@ -625,15 +626,10 @@ void Replay::write_ekf_logs(void)
void Replay::read_sensors(const char *type)
{
if (!done_parameters && !streq(type,"FMT") && !streq(type,"PARM")) {
done_parameters = true;
if (streq(type, "PARM")) {
set_user_parameters();
}
if (done_parameters && streq(type, "PARM")) {
set_user_parameters();
}
if (!done_home_init) {
if (streq(type, "GPS") &&
(_vehicle.gps.status() >= AP_GPS::GPS_OK_FIX_3D) && done_baro_init) {
@ -667,13 +663,21 @@ void Replay::read_sensors(const char *type)
}
}
static bool ekf_force_started = false;
if (!ekf_force_started) {
if (log_info.have_imt2 ||
log_info.have_imt) {
_vehicle.ahrs.force_ekf_start();
::fprintf(stderr, "EKF force-started at %u\n", AP_HAL::micros());
ekf_force_started = true;
}
}
bool run_ahrs = false;
if (log_info.have_imt2) {
run_ahrs = streq(type, "IMT2");
_vehicle.ahrs.force_ekf_start();
} else if (log_info.have_imt) {
run_ahrs = streq(type, "IMT");
_vehicle.ahrs.force_ekf_start();
} else if (log_info.have_imu2) {
run_ahrs = streq(type, "IMU2");
} else {
@ -740,7 +744,7 @@ void Replay::log_check_generate(void)
velocity.x,
velocity.y,
velocity.z
};
);
}
@ -828,11 +832,15 @@ void Replay::loop()
}
last_timestamp = AP_HAL::micros64();
read_sensors(type);
if (!streq(type,"ATT")) {
return;
if (streq(type, "FMT")) {
if (!seen_non_fmt) {
return;
}
} else {
seen_non_fmt = true;
}
read_sensors(type);
}

16
Tools/Replay/Replay.h
View File

@ -68,6 +68,8 @@ public:
AP_Vehicle::FixedWing aparm;
AP_Airspeed airspeed;
AP_Int32 unused; // logging is magic for Replay; this is unused
struct LogStructure log_structure[256] = {
};
DataFlash_Class dataflash{unused};
private:
@ -118,14 +120,21 @@ private:
SITL::SITL sitl;
#endif
LogReader logreader{_vehicle.ahrs, _vehicle.ins, _vehicle.compass, _vehicle.gps, _vehicle.airspeed, _vehicle.dataflash, nottypes};
LogReader logreader{_vehicle.ahrs,
_vehicle.ins,
_vehicle.compass,
_vehicle.gps,
_vehicle.airspeed,
_vehicle.dataflash,
_vehicle.log_structure,
0,
nottypes};
FILE *ekf1f;
FILE *ekf2f;
FILE *ekf3f;
FILE *ekf4f;
bool done_parameters;
bool done_baro_init;
bool done_home_init;
int32_t arm_time_ms = -1;
@ -161,6 +170,7 @@ private:
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);
@ -178,6 +188,8 @@ private:
void flush_and_exit();
FILE *xfopen(const char *f, const char *mode);
bool seen_non_fmt;
};
/*