From b469ad91f4425dc549da1af1e6fdf9991433c60d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 9 Mar 2020 14:28:23 +1100 Subject: [PATCH 1/4] 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 --- Tools/Replay/DataFlashFileReader.cpp | 4 +- Tools/Replay/DataFlashFileReader.h | 4 +- Tools/Replay/LR_MsgHandler.cpp | 33 +++++++++- Tools/Replay/LR_MsgHandler.h | 17 ++++++ Tools/Replay/LogReader.cpp | 40 +++++------- Tools/Replay/LogReader.h | 3 +- Tools/Replay/MsgHandler.cpp | 40 +++++++++--- Tools/Replay/Replay.cpp | 91 +++++++++++++++++++++------- Tools/Replay/Replay.h | 15 ++--- 9 files changed, 174 insertions(+), 73 deletions(-) diff --git a/Tools/Replay/DataFlashFileReader.cpp b/Tools/Replay/DataFlashFileReader.cpp index 490169be65dd0..3ab73e5ef8706 100644 --- a/Tools/Replay/DataFlashFileReader.cpp +++ b/Tools/Replay/DataFlashFileReader.cpp @@ -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); } diff --git a/Tools/Replay/DataFlashFileReader.h b/Tools/Replay/DataFlashFileReader.h index fce4283a2f1b0..7d70ab8363b1e 100644 --- a/Tools/Replay/DataFlashFileReader.h +++ b/Tools/Replay/DataFlashFileReader.h @@ -12,10 +12,10 @@ class AP_LoggerFileReader ~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[]); diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index 2e6274011a777..5e88977203cdc 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -2,6 +2,8 @@ #include "LogReader.h" #include "Replay.h" +#include + #include 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); diff --git a/Tools/Replay/LR_MsgHandler.h b/Tools/Replay/LR_MsgHandler.h index 8f343a8563ecb..871ace5b00a1d 100644 --- a/Tools/Replay/LR_MsgHandler.h +++ b/Tools/Replay/LR_MsgHandler.h @@ -12,6 +12,12 @@ class LR_MsgHandler : public MsgHandler { 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 @@ class LR_MsgHandler_NKF1 : public LR_MsgHandler 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 { diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index aec2f88b1b470..695a8fd63e5be 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -9,6 +9,8 @@ #include #include +#include + #include "LogReader.h" #include #include @@ -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; istop_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; diff --git a/Tools/Replay/LogReader.h b/Tools/Replay/LogReader.h index 8a7f8da64fd3b..739cb3fff0947 100644 --- a/Tools/Replay/LogReader.h +++ b/Tools/Replay/LogReader.h @@ -15,7 +15,6 @@ class LogReader : public AP_LoggerFileReader 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 @@ class LogReader : public AP_LoggerFileReader 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[]); diff --git a/Tools/Replay/MsgHandler.cpp b/Tools/Replay/MsgHandler.cpp index cee5779a418a1..bc6e368e0c360 100644 --- a/Tools/Replay/MsgHandler.cpp +++ b/Tools/Replay/MsgHandler.cpp @@ -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) diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index ec7fd6ab014a1..475483d3fcf94 100644 --- a/Tools/Replay/Replay.cpp +++ b/Tools/Replay/Replay.cpp @@ -14,6 +14,7 @@ */ #include +#include #include #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); } diff --git a/Tools/Replay/Replay.h b/Tools/Replay/Replay.h index 25c13c65a0736..1e0af1fdad099 100644 --- a/Tools/Replay/Replay.h +++ b/Tools/Replay/Replay.h @@ -58,18 +58,15 @@ class ReplayVehicle : public AP_Vehicle { 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 @@ class Replay : public AP_HAL::HAL::Callbacks { 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(); From 7273f7096f30872dd81e562af8e0ce84ea867e5c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 9 Mar 2020 13:58:28 +1100 Subject: [PATCH 2/4] AP_Vehicle: Replay now creates vehicle singleton reference --- libraries/AP_Vehicle/AP_Vehicle.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 093a9d248e03f..c09076ee7e992 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -42,7 +42,7 @@ const AP_Param::GroupInfo AP_Vehicle::var_info[] = { }; // reference to the vehicle. using AP::vehicle() here does not work on clang -#if APM_BUILD_TYPE(APM_BUILD_Replay) || APM_BUILD_TYPE(APM_BUILD_UNKNOWN) +#if APM_BUILD_TYPE(APM_BUILD_UNKNOWN) AP_Vehicle& vehicle = *AP_Vehicle::get_singleton(); #else extern AP_Vehicle& vehicle; From 5fa8f49a2cd4297f1823986c6cbd2deec2b62b8b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 11 Mar 2020 19:47:44 +1100 Subject: [PATCH 3/4] AP_Vehicle: don't do any GCS stuff in delay callback if we're Replay We haven't initialised the GCS at all, so it's not a great idea to update_receive() and the like. --- libraries/AP_Vehicle/AP_Vehicle.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index c09076ee7e992..a0bbfe7b80845 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -173,6 +173,11 @@ void AP_Vehicle::get_common_scheduler_tasks(const AP_Scheduler::Task*& tasks, ui */ void AP_Vehicle::scheduler_delay_callback() { +#if APM_BUILD_TYPE(APM_BUILD_Replay) + // compass.init() delays, so we end up here. + return; +#endif + static uint32_t last_1hz, last_50hz, last_5s; AP_Logger &logger = AP::logger(); From 270aed740b43fab6872a0ec70b7c38a3c8655210 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 12 Mar 2020 11:37:54 +1100 Subject: [PATCH 4/4] AP_HAL_Linux: throw warning if we ever stop-clock backwards Also don't compile in the stop_clock symbol to make it clear changes in this method won't affect vehicles --- libraries/AP_HAL_Linux/Scheduler.cpp | 18 +++++++++++++++--- libraries/AP_HAL_Linux/Scheduler.h | 1 + 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp index f8847af108c13..c3bd58640b12c 100644 --- a/libraries/AP_HAL_Linux/Scheduler.cpp +++ b/libraries/AP_HAL_Linux/Scheduler.cpp @@ -313,13 +313,25 @@ void Scheduler::reboot(bool hold_in_bootloader) exit(1); } +#if APM_BUILD_TYPE(APM_BUILD_Replay) void Scheduler::stop_clock(uint64_t time_usec) { - if (time_usec >= _stopped_clock_usec) { - _stopped_clock_usec = time_usec; - _run_io(); + if (time_usec < _stopped_clock_usec) { + ::fprintf(stderr, "Warning: setting time backwards from (%" PRIu64 ") to (%" PRIu64 ")\n", _stopped_clock_usec, time_usec); + return; } + + _stopped_clock_usec = time_usec; + _run_io(); } +#else +void Scheduler::stop_clock(uint64_t time_usec) +{ + // stop_clock() is not called outside of Replay, but we can't + // guard it in the header because of the vehicle-dependent-library + // checks in waf. +} +#endif bool Scheduler::SchedulerThread::_run() { diff --git a/libraries/AP_HAL_Linux/Scheduler.h b/libraries/AP_HAL_Linux/Scheduler.h index 03abe785d177f..4556b35d80e6c 100644 --- a/libraries/AP_HAL_Linux/Scheduler.h +++ b/libraries/AP_HAL_Linux/Scheduler.h @@ -3,6 +3,7 @@ #include #include "AP_HAL_Linux.h" + #include "Semaphores.h" #include "Thread.h"