From 4b8198fd14744ffa9931519b3f9bd0baa80972cc Mon Sep 17 00:00:00 2001 From: gguidone Date: Wed, 15 Jul 2026 14:49:55 +0200 Subject: [PATCH] feat(motor_failure_detector): add offline ulog replay tool Add a host tool that replays a recorded .ulg through the real MotorFailureDetector and prints the per-motor trip verdict, for validating and tuning the detector offline (EKF2-replay style: the real library drives recorded data, not a re-implementation). Built on ulog_cpp (PX4's C++ ULog library, added as a git submodule and built for the posix host target only), so it reads ESC current, motor command and arming state by field name from the log's own format -- one binary replays logs of any PX4 message version with no rebuild. The feed mirrors the runtime in escCheck: evaluated only while armed, at the runtime's 10 Hz cadence, with the per-ESC current-report startup latch and reset-on-disarm. Config comes from the log's MOTFAIL_* parameters with a built-in calibration fallback, and --set MOTFAIL_NAME=value overrides any knob for what-if retuning. Because the runtime's exact 10 Hz sample phase cannot be reconstructed offline, a single pass over the log drives several detector instances on staggered grid offsets and reports a per-motor peak band and trip fraction rather than a single phase's verdict, flagging when the result is phase-dependent. batch_replay.sh runs the tool over a folder of logs and summarizes each verdict (OK / FAIL / SKIP) from its exit code. Off by default so it never enters the firmware build or CI: it is a posix-only, EXCLUDE_FROM_ALL host executable gated behind the MFD_REPLAY_TOOL cache option. Build it with: cmake -DMFD_REPLAY_TOOL=ON build/px4_sitl_default ninja -C build/px4_sitl_default mfd_replay --- .gitmodules | 3 + src/lib/motor_failure_detector/CMakeLists.txt | 18 + .../tools/batch_replay.sh | 31 ++ .../tools/mfd_replay.cpp | 523 ++++++++++++++++++ src/lib/ulog_cpp | 1 + 5 files changed, 576 insertions(+) create mode 100755 src/lib/motor_failure_detector/tools/batch_replay.sh create mode 100644 src/lib/motor_failure_detector/tools/mfd_replay.cpp create mode 160000 src/lib/ulog_cpp diff --git a/.gitmodules b/.gitmodules index e93885fd04c..14b46d94fb9 100644 --- a/.gitmodules +++ b/.gitmodules @@ -122,3 +122,6 @@ [submodule "src/modules/simulation/gz_plugins/optical_flow/PX4-OpticalFlow"] path = src/modules/simulation/gz_plugins/optical_flow/PX4-OpticalFlow url = https://github.com/PX4/PX4-OpticalFlow.git +[submodule "src/lib/ulog_cpp"] + path = src/lib/ulog_cpp + url = https://github.com/PX4/ulog_cpp.git diff --git a/src/lib/motor_failure_detector/CMakeLists.txt b/src/lib/motor_failure_detector/CMakeLists.txt index e07dc6536b9..d4d5b755c45 100644 --- a/src/lib/motor_failure_detector/CMakeLists.txt +++ b/src/lib/motor_failure_detector/CMakeLists.txt @@ -38,3 +38,21 @@ px4_add_library(MotorFailureDetector target_include_directories(MotorFailureDetector PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) px4_add_unit_gtest(SRC MotorFailureDetectorTest.cpp LINKLIBS MotorFailureDetector mathlib hysteresis) + +# Offline log-replay tool: drives the real detector over a recorded ulog (host only). Opt-in (off by +# default) via -DMFD_REPLAY_TOOL=ON so a normal build never fetches/builds it or its ulog_cpp submodule. +set(MFD_REPLAY_TOOL OFF CACHE BOOL "Build the host-only MotorFailureDetector offline log-replay tool") + +if(${PX4_PLATFORM} MATCHES "posix" AND MFD_REPLAY_TOOL) + px4_add_git_submodule(TARGET git_ulog_cpp PATH "${PX4_SOURCE_DIR}/src/lib/ulog_cpp") + add_subdirectory(${PX4_SOURCE_DIR}/src/lib/ulog_cpp ${CMAKE_CURRENT_BINARY_DIR}/ulog_cpp EXCLUDE_FROM_ALL) + + # ulog_cpp is third-party: silence the specific warnings it trips under PX4's -Werror set + # (-Wshadow in its code, -Warray-bounds a libstdc++ false positive) and mark its headers system. + target_compile_options(ulog_cpp PRIVATE -Wno-shadow -Wno-array-bounds) + get_target_property(_ulogcpp_inc ulog_cpp INTERFACE_INCLUDE_DIRECTORIES) + set_target_properties(ulog_cpp PROPERTIES INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${_ulogcpp_inc}") + + add_executable(mfd_replay EXCLUDE_FROM_ALL tools/mfd_replay.cpp) + target_link_libraries(mfd_replay PRIVATE MotorFailureDetector mathlib hysteresis ulog_cpp::ulog_cpp) +endif() diff --git a/src/lib/motor_failure_detector/tools/batch_replay.sh b/src/lib/motor_failure_detector/tools/batch_replay.sh new file mode 100755 index 00000000000..c44acfd78cd --- /dev/null +++ b/src/lib/motor_failure_detector/tools/batch_replay.sh @@ -0,0 +1,31 @@ +#!/usr/bin/env bash +# Run mfd_replay on every .ulg in a folder and summarize each verdict. +# +# Usage: batch_replay.sh [--set MOTFAIL_NAME=value ...] +# env MFD_REPLAY=/path/to/mfd_replay overrides the binary location +# +# For a very large fleet, parallelize instead: ls DIR/*.ulg | xargs -P"$(nproc)" -n1 mfd_replay +set -uo pipefail + +# default: the standard SITL build, relative to the repo root (run this from there) +BIN="${MFD_REPLAY:-build/px4_sitl_default/mfd_replay}" +dir="${1:?usage: batch_replay.sh [--set NAME=value ...]}" +shift # anything after the dir ("$@") is passed straight through to mfd_replay + +shopt -s nullglob # an empty folder yields no iterations, not a literal "*.ulg" +ok=0 fail=0 skip=0 + +for log in "$dir"/*.ulg; do + name=$(basename "$log") + output=$("$BIN" "$log" "$@" 2>/dev/null) + + case $? in # mfd_replay exit code: 0 = clean, 1 = a motor tripped, 2 = unreadable / no ESC data + 0) echo "OK $name" ; ok=$((ok + 1)) ;; + 1) motors=$(echo "$output" | awk '/FAILED/ {printf "m%s ", $1}') + echo "FAIL $name [${motors% }]" ; fail=$((fail + 1)) ;; + *) echo "SKIP $name (no ESC data / unreadable)" ; skip=$((skip + 1)) ;; + esac +done + +echo "----" +echo "total=$((ok + fail + skip)) OK=$ok FAIL=$fail SKIP=$skip" diff --git a/src/lib/motor_failure_detector/tools/mfd_replay.cpp b/src/lib/motor_failure_detector/tools/mfd_replay.cpp new file mode 100644 index 00000000000..b8d8d0f5a5f --- /dev/null +++ b/src/lib/motor_failure_detector/tools/mfd_replay.cpp @@ -0,0 +1,523 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mfd_replay.cpp + * + * Replays the real MotorFailureDetector over a recorded .ulg and prints a per-motor trip verdict. + * Built on ulog_cpp, so it reads ESC current / motor command / arming state by field name from the + * log's own format -- a single binary replays logs of any PX4 message version with no rebuild. + * Mirrors escCheck.cpp: detector runs only when armed; config from the log's MOTFAIL_* params + * (per-field fallback to a built-in calibration), with --set NAME=value overrides for what-if retuning. + * + * The runtime's exact 10 Hz sample phase can't be reconstructed offline, so in a single pass over the + * log it runs kNumPhases detector instances on different grid offsets and reports a per-motor peak + * BAND + trip fraction -- a single-phase verdict is only a lower bound. + */ + +#include "MotorFailureDetector.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace +{ +constexpr int kMM = MotorFailureDetector::kMaxMotors; +constexpr int kMotor1 = 101; // esc_report::ACTUATOR_FUNCTION_MOTOR1 +constexpr int kArmed = 2; // vehicle_status::ARMING_STATE_ARMED +constexpr uint64_t kRuntimeTickUs = 100000; // runtime runs the check at 10 Hz (Commander.cpp) -- decimate to match +constexpr int kNumPhases = 5; // phase-sweep: run N detectors on staggered grid offsets to expose sampling-phase sensitivity + +// Default model fit; the FALLBACK used when the log carries no MOTFAIL_* params. +// Threshold band calibrated at the runtime's 10 Hz cadence (kMaxGap = 0.3 s, phase-swept): +// clean across the reference multirotor corpus. Illustrative for this airframe, not a shipped default. +MotorFailureDetector::Config makeConfig() +{ + MotorFailureDetector::Config cfg{}; + cfg.model_b = 34.8f; + cfg.model_c = 2.88f; + cfg.residual_lpf_tau_s = 0.2f; + cfg.threshold_a = 5.0f; + cfg.persistence_s = 0.4f; + return cfg; +} + +// MOTFAIL_* params -> Config fields, mirroring escCheck.cpp. tau is the compile-time constant there too. +struct ParamBind { + const char *name; + float MotorFailureDetector::Config::*field; +}; + +constexpr std::array kParamBinds { + ParamBind{"MOTFAIL_C2T", &MotorFailureDetector::Config::model_b}, + ParamBind{"MOTFAIL_IDLE", &MotorFailureDetector::Config::model_c}, + ParamBind{"MOTFAIL_OFF", &MotorFailureDetector::Config::threshold_a}, + ParamBind{"MOTFAIL_TIME", &MotorFailureDetector::Config::persistence_s}, +}; +constexpr int kNumParams = (int)kParamBinds.size(); + +// Set the Config field bound to param `name` (length `name_len`) to `value`. Returns its index, or -1. +int setParam(MotorFailureDetector::Config &cfg, const char *name, int name_len, float value) +{ + for (int i = 0; i < kNumParams; ++i) { + if ((int)std::strlen(kParamBinds[i].name) == name_len && std::strncmp(kParamBinds[i].name, name, name_len) == 0) { + cfg.*(kParamBinds[i].field) = value; + return i; + } + } + + return -1; +} +} + +class Replay : public ulog_cpp::DataContainer +{ +public: + // CLI overrides (from --set) are captured here and applied on top of the log's params in + // headerComplete(), once the log's own MOTFAIL_* values are known. + Replay(const MotorFailureDetector::Config &cli_cfg, const std::array &cli_set) + : ulog_cpp::DataContainer(ulog_cpp::DataContainer::StorageConfig::Header), + _cli_cfg(cli_cfg), _cli_set(cli_set) + { + for (auto &phase : _trip_time) { phase.fill(-1.0); } + } + + void addLoggedMessage(const ulog_cpp::AddLoggedMessage &msg) override + { + ulog_cpp::DataContainer::addLoggedMessage(msg); + + if (msg.multiId() != 0) { return; } + + auto it = messageFormats().find(msg.messageName()); + + if (it == messageFormats().end()) { return; } + + if (msg.messageName() == "esc_status") { + _esc_id = msg.msgId(); + _esc_fmt = it->second; + _esc_array_f = _esc_fmt->field("esc"); + _esc_slots = _esc_array_f->arrayLength(); + _esc_timestamp_f = _esc_fmt->field("timestamp"); + _esc_count_f = _esc_fmt->field("esc_count"); + _esc_function_f = _esc_array_f->nestedField("actuator_function"); + _esc_current_f = _esc_array_f->nestedField("esc_current"); + _esc_current_seen.assign(_esc_slots, 0); + + } else if (msg.messageName() == "actuator_motors") { + _actuator_id = msg.msgId(); + _actuator_fmt = it->second; + _actuator_control_f = _actuator_fmt->field("control"); + _control_channels = _actuator_control_f->arrayLength(); + _actuator_reversible_f = _actuator_fmt->field("reversible_flags"); + + } else if (msg.messageName() == "vehicle_status") { + _status_id = msg.msgId(); + _status_fmt = it->second; + _status_arming_f = _status_fmt->field("arming_state"); + _have_status = true; + } + } + + void parameter(const ulog_cpp::Parameter &p) override + { + ulog_cpp::DataContainer::parameter(p); + + if (_configured || p.field().type().type != ulog_cpp::Field::BasicType::FLOAT) { return; } + + const std::string &name = p.field().name(); + const int idx = setParam(_cfg, name.c_str(), (int)name.size(), p.value().as()); + + if (idx >= 0 && !_log_set[idx]) { + _log_set[idx] = true; + ++_params_from_log; + } + } + + void headerComplete() override + { + ulog_cpp::DataContainer::headerComplete(); + + // The header is fully parsed: the log's MOTFAIL_* params and message layouts are known. + // Apply the CLI overrides on top and configure every phase's detector once, before any data. + for (int i = 0; i < kNumParams; ++i) { + if (_cli_set[i]) { _cfg.*(kParamBinds[i].field) = _cli_cfg.*(kParamBinds[i].field); } + } + + for (auto &d : _det) { d.configure(_cfg); } + + _configured = true; + } + + void data(const ulog_cpp::Data &record) override + { + const uint16_t id = record.msgId(); + + if (id == _status_id) { + const bool now_armed = (ulog_cpp::TypedDataView(record, *_status_fmt)[_status_arming_f].as() == kArmed); + + if (_armed && !now_armed) { // disarm resets the detector (mirrors configure-on-disarm) + for (int p = 0; p < kNumPhases; ++p) { + _det[p].configure(_cfg); + _next_eval_us[p] = 0; + } + } + + _armed = now_armed; + return; + } + + if (id == _actuator_id) { + ulog_cpp::TypedDataView view(record, *_actuator_fmt); + const uint32_t reversible_mask = view[_actuator_reversible_f].as(); + const auto control = view[_actuator_control_f]; + + for (int m = 0; m < kMM && m < _control_channels; ++m) { + _command_latest[m] = control[m].as(); + _reversible_latest[m] = (reversible_mask >> m) & 1; + } + + return; + } + + if (id != _esc_id) { return; } + + ulog_cpp::TypedDataView view(record, *_esc_fmt); + + if (view[_esc_count_f].as() <= 0) { return; } // runtime skips esc_status with esc_count <= 0 + + if (_have_status && !_armed) { return; } // armed gate, like escCheck.cpp + + const uint64_t sample_us = view[_esc_timestamp_f].as(); + + // Shape the per-motor inputs once -- they don't depend on the sampling phase, only on the + // record. Which phase grids consume this sample is decided in the phase loop below. + std::array command; + std::array current {}; + std::array reversible {}; + command.fill(NAN); + + const auto esc_arr = view[_esc_array_f]; + + for (int i = 0; i < _esc_slots; ++i) { + const auto esc_i = esc_arr[i]; + const int function = esc_i[_esc_function_f].as(); + + if (function < kMotor1 || function - kMotor1 >= kMM) { continue; } + + const float slot_current = esc_i[_esc_current_f].as(); + + if (slot_current > FLT_EPSILON) { _esc_current_seen[i] = 1; } // latch first current report (per ESC slot) + + if (!_esc_current_seen[i]) { continue; } // don't judge until the ESC reports current + + const int m = function - kMotor1; + _seen[m] = true; + current[m] = slot_current; + command[m] = _command_latest[m]; // 0 until an actuator arrives (runtime zero-init) + reversible[m] = _reversible_latest[m]; + } + + // One parse, kNumPhases detectors: feed each phase whose 10 Hz grid this sample crosses. Each + // phase's grid is offset by p/kNumPhases of a tick, so a single pass over the log covers the + // sampling-phase spread the runtime's unknown tick alignment could land on (no file re-read). + for (int p = 0; p < kNumPhases; ++p) { + if (_next_eval_us[p] == 0) { // first tick: anchor this phase's grid at its offset + _next_eval_us[p] = sample_us + (uint64_t)p * kRuntimeTickUs / kNumPhases; + } + + if (sample_us < _next_eval_us[p]) { continue; } + + _next_eval_us[p] += kRuntimeTickUs; + + if (_start_us < 0) { _start_us = (double)sample_us; } + + const double elapsed_s = ((double)sample_us - _start_us) / 1e6; + + _det[p].update(kMM, (hrt_abstime)sample_us, command.data(), current.data(), reversible.data()); + + for (int m = 0; m < kMM; ++m) { + if (!_seen[m]) { continue; } + + const float abs_residual_lpf = std::fabs(_det[p].status(m).residual_lpf); + + if (!std::isnan(abs_residual_lpf) && abs_residual_lpf > _peak[p][m]) { _peak[p][m] = abs_residual_lpf; } + + if (_det[p].status(m).failed && _trip_time[p][m] < 0) { _trip_time[p][m] = elapsed_s; } + } + + if (p == 0) { ++_evaluated; } + } + } + + // Results, read after the whole log has been parsed. + bool seen(int m) const { return _seen[m]; } + float peak(int phase, int m) const { return _peak[phase][m]; } + double tripTime(int phase, int m) const { return _trip_time[phase][m]; } + bool haveStatus() const { return _have_status; } + int paramsFromLog() const { return _params_from_log; } + long evaluated() const { return _evaluated; } + const MotorFailureDetector::Config &config() const { return _cfg; } + +private: + MotorFailureDetector::Config _cli_cfg {}; + std::array _cli_set {}; + + // One detector instance per sampling phase; a single parse feeds all of them. + std::array _det; + MotorFailureDetector::Config _cfg = makeConfig(); + std::array _log_set {}; + int _params_from_log = 0; + bool _configured = false; + + bool _armed = false; + bool _have_status = false; + std::array _command_latest {}; + std::array _reversible_latest {}; + std::array, kNumPhases> _peak {}; + std::array, kNumPhases> _trip_time; // -1 until a trip (filled in the ctor) + std::array _seen {}; + double _start_us = -1.0; + long _evaluated = 0; // phase-0 evaluation count (== per-phase sample count) + + // Cached ULog "legend" (filled in addLoggedMessage): each message's id + layout, and handles to the + // fields we read -- resolved once so the per-record loop reads by handle, not by name. + uint16_t _esc_id = 0xffff; // 0xffff = message type not seen yet + uint16_t _actuator_id = 0xffff; + uint16_t _status_id = 0xffff; + std::shared_ptr _esc_fmt; + std::shared_ptr _actuator_fmt; + std::shared_ptr _status_fmt; + int _esc_slots = 0; // esc[] slots from the layout + int _control_channels = 0; // control[] channels from the layout + + std::vector _esc_current_seen; // per-ESC-slot "has reported current" latch (mirrors escCheck), never reset + std::array _next_eval_us {}; // per-phase next 10 Hz evaluation time (runtime-cadence match) + + // esc_status field handles (suffix _f) + std::shared_ptr _esc_timestamp_f; + std::shared_ptr _esc_array_f; + std::shared_ptr _esc_function_f; + std::shared_ptr _esc_current_f; + std::shared_ptr _esc_count_f; + // actuator_motors field handles + std::shared_ptr _actuator_control_f; + std::shared_ptr _actuator_reversible_f; + // vehicle_status field handles + std::shared_ptr _status_arming_f; +}; + +// Parse the whole log once through a fresh Replay (which runs all kNumPhases detectors internally). +// Returns the populated Replay, or nullptr on a fatal error (already reported to stderr). +std::shared_ptr parseLog(const char *ulog_path, const MotorFailureDetector::Config &cli_cfg, + const std::array &cli_set) +{ + auto replay = std::make_shared(cli_cfg, cli_set); + + FILE *file = fopen(ulog_path, "rb"); + + if (!file) { std::fprintf(stderr, "cannot open %s\n", ulog_path); return nullptr; } + + std::array buf; + int bytes_read; + ulog_cpp::Reader reader{replay}; + + while ((bytes_read = fread(buf.data(), 1, buf.size(), file)) > 0) { + try { + reader.readChunk(buf.data(), bytes_read); + + } catch (const ulog_cpp::ExceptionBase &e) { + std::fprintf(stderr, "%s: %s\n (a needed field is missing -- the log's message layout differs from what this tool expects)\n", + ulog_path, e.what()); + fclose(file); + return nullptr; + } + + if (replay->hadFatalError()) { break; } + } + + fclose(file); + + if (replay->hadFatalError()) { std::fprintf(stderr, "fatal parse error in %s\n", ulog_path); return nullptr; } + + return replay; +} + +int main(int argc, char **argv) +{ + const char *usage = "usage: %s [--set MOTFAIL_NAME=value ...]\n"; + const char *ulog_path = nullptr; + MotorFailureDetector::Config cli_cfg{}; + std::array cli_set{}; + int cli_overrides = 0; + + for (int i = 1; i < argc; ++i) { + if (!std::strcmp(argv[i], "--set") && i + 1 < argc) { + const char *kv = argv[++i]; + const char *eq = std::strchr(kv, '='); + char *end = nullptr; + const float val = eq ? std::strtof(eq + 1, &end) : 0.f; + const int idx = (eq && end != eq + 1) ? setParam(cli_cfg, kv, (int)(eq - kv), val) : -1; + + if (idx < 0) { + std::fprintf(stderr, "bad --set '%s' (expected MOTFAIL_NAME=value)\n", kv); + return 2; + } + + if (!cli_set[idx]) { ++cli_overrides; } + + cli_set[idx] = true; + + } else if (!ulog_path && argv[i][0] != '-') { + ulog_path = argv[i]; + + } else { + std::fprintf(stderr, usage, argv[0]); + return 2; + } + } + + if (!ulog_path) { std::fprintf(stderr, usage, argv[0]); return 2; } + + auto replay = parseLog(ulog_path, cli_cfg, cli_set); + + if (!replay) { return 2; } + + // Aggregate the per-phase results into a peak band + trip fraction per motor. + std::array seen_any{}; + std::array peak_min; + std::array peak_max; + std::array trip_count{}; + + peak_min.fill(FLT_MAX); + peak_max.fill(0.f); + + for (int m = 0; m < kMM; ++m) { + if (!replay->seen(m)) { continue; } + + seen_any[m] = true; + + for (int p = 0; p < kNumPhases; ++p) { + peak_min[m] = std::min(peak_min[m], replay->peak(p, m)); + peak_max[m] = std::max(peak_max[m], replay->peak(p, m)); + + if (replay->tripTime(p, m) >= 0) { ++trip_count[m]; } + } + } + + for (const std::string &err : replay->parsingErrors()) { std::fprintf(stderr, "parse: %s\n", err.c_str()); } + + bool any_motor = false; + + for (int m = 0; m < kMM; ++m) { any_motor |= seen_any[m]; } + + if (!any_motor) { + std::fprintf(stderr, "no armed esc_status samples in %s\n", ulog_path); + return 2; + } + + if (!replay->haveStatus()) { + std::fprintf(stderr, "warning: no vehicle_status in %s -- arming gate disabled, evaluating all esc samples\n", ulog_path); + } + + std::array config_source {}; + int n = replay->paramsFromLog() > 0 + ? std::snprintf(config_source.data(), config_source.size(), "from log") + : std::snprintf(config_source.data(), config_source.size(), "built-in calibration"); + n = std::min(n, (int)config_source.size() - 1); + + if (cli_overrides > 0) { + n += std::snprintf(config_source.data() + n, config_source.size() - n, ", override%s:", cli_overrides > 1 ? "s" : ""); + + for (int i = 0; i < kNumParams && n < (int)config_source.size() - 1; ++i) { + if (cli_set[i]) { n += std::snprintf(config_source.data() + n, config_source.size() - n, " %s", kParamBinds[i].name); } + } + } + + const MotorFailureDetector::Config &cfg = replay->config(); + const char *count_label = replay->haveStatus() ? "armed samples each" : "samples each, arming gate OFF"; + std::printf("# %s (%d phases, %ld %s)\n" + "# config (%s): I_exp = %.2f*u + %.2f A, trip if |LPF(I-I_exp)| > %.2f for %.2f s\n", + ulog_path, kNumPhases, replay->evaluated(), count_label, config_source.data(), + (double)cfg.model_b, (double)cfg.model_c, + (double)cfg.threshold_a, (double)cfg.persistence_s); + std::printf("# %-5s %-15s %-5s %s\n", "motor", "peak|LPF(r)|[A]", "trips", "verdict"); + + bool any_failed = false; + bool phase_dependent = false; + + for (int m = 0; m < kMM; ++m) { + if (!seen_any[m]) { continue; } + + const char *verdict; + + if (trip_count[m] == kNumPhases) { + verdict = "FAILED (all phases)"; + any_failed = true; + + } else if (trip_count[m] > 0) { + verdict = "FAILED (sampling-dependent!)"; + any_failed = true; + phase_dependent = true; + + } else { + verdict = "ok"; + } + + if (peak_max[m] > 1.3f * peak_min[m]) { phase_dependent = true; } // wide band => phase-sensitive + + std::array trips_str {}; + std::snprintf(trips_str.data(), trips_str.size(), "%d/%d", trip_count[m], kNumPhases); + std::printf(" %5d %6.2f - %-6.2f %-5s %s\n", + m, (double)peak_min[m], (double)peak_max[m], trips_str.data(), verdict); + } + + if (phase_dependent) { + std::printf("# NOTE: the verdict depends on which samples are used -- treat any trip as a\n" + "# possible failure on the vehicle, not a pass.\n"); + } + + return any_failed ? 1 : 0; +} diff --git a/src/lib/ulog_cpp b/src/lib/ulog_cpp new file mode 160000 index 00000000000..af366a91cb9 --- /dev/null +++ b/src/lib/ulog_cpp @@ -0,0 +1 @@ +Subproject commit af366a91cb90a454f8840cc019ae82571cc4cef6