diff --git a/Tools/Replay/DataFlashFileReader.cpp b/Tools/Replay/DataFlashFileReader.cpp index 207e127368..8f1692bc11 100644 --- a/Tools/Replay/DataFlashFileReader.cpp +++ b/Tools/Replay/DataFlashFileReader.cpp @@ -1,4 +1,5 @@ #include "DataFlashFileReader.h" +#include #include #include @@ -12,28 +13,17 @@ #define PRIu64 "llu" #endif -// flogged from AP_Hal_Linux/system.cpp; we don't want to use stopped clock here -uint64_t now() { - struct timespec ts; - clock_gettime(CLOCK_MONOTONIC, &ts); - return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9))); -} - -AP_LoggerFileReader::AP_LoggerFileReader() : - start_micros(now()) +AP_LoggerFileReader::AP_LoggerFileReader() {} AP_LoggerFileReader::~AP_LoggerFileReader() { - const uint64_t micros = now(); - const uint64_t delta = micros - start_micros; ::printf("Replay counts: %" PRIu64 " bytes %u entries\n", bytes_read, message_count); - ::printf("Replay rates: %" PRIu64 " bytes/second %" PRIu64 " messages/second\n", bytes_read*1000000/delta, message_count*1000000/delta); } bool AP_LoggerFileReader::open_log(const char *logfile) { - fd = ::open(logfile, O_RDONLY|O_CLOEXEC); + fd = AP::FS().open(logfile, O_RDONLY); if (fd == -1) { return false; } @@ -42,7 +32,7 @@ bool AP_LoggerFileReader::open_log(const char *logfile) ssize_t AP_LoggerFileReader::read_input(void *buffer, const size_t count) { - uint64_t ret = ::read(fd, buffer, count); + uint64_t ret = AP::FS().read(fd, buffer, count); bytes_read += ret; return ret; } @@ -72,6 +62,12 @@ bool AP_LoggerFileReader::update() return false; } +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + // running on stm32 is slow enough it is nice to see progress + if (message_count % 500 == 0) { + ::printf("line %u pkt 0x%02x t=%u\n", message_count, hdr[2], AP_HAL::millis()); + } +#endif packet_counts[hdr[2]]++; if (hdr[2] == LOG_FORMAT_MSG) { diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index 3ca05a6426..bc786eca5e 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -2,39 +2,41 @@ #include "LogReader.h" #include "Replay.h" -#include #include #include extern const AP_HAL::HAL& hal; -#define MSG_CAST(sname,msg) *((log_ ##sname *)(msg+3)) +#define MSG_CREATE(sname,msgbytes) log_ ##sname msg; memcpy((void*)&msg, (msgbytes)+3, sizeof(msg)); LR_MsgHandler::LR_MsgHandler(struct log_Format &_f) : MsgHandler(_f) { } -void LR_MsgHandler_RFRH::process_message(uint8_t *msg) +void LR_MsgHandler_RFRH::process_message(uint8_t *msgbytes) { - AP::dal().handle_message(MSG_CAST(RFRH,msg)); + MSG_CREATE(RFRH, msgbytes); + AP::dal().handle_message(msg); } -void LR_MsgHandler_RFRF::process_message(uint8_t *msg) +void LR_MsgHandler_RFRF::process_message(uint8_t *msgbytes) { - AP::dal().handle_message(MSG_CAST(RFRF,msg), ekf2, ekf3); + MSG_CREATE(RFRF, msgbytes); + AP::dal().handle_message(msg, ekf2, ekf3); } -void LR_MsgHandler_RFRN::process_message(uint8_t *msg) +void LR_MsgHandler_RFRN::process_message(uint8_t *msgbytes) { - AP::dal().handle_message(MSG_CAST(RFRN,msg)); + MSG_CREATE(RFRN, msgbytes); + AP::dal().handle_message(msg); } -void LR_MsgHandler_REV2::process_message(uint8_t *msg) +void LR_MsgHandler_REV2::process_message(uint8_t *msgbytes) { - const log_REV2 &rev2 = MSG_CAST(REV2,msg); + MSG_CREATE(REV2, msgbytes); - switch ((AP_DAL::Event2)rev2.event) { + switch ((AP_DAL::Event2)msg.event) { case AP_DAL::Event2::resetGyroBias: ekf2.resetGyroBias(); @@ -78,28 +80,28 @@ void LR_MsgHandler_REV2::process_message(uint8_t *msg) } } -void LR_MsgHandler_RSO2::process_message(uint8_t *msg) +void LR_MsgHandler_RSO2::process_message(uint8_t *msgbytes) { - const log_RSO2 &rso2 = MSG_CAST(RSO2,msg); + MSG_CREATE(RSO2, msgbytes); Location loc; - loc.lat = rso2.lat; - loc.lng = rso2.lng; - loc.alt = rso2.alt; + loc.lat = msg.lat; + loc.lng = msg.lng; + loc.alt = msg.alt; ekf2.setOriginLLH(loc); } -void LR_MsgHandler_RWA2::process_message(uint8_t *msg) +void LR_MsgHandler_RWA2::process_message(uint8_t *msgbytes) { - const log_RWA2 &rwa2 = MSG_CAST(RWA2,msg); - ekf2.writeDefaultAirSpeed(rwa2.airspeed); + MSG_CREATE(RWA2, msgbytes); + ekf2.writeDefaultAirSpeed(msg.airspeed); } -void LR_MsgHandler_REV3::process_message(uint8_t *msg) +void LR_MsgHandler_REV3::process_message(uint8_t *msgbytes) { - const log_REV3 &rev3 = MSG_CAST(REV3,msg); + MSG_CREATE(REV3, msgbytes); - switch ((AP_DAL::Event3)rev3.event) { + switch ((AP_DAL::Event3)msg.event) { case AP_DAL::Event3::resetGyroBias: ekf3.resetGyroBias(); @@ -143,123 +145,150 @@ void LR_MsgHandler_REV3::process_message(uint8_t *msg) } } -void LR_MsgHandler_RSO3::process_message(uint8_t *msg) +void LR_MsgHandler_RSO3::process_message(uint8_t *msgbytes) { - const log_RSO3 &rso3 = MSG_CAST(RSO3,msg); + MSG_CREATE(RSO3, msgbytes); Location loc; - loc.lat = rso3.lat; - loc.lng = rso3.lng; - loc.alt = rso3.alt; + loc.lat = msg.lat; + loc.lng = msg.lng; + loc.alt = msg.alt; ekf3.setOriginLLH(loc); } -void LR_MsgHandler_RWA3::process_message(uint8_t *msg) +void LR_MsgHandler_RWA3::process_message(uint8_t *msgbytes) { - const log_RWA3 &rwa3 = MSG_CAST(RWA3,msg); - ekf3.writeDefaultAirSpeed(rwa3.airspeed); + MSG_CREATE(RWA3, msgbytes); + ekf3.writeDefaultAirSpeed(msg.airspeed); } -void LR_MsgHandler_REY3::process_message(uint8_t *msg) +void LR_MsgHandler_REY3::process_message(uint8_t *msgbytes) { - const log_REY3 &rey3 = MSG_CAST(REY3,msg); - ekf3.writeEulerYawAngle(rey3.yawangle, rey3.yawangleerr, rey3.timestamp_ms, rey3.type); + MSG_CREATE(REY3, msgbytes); + ekf3.writeEulerYawAngle(msg.yawangle, msg.yawangleerr, msg.timestamp_ms, msg.type); } -void LR_MsgHandler_RISH::process_message(uint8_t *msg) +void LR_MsgHandler_RISH::process_message(uint8_t *msgbytes) { - AP::dal().handle_message(MSG_CAST(RISH,msg)); + MSG_CREATE(RISH, msgbytes); + AP::dal().handle_message(msg); } -void LR_MsgHandler_RISI::process_message(uint8_t *msg) +void LR_MsgHandler_RISI::process_message(uint8_t *msgbytes) { - AP::dal().handle_message(MSG_CAST(RISI,msg)); + MSG_CREATE(RISI, msgbytes); + AP::dal().handle_message(msg); } -void LR_MsgHandler_RASH::process_message(uint8_t *msg) +void LR_MsgHandler_RASH::process_message(uint8_t *msgbytes) { - AP::dal().handle_message(MSG_CAST(RASH,msg)); + MSG_CREATE(RASH, msgbytes); + AP::dal().handle_message(msg); } -void LR_MsgHandler_RASI::process_message(uint8_t *msg) +void LR_MsgHandler_RASI::process_message(uint8_t *msgbytes) { - AP::dal().handle_message(MSG_CAST(RASI,msg)); + MSG_CREATE(RASI, msgbytes); + AP::dal().handle_message(msg); } -void LR_MsgHandler_RBRH::process_message(uint8_t *msg) +void LR_MsgHandler_RBRH::process_message(uint8_t *msgbytes) { - 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)); + MSG_CREATE(RBRH, msgbytes); + AP::dal().handle_message(msg); } -void LR_MsgHandler_RRNH::process_message(uint8_t *msg) +void LR_MsgHandler_RBRI::process_message(uint8_t *msgbytes) { - 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)); + MSG_CREATE(RBRI, msgbytes); + AP::dal().handle_message(msg); } -void LR_MsgHandler_RGPH::process_message(uint8_t *msg) +void LR_MsgHandler_RRNH::process_message(uint8_t *msgbytes) { - 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)); + MSG_CREATE(RRNH, msgbytes); + AP::dal().handle_message(msg); } -void LR_MsgHandler_RMGH::process_message(uint8_t *msg) +void LR_MsgHandler_RRNI::process_message(uint8_t *msgbytes) { - 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)); + MSG_CREATE(RRNI, msgbytes); + AP::dal().handle_message(msg); } -void LR_MsgHandler_RBCH::process_message(uint8_t *msg) +void LR_MsgHandler_RGPH::process_message(uint8_t *msgbytes) { - 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)); + MSG_CREATE(RGPH, msgbytes); + AP::dal().handle_message(msg); } -void LR_MsgHandler_RVOH::process_message(uint8_t *msg) +void LR_MsgHandler_RGPI::process_message(uint8_t *msgbytes) { - AP::dal().handle_message(MSG_CAST(RVOH,msg)); + MSG_CREATE(RGPI, msgbytes); + AP::dal().handle_message(msg); } -void LR_MsgHandler_ROFH::process_message(uint8_t *msg) +void LR_MsgHandler_RGPJ::process_message(uint8_t *msgbytes) { - AP::dal().handle_message(MSG_CAST(ROFH,msg), ekf2, ekf3); + MSG_CREATE(RGPJ, msgbytes); + AP::dal().handle_message(msg); } -void LR_MsgHandler_RWOH::process_message(uint8_t *msg) +void LR_MsgHandler_RMGH::process_message(uint8_t *msgbytes) { - AP::dal().handle_message(MSG_CAST(RWOH,msg), ekf2, ekf3); + MSG_CREATE(RMGH, msgbytes); + AP::dal().handle_message(msg); } -void LR_MsgHandler_RBOH::process_message(uint8_t *msg) +void LR_MsgHandler_RMGI::process_message(uint8_t *msgbytes) { - AP::dal().handle_message(MSG_CAST(RBOH,msg), ekf2, ekf3); + MSG_CREATE(RMGI, msgbytes); + AP::dal().handle_message(msg); } -void LR_MsgHandler_REPH::process_message(uint8_t *msg) +void LR_MsgHandler_RBCH::process_message(uint8_t *msgbytes) { - AP::dal().handle_message(MSG_CAST(REPH,msg), ekf2, ekf3); + MSG_CREATE(RBCH, msgbytes); + AP::dal().handle_message(msg); } -void LR_MsgHandler_REVH::process_message(uint8_t *msg) +void LR_MsgHandler_RBCI::process_message(uint8_t *msgbytes) { - AP::dal().handle_message(MSG_CAST(REVH,msg), ekf2, ekf3); + MSG_CREATE(RBCI, msgbytes); + AP::dal().handle_message(msg); +} + +void LR_MsgHandler_RVOH::process_message(uint8_t *msgbytes) +{ + MSG_CREATE(RVOH, msgbytes); + AP::dal().handle_message(msg); +} + +void LR_MsgHandler_ROFH::process_message(uint8_t *msgbytes) +{ + MSG_CREATE(ROFH, msgbytes); + AP::dal().handle_message(msg, ekf2, ekf3); +} + +void LR_MsgHandler_RWOH::process_message(uint8_t *msgbytes) +{ + MSG_CREATE(RWOH, msgbytes); + AP::dal().handle_message(msg, ekf2, ekf3); +} + +void LR_MsgHandler_RBOH::process_message(uint8_t *msgbytes) +{ + MSG_CREATE(RBOH, msgbytes); + AP::dal().handle_message(msg, ekf2, ekf3); +} + +void LR_MsgHandler_REPH::process_message(uint8_t *msgbytes) +{ + MSG_CREATE(REPH, msgbytes); + AP::dal().handle_message(msg, ekf2, ekf3); +} + +void LR_MsgHandler_REVH::process_message(uint8_t *msgbytes) +{ + MSG_CREATE(REVH, msgbytes); + AP::dal().handle_message(msg, ekf2, ekf3); } #include @@ -278,24 +307,13 @@ bool LR_MsgHandler_PARM::set_parameter(const char *name, const float value) } } - return _set_parameter_callback(name, value); + return LogReader::set_parameter(name, value); } void LR_MsgHandler_PARM::process_message(uint8_t *msg) { const uint8_t parameter_name_len = AP_MAX_NAME_SIZE + 1; // null-term char parameter_name[parameter_name_len]; - uint64_t time_us; - - if (field_value(msg, "TimeUS", time_us)) { - } else { - // older logs can have a lot of FMT and PARM messages up the - // front which don't have timestamps. Since in Replay we run - // 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(((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 81dc34be90..9c9fdde695 100644 --- a/Tools/Replay/LR_MsgHandler.h +++ b/Tools/Replay/LR_MsgHandler.h @@ -6,8 +6,6 @@ #include #include -#include - class LR_MsgHandler : public MsgHandler { public: LR_MsgHandler(struct log_Format &f); @@ -262,15 +260,12 @@ public: class LR_MsgHandler_PARM : public LR_MsgHandler { public: - LR_MsgHandler_PARM(log_Format &_f, - const std::function&set_parameter_callback) : - LR_MsgHandler(_f), - _set_parameter_callback(set_parameter_callback) + LR_MsgHandler_PARM(log_Format &_f) : + LR_MsgHandler(_f) {}; void process_message(uint8_t *msg) override; private: bool set_parameter(const char *name, const float value); - const std::function_set_parameter_callback; }; diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index a791a368c8..881aab1b4a 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -73,7 +73,6 @@ 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", n); bool already_mapped = false; for (uint16_t i=0; i 1.0e-12) { - ::fprintf(stderr, "Changed %s to %.8f from %.8f\n", name, value, old_value); + ::printf("Changed %s to %.8f from %.8f\n", name, value, old_value); } return true; } + diff --git a/Tools/Replay/LogReader.h b/Tools/Replay/LogReader.h index 2aff2fdbe7..97bf0e749f 100644 --- a/Tools/Replay/LogReader.h +++ b/Tools/Replay/LogReader.h @@ -12,8 +12,8 @@ public: VehicleType::vehicle_type vehicle; - bool check_user_param(const char *name); - bool set_parameter(const char *name, float value, bool force=false); + static bool check_user_param(const char *name); + static bool set_parameter(const char *name, float value, bool force=false); bool handle_log_format_msg(const struct log_Format &f) override; bool handle_msg(const struct log_Format &f, uint8_t *msg) override; diff --git a/Tools/Replay/MsgHandler.cpp b/Tools/Replay/MsgHandler.cpp index 6f0f484d1d..79ae0f19c6 100644 --- a/Tools/Replay/MsgHandler.cpp +++ b/Tools/Replay/MsgHandler.cpp @@ -1,31 +1,5 @@ #include "MsgHandler.h" -void fatal(const char *msg) { - ::printf("%s",msg); - ::printf("\n"); - exit(1); -} - -char *xstrdup(const char *string) -{ - char *ret = strdup(string); - if (ret == NULL) { - perror("strdup"); - fatal("strdup failed"); - } - 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; @@ -35,7 +9,7 @@ uint8_t MsgHandler::size_for_type(char type) { uint8_t ret = size_for_type_table[(uint8_t)(type > 'A' ? (type-'A') : (type-'a'))]; if (ret == 0) { - ::fprintf(stderr, "Unknown type (%c)\n", type); + ::printf("Unknown type (%c)\n", type); abort(); } return ret; @@ -83,16 +57,16 @@ MsgHandler::MsgHandler(const struct log_Format &_f) : next_field(0), f(_f) void MsgHandler::add_field(const char *_label, uint8_t _type, uint8_t _offset, uint8_t _length) { - field_info[next_field].label = xstrdup(_label); + field_info[next_field].label = strdup(_label); field_info[next_field].type = _type; field_info[next_field].offset = _offset; field_info[next_field].length = _length; next_field++; } -char *get_string_field(char *field, uint8_t fieldlen) +static char *get_string_field(char *field, uint8_t fieldlen) { - char *ret = xcalloc(1, fieldlen+1); + char *ret = (char *)malloc(fieldlen+1); memcpy(ret, field, fieldlen); return ret; } diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index 9ab84da1ed..72b27fa6ab 100644 --- a/Tools/Replay/Replay.cpp +++ b/Tools/Replay/Replay.cpp @@ -23,8 +23,12 @@ #include #include +#include +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX #include +#endif #define streq(x, y) (!strcmp(x, y)) @@ -194,12 +198,17 @@ void Replay::setup() set_user_parameters(); if (filename == nullptr) { +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + // allow replay on stm32 + filename = "APM/replayin.bin"; +#else ::printf("You must supply a log filename\n"); exit(1); +#endif } // LogReader reader = LogReader(log_structure); if (!reader.open_log(filename)) { - ::fprintf(stderr, "open(%s): %m\n", filename); + ::printf("open(%s): %m\n", filename); exit(1); } } diff --git a/Tools/Replay/wscript b/Tools/Replay/wscript index 648fc06766..f6636d0cd6 100644 --- a/Tools/Replay/wscript +++ b/Tools/Replay/wscript @@ -4,9 +4,6 @@ import boards def build(bld): - if isinstance(bld.get_board(), boards.chibios): - return - vehicle = bld.path.name bld.ap_stlib(