mirror of https://github.com/ArduPilot/ardupilot
Replay: correct various atrophications in Replay tool
Replay: tear down threads before exitting NKQ is a generated name - don't copy it across to output Stop whinging about presence of NKF6 and friends; we know these generated names are not going to be present in modern logs memcpy rather than strncpy within log_FMT Correct strings vs optionally-terminated structure entries in sanity checks Call AP_Param::load_all() to start the parameter saving thread. AP_Compass' init() method now saves parameters (compass reordering), and because we're disarmed we will block until the parameter is pushed onto the to-save queue; if there's no thread popping off that list we block indefinitely. Remove duplicate definitions of various singleton objects. Replay: write out GPS message to output log Useful for diagnosis, but also because we struggle to find a time base without this and the pymavlink tools take forever to work Replay: set COMPASS_DEV_ID and COMPASS_PRIO1_ID so EKF gets mag data Replay: avoid use of system clock; use stopped-clock only Replay: constraint to emitting output for single core only
This commit is contained in:
Peter Barker
Andrew Tridgell
parent
6cc6daa150
commit
1cefd2943b
|
|
@ -61,7 +61,7 @@ void AP_LoggerFileReader::get_packet_counts(uint64_t dest[])
|
|||
memcpy(dest, packet_counts, sizeof(packet_counts));
|
||||
}
|
||||
|
||||
bool AP_LoggerFileReader::update(char type[5])
|
||||
bool AP_LoggerFileReader::update(char type[5], uint8_t &core)
|
||||
{
|
||||
uint8_t hdr[3];
|
||||
if (read_input(hdr, 3) != 3) {
|
||||
|
|
@ -107,5 +107,5 @@ bool AP_LoggerFileReader::update(char type[5])
|
|||
type[4] = 0;
|
||||
|
||||
message_count++;
|
||||
return handle_msg(f,msg);
|
||||
return handle_msg(f, msg, core);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -12,10 +12,10 @@ public:
|
|||
~AP_LoggerFileReader();
|
||||
|
||||
bool open_log(const char *logfile);
|
||||
bool update(char type[5]);
|
||||
bool update(char type[5], uint8_t &core);
|
||||
|
||||
virtual bool handle_log_format_msg(const struct log_Format &f) = 0;
|
||||
virtual bool handle_msg(const struct log_Format &f, uint8_t *msg) = 0;
|
||||
virtual bool handle_msg(const struct log_Format &f, uint8_t *msg, uint8_t &core) = 0;
|
||||
|
||||
void format_type(uint16_t type, char dest[5]);
|
||||
void get_packet_counts(uint64_t dest[]);
|
||||
|
|
|
|||
|
|
@ -2,6 +2,8 @@
|
|||
#include "LogReader.h"
|
||||
#include "Replay.h"
|
||||
|
||||
#include <AP_HAL_Linux/Scheduler.h>
|
||||
|
||||
#include <cinttypes>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
|
@ -80,6 +82,31 @@ 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)
|
||||
{
|
||||
|
|
@ -180,6 +207,10 @@ void LR_MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *ms
|
|||
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);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -408,7 +439,7 @@ void LR_MsgHandler_PARM::process_message(uint8_t *msg)
|
|||
// AP_Logger's IO only when stop_clock is called, we can
|
||||
// overflow AP_Logger's ringbuffer. This should force us to
|
||||
// do IO:
|
||||
hal.scheduler->stop_clock(last_timestamp_usec);
|
||||
hal.scheduler->stop_clock(((Linux::Scheduler*)hal.scheduler)->stopped_clock_usec());
|
||||
}
|
||||
|
||||
require_field(msg, "Name", parameter_name, parameter_name_len);
|
||||
|
|
|
|||
|
|
@ -12,6 +12,12 @@ public:
|
|||
AP_Logger &_logger,
|
||||
uint64_t &last_timestamp_usec);
|
||||
virtual void process_message(uint8_t *msg) = 0;
|
||||
virtual void process_message(uint8_t *msg, uint8_t &core) {
|
||||
// base implementation just ignores the core parameter;
|
||||
// subclasses can override to fill the core in if they feel
|
||||
// like it.
|
||||
process_message(msg);
|
||||
}
|
||||
|
||||
// state for CHEK message
|
||||
struct CheckState {
|
||||
|
|
@ -80,8 +86,19 @@ public:
|
|||
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_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
|
||||
{
|
||||
|
|
|
|||
|
|
@ -9,6 +9,8 @@
|
|||
#include <AP_InertialSensor/AP_InertialSensor.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
#include <AP_HAL_Linux/Scheduler.h>
|
||||
|
||||
#include "LogReader.h"
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
|
|
@ -116,9 +118,9 @@ static const char *generated_names[] = {
|
|||
"FMT",
|
||||
"FMTU",
|
||||
"NKF1", "NKF2", "NKF3", "NKF4", "NKF5", "NKF6", "NKF7", "NKF8", "NKF9", "NKF0",
|
||||
"NKQ1", "NKQ2",
|
||||
"NKQ", "NKQ1", "NKQ2",
|
||||
"XKF1", "XKF2", "XKF3", "XKF4", "XKF5", "XKF6", "XKF7", "XKF8", "XKF9", "XKF0",
|
||||
"XKQ1", "XKQ2", "XKFD", "XKV1", "XKV2",
|
||||
"XKQ", "XKQ1", "XKQ2", "XKFD", "XKV1", "XKV2",
|
||||
"AHR2",
|
||||
"ORGN",
|
||||
"POS",
|
||||
|
|
@ -182,11 +184,13 @@ void LogReader::initialise_fmt_map()
|
|||
// with....
|
||||
continue;
|
||||
}
|
||||
::fprintf(stderr, "Failed to find apparently-generated-name (%s) in COMMON_LOG_STRUCTURES\n", *name);
|
||||
if (strncmp(*name, "NK", 2)==0 || strncmp(*name, "XK", 2) == 0) {
|
||||
// cope with older logs
|
||||
// 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();
|
||||
}
|
||||
}
|
||||
|
|
@ -206,7 +210,7 @@ uint8_t LogReader::map_fmt_type(const char *name, uint8_t intype)
|
|||
return mapped_msgid[intype];
|
||||
}
|
||||
for (uint8_t n=next_msgid; n<255; n++) {
|
||||
::fprintf(stderr, "next_msgid=%u\n", next_msgid);
|
||||
::fprintf(stderr, "next_msgid=%u\n", n);
|
||||
bool already_mapped = false;
|
||||
for (uint16_t i=0; i<sizeof(mapped_msgid); i++) {
|
||||
if (mapped_msgid[i] == n) {
|
||||
|
|
@ -288,9 +292,9 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
|
|||
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));
|
||||
memcpy(pkt.name, s.name, sizeof(pkt.name));
|
||||
memcpy(pkt.format, s.format, sizeof(pkt.format));
|
||||
memcpy(pkt.labels, s.labels, sizeof(pkt.labels));
|
||||
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
|
|
@ -409,7 +413,7 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
|
|||
return true;
|
||||
}
|
||||
|
||||
bool LogReader::handle_msg(const struct log_Format &f, uint8_t *msg) {
|
||||
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);
|
||||
|
|
@ -429,7 +433,7 @@ bool LogReader::handle_msg(const struct log_Format &f, uint8_t *msg) {
|
|||
// a MsgHandler would probably have found a timestamp and
|
||||
// caled stop_clock. This runs IO, clearing logger's
|
||||
// buffer.
|
||||
hal.scheduler->stop_clock(last_timestamp_usec);
|
||||
hal.scheduler->stop_clock(((Linux::Scheduler*)hal.scheduler)->stopped_clock_usec());
|
||||
}
|
||||
|
||||
LR_MsgHandler *p = msgparser[f.type];
|
||||
|
|
@ -437,27 +441,13 @@ bool LogReader::handle_msg(const struct log_Format &f, uint8_t *msg) {
|
|||
return true;
|
||||
}
|
||||
|
||||
p->process_message(msg);
|
||||
p->process_message(msg, core);
|
||||
|
||||
maybe_install_vehicle_specific_parsers();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool LogReader::wait_type(const char *wtype)
|
||||
{
|
||||
while (true) {
|
||||
char type[5];
|
||||
if (!update(type)) {
|
||||
return false;
|
||||
}
|
||||
if (streq(type,wtype)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool LogReader::set_parameter(const char *name, float value)
|
||||
{
|
||||
enum ap_var_type var_type;
|
||||
|
|
|
|||
|
|
@ -15,7 +15,6 @@ public:
|
|||
struct LogStructure *log_structure,
|
||||
uint8_t log_structure_count,
|
||||
const char **¬types);
|
||||
bool wait_type(const char *type);
|
||||
|
||||
const Vector3f &get_attitude(void) const { return attitude; }
|
||||
const Vector3f &get_ahr2_attitude(void) const { return ahr2_attitude; }
|
||||
|
|
@ -35,7 +34,7 @@ public:
|
|||
|
||||
uint64_t last_timestamp_us(void) const { return last_timestamp_usec; }
|
||||
bool handle_log_format_msg(const struct log_Format &f) override;
|
||||
bool handle_msg(const struct log_Format &f, uint8_t *msg) override;
|
||||
bool handle_msg(const struct log_Format &f, uint8_t *msg, uint8_t &core) override;
|
||||
|
||||
static bool in_list(const char *type, const char *list[]);
|
||||
|
||||
|
|
|
|||
|
|
@ -17,6 +17,16 @@ char *xstrdup(const char *string)
|
|||
return ret;
|
||||
}
|
||||
|
||||
char *xcalloc(uint8_t count, uint32_t len)
|
||||
{
|
||||
char *ret = (char*)calloc(count, len);
|
||||
if (ret == nullptr) {
|
||||
perror("calloc");
|
||||
fatal("calloc failed");
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
void MsgHandler::add_field_type(char type, size_t size)
|
||||
{
|
||||
size_for_type_table[(type > 'A' ? (type-'A') : (type-'a'))] = size;
|
||||
|
|
@ -81,22 +91,31 @@ void MsgHandler::add_field(const char *_label, uint8_t _type, uint8_t _offset,
|
|||
next_field++;
|
||||
}
|
||||
|
||||
char *get_string_field(char *field, uint8_t fieldlen)
|
||||
{
|
||||
char *ret = xcalloc(1, fieldlen+1);
|
||||
memcpy(ret, field, fieldlen);
|
||||
return ret;
|
||||
}
|
||||
|
||||
void MsgHandler::parse_format_fields()
|
||||
{
|
||||
char *labels = xstrdup(f.labels);
|
||||
char *labels = get_string_field(f.labels, sizeof(f.labels));
|
||||
char * arg = labels;
|
||||
uint8_t label_offset = 0;
|
||||
char *next_label;
|
||||
uint8_t msg_offset = 3; // 3 bytes for the header
|
||||
|
||||
char *format = get_string_field(f.format, ARRAY_SIZE(f.format));
|
||||
|
||||
while ((next_label = strtok(arg, ",")) != NULL) {
|
||||
if (label_offset > strlen(f.format)) {
|
||||
free(labels);
|
||||
printf("too few field times for labels %s (format=%s) (labels=%s)\n",
|
||||
f.name, f.format, f.labels);
|
||||
exit(1);
|
||||
}
|
||||
uint8_t field_type = f.format[label_offset];
|
||||
if (label_offset > strlen(format)) {
|
||||
free(labels);
|
||||
printf("too few field times for labels %s (format=%s) (labels=%s)\n",
|
||||
f.name, format, labels);
|
||||
exit(1);
|
||||
}
|
||||
uint8_t field_type = format[label_offset];
|
||||
uint8_t length = size_for_type(field_type);
|
||||
add_field(next_label, field_type, msg_offset, length);
|
||||
arg = NULL;
|
||||
|
|
@ -104,12 +123,13 @@ void MsgHandler::parse_format_fields()
|
|||
label_offset++;
|
||||
}
|
||||
|
||||
if (label_offset != strlen(f.format)) {
|
||||
if (label_offset != strlen(format)) {
|
||||
printf("too few labels for format (format=%s) (labels=%s)\n",
|
||||
f.format, f.labels);
|
||||
format, labels);
|
||||
}
|
||||
|
||||
free(labels);
|
||||
free(format);
|
||||
}
|
||||
|
||||
bool MsgHandler::field_value(uint8_t *msg, const char *label, char *ret, uint8_t retlen)
|
||||
|
|
|
|||
|
|
@ -14,6 +14,7 @@
|
|||
*/
|
||||
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <AP_HAL_Linux/Scheduler.h>
|
||||
#include <GCS_MAVLink/GCS_Dummy.h>
|
||||
#include "Parameters.h"
|
||||
#include "VehicleType.h"
|
||||
|
|
@ -38,7 +39,7 @@
|
|||
|
||||
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
||||
|
||||
ReplayVehicle replayvehicle;
|
||||
static ReplayVehicle replayvehicle;
|
||||
|
||||
struct globals globals;
|
||||
|
||||
|
|
@ -93,13 +94,37 @@ void ReplayVehicle::load_parameters(void)
|
|||
if (!AP_Param::check_var_info()) {
|
||||
AP_HAL::panic("Bad parameter table");
|
||||
}
|
||||
AP_Param::set_default_by_name("EK2_ENABLE", 1);
|
||||
AP_Param::set_default_by_name("EK2_IMU_MASK", 1);
|
||||
AP_Param::set_default_by_name("EK3_ENABLE", 1);
|
||||
AP_Param::set_default_by_name("EK3_IMU_MASK", 1);
|
||||
AP_Param::set_default_by_name("LOG_REPLAY", 1);
|
||||
AP_Param::set_default_by_name("AHRS_EKF_TYPE", 2);
|
||||
AP_Param::set_default_by_name("LOG_FILE_BUFSIZE", 60);
|
||||
// Load all auto-loaded EEPROM variables - also registers thread
|
||||
// which saves parameters, which Compass now does in its init() routine
|
||||
AP_Param::load_all();
|
||||
|
||||
// this compass has always been at war with the new compass priority code
|
||||
const uint32_t compass_dev_id = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, 0, 0, AP_Compass_Backend::DevTypes::DEVTYPE_SITL);
|
||||
// logreader.set_parameter("DEV_ID", compass_dev_id);
|
||||
// logreader.set_parameter("PRIO1_ID", compass_dev_id);
|
||||
AP_Param::set_default_by_name("DEV_ID", compass_dev_id);
|
||||
AP_Param::set_default_by_name("PRIO1_ID", compass_dev_id);
|
||||
|
||||
const struct param_default {
|
||||
const char *name;
|
||||
float value;
|
||||
} param_defaults[] {
|
||||
{ "EK2_ENABLE", 1 },
|
||||
{ "EK2_IMU_MASK", 1 },
|
||||
{ "EK3_ENABLE", 1 },
|
||||
{ "EK3_IMU_MASK", 1 },
|
||||
{ "LOG_REPLAY", 1 },
|
||||
{ "AHRS_EKF_TYPE", 2 },
|
||||
{ "LOG_FILE_BUFSIZE", 60 },
|
||||
{ "COMPASS_DEV_ID", (float)compass_dev_id },
|
||||
{ "COMPASS_PRIO1_ID", (float)compass_dev_id },
|
||||
};
|
||||
for (auto some_default : param_defaults) {
|
||||
if (!AP_Param::set_default_by_name(some_default.name, some_default.value)) {
|
||||
::fprintf(stderr, "set default failed\n");
|
||||
abort();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ReplayVehicle::init_ardupilot(void)
|
||||
|
|
@ -131,6 +156,7 @@ void ReplayVehicle::init_ardupilot(void)
|
|||
}
|
||||
|
||||
Replay replay(replayvehicle);
|
||||
AP_Vehicle& vehicle = replayvehicle;
|
||||
|
||||
void Replay::usage(void)
|
||||
{
|
||||
|
|
@ -330,7 +356,7 @@ class IMUCounter : public AP_LoggerFileReader {
|
|||
public:
|
||||
IMUCounter() {}
|
||||
bool handle_log_format_msg(const struct log_Format &f) override;
|
||||
bool handle_msg(const struct log_Format &f, uint8_t *msg) override;
|
||||
bool handle_msg(const struct log_Format &f, uint8_t *msg, uint8_t &core) override;
|
||||
|
||||
uint64_t last_clock_timestamp = 0;
|
||||
float last_parm_value = 0;
|
||||
|
|
@ -354,7 +380,7 @@ bool IMUCounter::handle_log_format_msg(const struct log_Format &f) {
|
|||
return true;
|
||||
};
|
||||
|
||||
bool IMUCounter::handle_msg(const struct log_Format &f, uint8_t *msg) {
|
||||
bool IMUCounter::handle_msg(const struct log_Format &f, uint8_t *msg, uint8_t &core) {
|
||||
if (strncmp(f.name,"PARM",4) == 0) {
|
||||
// gather parameter values to check for SCHED_LOOP_RATE
|
||||
parm_handler->field_value(msg, "Name", last_parm_name, sizeof(last_parm_name));
|
||||
|
|
@ -395,7 +421,8 @@ bool Replay::find_log_info(struct log_information &info)
|
|||
const uint16_t samples_required = 1000;
|
||||
while (samplecount < samples_required) {
|
||||
char type[5];
|
||||
if (!reader.update(type)) {
|
||||
uint8_t core; // unused
|
||||
if (!reader.update(type, core)) {
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -513,6 +540,9 @@ void Replay::setup()
|
|||
|
||||
set_signal_handlers();
|
||||
|
||||
// never use the system clock:
|
||||
hal.scheduler->stop_clock(1);
|
||||
|
||||
hal.console->printf("Processing log %s\n", filename);
|
||||
|
||||
// remember filename for reporting
|
||||
|
|
@ -535,6 +565,7 @@ void Replay::setup()
|
|||
inhibit_gyro_cal();
|
||||
force_log_disarmed();
|
||||
|
||||
// FIXME: base this on key parameters rather than the loop rate
|
||||
if (log_info.update_rate == 400) {
|
||||
// assume copter for 400Hz
|
||||
_vehicle.ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
|
||||
|
|
@ -624,7 +655,7 @@ void Replay::write_ekf_logs(void)
|
|||
}
|
||||
}
|
||||
|
||||
void Replay::read_sensors(const char *type)
|
||||
void Replay::read_sensors(const char *type, uint8_t core)
|
||||
{
|
||||
if (streq(type, "PARM")) {
|
||||
set_user_parameters();
|
||||
|
|
@ -669,7 +700,7 @@ void Replay::read_sensors(const char *type)
|
|||
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());
|
||||
::fprintf(stderr, "EKF force-started at %" PRIu64 "\n", AP_HAL::micros64());
|
||||
ekf_force_started = true;
|
||||
}
|
||||
}
|
||||
|
|
@ -709,8 +740,12 @@ void Replay::read_sensors(const char *type)
|
|||
log_check_solution();
|
||||
}
|
||||
}
|
||||
|
||||
if (logmatch && (streq(type, "NKF1") || streq(type, "XKF1"))) {
|
||||
|
||||
// 255 here is a special marker for "no core present in log".
|
||||
// This may give us a hope of backwards-compatability.
|
||||
if (logmatch &&
|
||||
(streq(type, "NKF1") || streq(type, "XKF1")) &&
|
||||
(core == 0 || core == 255)) {
|
||||
write_ekf_logs();
|
||||
}
|
||||
}
|
||||
|
|
@ -788,6 +823,10 @@ void Replay::flush_and_exit()
|
|||
show_packet_counts();
|
||||
}
|
||||
|
||||
// If we don't tear down the threads then they continue to access
|
||||
// global state during object destruction.
|
||||
((Linux::Scheduler*)hal.scheduler)->teardown();
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
|
@ -811,6 +850,7 @@ void Replay::show_packet_counts()
|
|||
void Replay::loop()
|
||||
{
|
||||
char type[5];
|
||||
uint8_t core;
|
||||
|
||||
if (arm_time_ms >= 0 && AP_HAL::millis() > (uint32_t)arm_time_ms) {
|
||||
if (!hal.util->get_soft_armed()) {
|
||||
|
|
@ -819,19 +859,26 @@ void Replay::loop()
|
|||
}
|
||||
}
|
||||
|
||||
if (!logreader.update(type)) {
|
||||
if (!logreader.update(type, core)) {
|
||||
::printf("End of log at %.1f seconds\n", AP_HAL::millis()*0.001f);
|
||||
flush_and_exit();
|
||||
}
|
||||
|
||||
const uint64_t now64 = AP_HAL::micros64();
|
||||
if (last_timestamp != 0) {
|
||||
uint64_t gap = AP_HAL::micros64() - last_timestamp;
|
||||
if (gap > 40000) {
|
||||
::printf("Gap in log at timestamp=%" PRIu64 " of length %" PRIu64 "us\n",
|
||||
last_timestamp, gap);
|
||||
if (now64 < last_timestamp) {
|
||||
::printf("time going backwards?! now=%" PRIu64 " last_timestamp=%" PRIu64 "us\n",
|
||||
now64, last_timestamp);
|
||||
exit(1);
|
||||
} else {
|
||||
const uint64_t gap = now64 - last_timestamp;
|
||||
if (gap > 40000) {
|
||||
::printf("Gap in log at timestamp=%" PRIu64 " of length %" PRIu64 "us\n",
|
||||
last_timestamp, gap);
|
||||
}
|
||||
}
|
||||
}
|
||||
last_timestamp = AP_HAL::micros64();
|
||||
last_timestamp = now64;
|
||||
|
||||
if (streq(type, "FMT")) {
|
||||
if (!seen_non_fmt) {
|
||||
|
|
@ -841,7 +888,7 @@ void Replay::loop()
|
|||
seen_non_fmt = true;
|
||||
}
|
||||
|
||||
read_sensors(type);
|
||||
read_sensors(type, core);
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -58,18 +58,15 @@ public:
|
|||
void load_parameters(void) override;
|
||||
void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
|
||||
uint8_t &task_count,
|
||||
uint32_t &log_bit) override { };
|
||||
uint32_t &log_bit) override {
|
||||
tasks = nullptr;
|
||||
task_count = 0;
|
||||
log_bit = 0;
|
||||
};
|
||||
|
||||
virtual bool set_mode(const uint8_t new_mode, const ModeReason reason) override { return true; }
|
||||
virtual uint8_t get_mode() const override { return 0; }
|
||||
|
||||
AP_InertialSensor ins;
|
||||
AP_Baro barometer;
|
||||
AP_GPS gps;
|
||||
Compass compass;
|
||||
AP_SerialManager serial_manager;
|
||||
RangeFinder rng;
|
||||
AP_AHRS_NavEKF ahrs;
|
||||
AP_Vehicle::FixedWing aparm;
|
||||
AP_Airspeed airspeed;
|
||||
AP_Int32 unused; // logging is magic for Replay; this is unused
|
||||
|
|
@ -183,7 +180,7 @@ private:
|
|||
|
||||
void usage(void);
|
||||
void set_user_parameters(void);
|
||||
void read_sensors(const char *type);
|
||||
void read_sensors(const char *type, uint8_t core);
|
||||
void write_ekf_logs(void);
|
||||
void log_check_generate();
|
||||
void log_check_solution();
|
||||
|
|
|
|||
Loading…
Reference in New Issue