From 6de07ecf7789f97abba0aad71ad87877e9ac2219 Mon Sep 17 00:00:00 2001 From: gguidone Date: Mon, 29 Jun 2026 15:16:51 +0200 Subject: [PATCH] feat(motor_failure_detector): add current-residual detection library Per-motor model-based motor-failure detector for multirotors. Predicts the expected ESC current from the commanded actuator signal (I_exp = a*u^2 + c) and trips on a low-pass-filtered residual held above a threshold for a persistence time. The command (not measured rpm) is the fault-independent reference: a dead motor that draws no current while still commanded produces a large residual, whereas a current-vs-rpm model would mask a clean stop. The trip test is two-sided (|LPF(residual)| >= threshold), so it covers both under-current faults (open circuit, shed propeller, desync) and over-current faults (winding short, mechanical jam, demagnetization). Faults are caught only outside a dead band around the healthy current; mid-range partial losses fall inside it. The threshold is throttle-relative: threshold_a + threshold_rel*I_expected. The empirical healthy residual floor is a roughly constant noise term plus a fraction of the expected current, so an affine band tracks it better than a flat one -- tighter at low/mid throttle, wider only at high throttle. threshold_rel = 0 recovers a flat threshold. Built on PX4 primitives rather than hand-rolled equivalents: update() takes a timestamp (esc_status.timestamp in flight, the log timestamp in replay) and derives dt internally; the residual filter is a mathlib AlphaFilter; the persistence debounce is a systemlib::Hysteresis (with a sticky latch on top, and reset across data gaps so a dropout can't complete a pending trip). The data-gap threshold (kMaxGap = 0.3 s) is sized above the ~10 Hz arming-check feed interval and below the 400 ms ESC-offline timeout: a live feed is never mistaken for a gap (a smaller value equal to the feed interval reset the filter and debounce on the majority of jittered ticks, so a sustained fault never survived the persistence window -- offline replay: 0/100 sampling phases latched vs 100/100 gap-free), while a genuine outage still resets and is handled by the separate ESC liveness guard. Operator thresholds must then be tuned against the accumulated healthy residual (roughly an affine ~1.92 + 0.131*I_expected band); shipped MOTFAIL_* defaults stay 0 (monitor-only), so default behavior is unchanged. Includes a 17-case gtest spanning the failure-mode spectrum and edge cases: healthy-never-trips (incl. high throttle), open-circuit and over-current trips, severe prop-loss under-current, the missed ~50% partial loss (documents the detection floor), throttle-relative band scaling, persistence (short-spike crosses-threshold-but-recovers), gap reset, the 10 Hz feed vs. real-gap boundary, dropout-holds, multi-motor, the exclusion gate, monitor-only, and reset/latch. --- src/lib/CMakeLists.txt | 1 + src/lib/motor_failure_detector/CMakeLists.txt | 40 +++ .../MotorFailureDetector.cpp | 135 ++++++++ .../MotorFailureDetector.hpp | 100 ++++++ .../MotorFailureDetectorTest.cpp | 295 ++++++++++++++++++ 5 files changed, 571 insertions(+) create mode 100644 src/lib/motor_failure_detector/CMakeLists.txt create mode 100644 src/lib/motor_failure_detector/MotorFailureDetector.cpp create mode 100644 src/lib/motor_failure_detector/MotorFailureDetector.hpp create mode 100644 src/lib/motor_failure_detector/MotorFailureDetectorTest.cpp diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 33d58ab640d..4fc30a3831b 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -59,6 +59,7 @@ add_subdirectory(matrix EXCLUDE_FROM_ALL) add_subdirectory(mathlib EXCLUDE_FROM_ALL) add_subdirectory(mixer_module EXCLUDE_FROM_ALL) add_subdirectory(motion_planning EXCLUDE_FROM_ALL) +add_subdirectory(motor_failure_detector EXCLUDE_FROM_ALL) add_subdirectory(npfg EXCLUDE_FROM_ALL) add_subdirectory(perf EXCLUDE_FROM_ALL) add_subdirectory(fw_performance_model EXCLUDE_FROM_ALL) diff --git a/src/lib/motor_failure_detector/CMakeLists.txt b/src/lib/motor_failure_detector/CMakeLists.txt new file mode 100644 index 00000000000..e07dc6536b9 --- /dev/null +++ b/src/lib/motor_failure_detector/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(MotorFailureDetector + MotorFailureDetector.cpp + MotorFailureDetector.hpp +) +target_include_directories(MotorFailureDetector PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC MotorFailureDetectorTest.cpp LINKLIBS MotorFailureDetector mathlib hysteresis) diff --git a/src/lib/motor_failure_detector/MotorFailureDetector.cpp b/src/lib/motor_failure_detector/MotorFailureDetector.cpp new file mode 100644 index 00000000000..8e80c939309 --- /dev/null +++ b/src/lib/motor_failure_detector/MotorFailureDetector.cpp @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "MotorFailureDetector.hpp" + +#include + +void MotorFailureDetector::reset() +{ + const hrt_abstime persistence_us = (hrt_abstime)(_cfg.persistence_s * 1e6f); + + for (int i = 0; i < kMaxMotors; ++i) { + _status[i] = MotorStatus{}; + _residual_lpf[i].reset(0.f); + _hyst[i] = systemlib::Hysteresis(); + _hyst[i].set_hysteresis_time_from(false, persistence_us); // false = rising-edge debounce time + } + + _last_us = 0; +} + +void MotorFailureDetector::update(int num_motors, hrt_abstime now_us, + const float command[], const float current[], + const bool reversible[]) +{ + // disable the current-residual check + if (_cfg.threshold_a <= 0.f) { + return; + } + + const float dt = static_cast(now_us - _last_us) * 1e-6f; + _last_us = now_us; + + // dt > kMaxGap (also the first call) is a gap: reset filters + debounce, skip (latch kept). + if (dt > kMaxGap) { + for (int i = 0; i < num_motors; ++i) { + _status[i].residual_lpf = 0.f; + _residual_lpf[i].reset(0.f); + _hyst[i].set_state_and_update(false, now_us); // cancel any pending trip across the gap + } + + return; + } + + for (int i = 0; i < num_motors; ++i) { + MotorStatus &s = _status[i]; + + const float u = command[i]; + + // Cannot evaluate -> exclude and reset (re-entry starts clean; latch kept). + const bool excluded = std::isnan(u) + || reversible[i]; + + s.excluded = excluded; + + if (excluded) { + s.residual_lpf = 0.f; + _residual_lpf[i].reset(0.f); + _hyst[i].set_state_and_update(false, now_us); // cancel pending; keep any latch + continue; + } + + // Command-only, so valid even on a dropout. + const float i_expected = _cfg.model_b * u + _cfg.model_c; + + if (std::isfinite(current[i])) { + s.residual = current[i] - i_expected; + _residual_lpf[i].setParameters(dt, _cfg.residual_lpf_tau_s); + _residual_lpf[i].update(s.residual); + s.residual_lpf = _residual_lpf[i].getState(); + } + + // else: dropout -- residual held; the debounce keeps running on it (fault + dropout still latches). + + // Over-threshold must hold persistence_s (Hysteresis); the decision is then latched (sticky). + const bool over = std::fabs(s.residual_lpf) >= _cfg.threshold_a; + _hyst[i].set_state_and_update(over, now_us); + + if (_hyst[i].get_state()) { + s.failed = true; + } + } +} + +bool MotorFailureDetector::anyFailed() const +{ + for (int i = 0; i < kMaxMotors; ++i) { + if (_status[i].failed) { + return true; + } + } + + return false; +} + +int MotorFailureDetector::firstFailed() const +{ + for (int i = 0; i < kMaxMotors; ++i) { + if (_status[i].failed) { + return i; + } + } + + return -1; +} diff --git a/src/lib/motor_failure_detector/MotorFailureDetector.hpp b/src/lib/motor_failure_detector/MotorFailureDetector.hpp new file mode 100644 index 00000000000..b89680e3bd2 --- /dev/null +++ b/src/lib/motor_failure_detector/MotorFailureDetector.hpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * 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 MotorFailureDetector.hpp + * + * Per-motor current-residual motor-failure detector: expected current from the + * commanded signal (linear, I = slope*u + idle), fault latched when |LPF(residual)| + * holds above the threshold for the persistence time. + */ + +#pragma once + +#include +#include +#include + +class MotorFailureDetector +{ +public: + static constexpr int kMaxMotors = 12; + + // Step longer than this [s] => data gap (resets filters + debounce). + // Between the ~10 Hz stream and the 400 ms ESC-offline timeout. + static constexpr float kMaxGap = 0.3f; + + // One model for all motors + struct Config { + float model_b; ///< current-vs-command slope [A] + float model_c; ///< idle current offset [A] + float residual_lpf_tau_s; ///< residual LPF time constant [s] + float threshold_a; ///< trip threshold [A]; <= 0 => check disabled + float persistence_s; ///< time over threshold before latching [s] + }; + + struct MotorStatus { + float residual; ///< raw signed residual I - I_expected [A] + float residual_lpf; ///< low-pass filtered residual [A] + bool failed; ///< latched failure decision + bool excluded; ///< not evaluated this step (reversible or NaN command) + }; + + MotorFailureDetector() { reset(); } + + void configure(const Config &cfg) { _cfg = cfg; reset(); } + + /** Reset all per-motor state (filters, debounce, latch). */ + void reset(); + + /** + * Advance to timestamp now_us [us] (dt derived internally). NaN or reversible + * commands exclude a motor; a non-finite current is a held dropout (the debounce + * keeps running, so a fault that also drops telemetry still latches); a step > + * kMaxGap resets. configure() first; threshold_a <= 0 disables the check. + */ + void update(int num_motors, hrt_abstime now_us, + const float command[], const float current[], + const bool reversible[]); + + const MotorStatus &status(int i) const { return _status[i]; } + bool anyFailed() const; + int firstFailed() const; ///< index of first latched-failed motor, or -1 + +private: + Config _cfg{}; + MotorStatus _status[kMaxMotors] {}; + AlphaFilter _residual_lpf[kMaxMotors]; + systemlib::Hysteresis _hyst[kMaxMotors]; + hrt_abstime _last_us{0}; +}; diff --git a/src/lib/motor_failure_detector/MotorFailureDetectorTest.cpp b/src/lib/motor_failure_detector/MotorFailureDetectorTest.cpp new file mode 100644 index 00000000000..4efdf3d8dca --- /dev/null +++ b/src/lib/motor_failure_detector/MotorFailureDetectorTest.cpp @@ -0,0 +1,295 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include +#include + +#include +#include + +namespace +{ +constexpr float kDt = 1.f / 184.f; // esc_status rate in the logs +constexpr float kBase = 21.f * 0.3f; // healthy current at u=0.3: model_b*u + +hrt_abstime us(float seconds) { return (hrt_abstime)(seconds * 1e6f); } + +// Config from a healthy hexrotor fit (linear, slope ~21 A/command). +MotorFailureDetector::Config makeConfig() +{ + MotorFailureDetector::Config cfg{}; + cfg.model_b = 21.f; + cfg.residual_lpf_tau_s = 0.2f; + cfg.threshold_a = 5.f; + cfg.persistence_s = 0.5f; + return cfg; +} + +// Drive motor 0 for `seconds` at fixed dt, advancing the timestamp `now`, with command u +// and current current_fn(t). The first update() of a fresh detector just seeds the clock. +void run(MotorFailureDetector &d, hrt_abstime &now, float seconds, float dt, float u, + const std::function ¤t_fn, bool reversible = false) +{ + const float cmd[1] = {u}; + const bool rev[1] = {reversible}; + + for (float t = 0.f; t < seconds; t += dt) { + now += us(dt); + const float cur[1] = {current_fn(t)}; + d.update(1, now, cmd, cur, rev); + } +} + +// Sustained open failure at fixed dt for >> persistence; did it latch? Isolates the kMaxGap boundary. +bool latchesAtCadence(float dt) +{ + MotorFailureDetector d; + d.configure(makeConfig()); // persistence 0.5 s + hrt_abstime now = 0; + run(d, now, 3.f, dt, 0.3f, [](float) { return 0.f; }); // 3 s >> persistence + return d.anyFailed(); +} +} // namespace + +// Current matches the model exactly -> residual ~0 -> never trips (the false-positive guard). +TEST(MotorFailureDetectorTest, HealthyNeverTrips) +{ + MotorFailureDetector d; + d.configure(makeConfig()); + hrt_abstime now = 0; + run(d, now, 5.f, kDt, 0.3f, [](float) { return kBase; }); + EXPECT_FALSE(d.anyFailed()); + EXPECT_LT(std::fabs(d.status(0).residual_lpf), 0.1f); +} + +// Commanded high but current collapses to ~0 -> trips after persistence. +TEST(MotorFailureDetectorTest, OpenFailureTrips) +{ + MotorFailureDetector d; + d.configure(makeConfig()); + hrt_abstime now = 0; + run(d, now, 3.f, kDt, 0.3f, [](float) { return 0.f; }); + EXPECT_TRUE(d.anyFailed()); + EXPECT_EQ(d.firstFailed(), 0); + EXPECT_LT(d.status(0).residual_lpf, -5.f); +} + +// A fault that crosses the threshold but recovers before the persistence window must not latch +// (exercises the debounce-reject path: the residual goes over-threshold, then clears). +TEST(MotorFailureDetectorTest, ShortSpikeDoesNotLatch) +{ + MotorFailureDetector d; + d.configure(makeConfig()); + hrt_abstime now = 0; + run(d, now, 0.4f, kDt, 0.3f, [](float) { return 0.f; }); // residual crosses threshold ~0.1 s < 0.5 s persistence + ASSERT_GT(std::fabs(d.status(0).residual_lpf), makeConfig().threshold_a); // genuinely over-threshold... + run(d, now, 3.f, kDt, 0.3f, [](float) { return kBase; }); // ...then recovers before latching + EXPECT_FALSE(d.anyFailed()); +} + +// A winding short / mechanical jam draws far MORE current than commanded -> large +// positive residual. The |.| threshold catches the over-current side, not just open circuits. +TEST(MotorFailureDetectorTest, OvercurrentTrips) +{ + MotorFailureDetector d; + d.configure(makeConfig()); + hrt_abstime now = 0; + run(d, now, 3.f, kDt, 0.3f, [](float) { return 2.5f * kBase; }); // ~2.5x expected + EXPECT_TRUE(d.anyFailed()); + EXPECT_EQ(d.firstFailed(), 0); + EXPECT_GT(d.status(0).residual_lpf, 5.f); // positive residual trips +} + +// A fully shed propeller unloads the motor: current collapses well below the model +// (~15% here) -> negative residual past the threshold. This is the case a current-vs-rpm +// model would miss (rpm stays high while thrust is gone); command-residual catches it. +TEST(MotorFailureDetectorTest, PropLossSevereTrips) +{ + MotorFailureDetector d; + d.configure(makeConfig()); + hrt_abstime now = 0; + run(d, now, 3.f, kDt, 0.3f, [](float) { return 0.15f * kBase; }); + EXPECT_TRUE(d.anyFailed()); + EXPECT_LT(d.status(0).residual_lpf, -5.f); +} + +// A ~50% partial current loss sits inside the dead band (|residual| < threshold) and is +// NOT caught with a global threshold -- the known partial-fault limit. Lowering the +// threshold only helps down to the in-flight maneuver noise floor. +TEST(MotorFailureDetectorTest, PartialLossNearFloorMissed) +{ + MotorFailureDetector d; + d.configure(makeConfig()); + hrt_abstime now = 0; + run(d, now, 5.f, kDt, 0.3f, [](float) { return 0.5f * kBase; }); // 50% loss + EXPECT_FALSE(d.anyFailed()); + EXPECT_GT(std::fabs(d.status(0).residual_lpf), 0.f); // residual present... + EXPECT_LT(std::fabs(d.status(0).residual_lpf), makeConfig().threshold_a); // ...but below trip +} + +// A data gap mid-build must RESET the persistence debounce, not just avoid latching in one step. +TEST(MotorFailureDetectorTest, GapResetsPersistenceTimer) +{ + MotorFailureDetector d; + d.configure(makeConfig()); // persistence 0.5 s + hrt_abstime now = 0; + run(d, now, 0.45f, kDt, 0.3f, [](float) { return 0.f; }); // fault building, < 0.5 s, not latched + ASSERT_FALSE(d.anyFailed()); + + const float cmd[1] = {0.3f}, cur[1] = {0.f}; + const bool rev[1] = {false}; + now += us(0.5f); // > kMaxGap -> gap resets filters + debounce + d.update(1, now, cmd, cur, rev); + + run(d, now, 0.45f, kDt, 0.3f, [](float) { return 0.f; }); // rebuild from zero, < 0.5 s -> must NOT latch + EXPECT_FALSE(d.anyFailed()); +} + +// kMaxGap must clear the ~10 Hz feed interval: at kMaxGap=0.1 the feed (dt~=0.1 s) hit the gap-reset +// every tick, so a sustained fault never latched (replay: 0/100 phases). A real outage still resets. +TEST(MotorFailureDetectorTest, TenHzFeedLatchesButRealGapStillResets) +{ + static_assert(MotorFailureDetector::kMaxGap > 0.1f, "kMaxGap must exceed the ~10 Hz feed interval"); + static_assert(MotorFailureDetector::kMaxGap < 0.4f, "kMaxGap must stay below the 400 ms ESC-offline timeout"); + + EXPECT_TRUE(latchesAtCadence(0.105f)); // 10 Hz feed + EXPECT_FALSE(latchesAtCadence(0.4f)); // real outage +} + +// Exclusion gates: a reversible or NaN (motor-off) command is never evaluated. +TEST(MotorFailureDetectorTest, ExcludedGates) +{ + for (auto kind : {0, 1}) { + MotorFailureDetector d; + d.configure(makeConfig()); + hrt_abstime now = 0; + const float u = (kind == 1) ? NAN : 0.3f; + run(d, now, 5.f, kDt, u, [](float) { return 150.f; }, /*reversible=*/kind == 0); + EXPECT_FALSE(d.anyFailed()); + EXPECT_TRUE(d.status(0).excluded); + } +} + +// threshold_a <= 0 (incl. a default-constructed Config) disables the check: never latches. +TEST(MotorFailureDetectorTest, MonitorOnlyNeverLatches) +{ + MotorFailureDetector unconfigured; // zero Config + hrt_abstime now0 = 0; + run(unconfigured, now0, 5.f, kDt, 0.5f, [](float) { return 123.f; }); + EXPECT_FALSE(unconfigured.anyFailed()); + + MotorFailureDetector::Config cfg = makeConfig(); + cfg.threshold_a = 0.f; + MotorFailureDetector d; + d.configure(cfg); + hrt_abstime now = 0; + run(d, now, 5.f, kDt, 0.3f, [](float) { return 0.f; }); + EXPECT_FALSE(d.anyFailed()); +} + +// Telemetry dropout (NaN current) holds the residual: the debounce keeps running on the +// held value (so a fault that also drops telemetry still latches) and state stays finite. +TEST(MotorFailureDetectorTest, DropoutHoldsTimerAndStaysFinite) +{ + MotorFailureDetector d; + d.configure(makeConfig()); // persistence 0.5 s + hrt_abstime now = 0; + run(d, now, 0.45f, kDt, 0.3f, [](float) { return 0.f; }); // fault building, not yet latched + ASSERT_FALSE(d.anyFailed()); + run(d, now, 1.0f, kDt, 0.3f, [](float) { return NAN; }); // dropout: held residual keeps the debounce running + EXPECT_TRUE(d.anyFailed()); + EXPECT_TRUE(std::isfinite(d.status(0).residual_lpf)); +} + +// A single step longer than kMaxGap is a data gap: it must reset, not latch in one jump. +TEST(MotorFailureDetectorTest, LargeDtGapNoLatch) +{ + MotorFailureDetector d; + d.configure(makeConfig()); // persistence 0.5 s + const float cmd[1] = {0.3f}, cur[1] = {0.f}; + const bool rev[1] = {false}; + hrt_abstime now = 0; + d.update(1, now, cmd, cur, rev); // seed the clock + now += us(10.f); // 10 s >> kMaxGap, single step -> gap, no latch + d.update(1, now, cmd, cur, rev); + EXPECT_FALSE(d.anyFailed()); +} + +// A residual built up before exclusion must not survive it and trip on re-entry. +TEST(MotorFailureDetectorTest, ExclusionReentryNoStaleTrip) +{ + MotorFailureDetector d; + d.configure(makeConfig()); + hrt_abstime now = 0; + run(d, now, 0.3f, kDt, 0.3f, [](float) { return 0.f; }); // big residual (< persistence) + run(d, now, 1.f, kDt, NAN, [](float) { return 0.f; }); // excluded (motor commanded off) + ASSERT_TRUE(d.status(0).excluded); + run(d, now, 3.f, kDt, 0.3f, [](float) { return kBase; }); // healthy again + EXPECT_FALSE(d.anyFailed()); + EXPECT_LT(std::fabs(d.status(0).residual_lpf), 0.5f); // reseeded, not stale +} + +// reset() clears the sticky failure latch and the firstFailed() index (re-arm). +TEST(MotorFailureDetectorTest, ResetClearsLatch) +{ + MotorFailureDetector d; + d.configure(makeConfig()); + hrt_abstime now = 0; + run(d, now, 3.f, kDt, 0.3f, [](float) { return 0.f; }); + ASSERT_TRUE(d.anyFailed()); + d.reset(); + EXPECT_FALSE(d.anyFailed()); + EXPECT_EQ(d.firstFailed(), -1); +} + +// firstFailed() returns the lowest index among multiple failed motors. +TEST(MotorFailureDetectorTest, MultiMotorFirstFailed) +{ + MotorFailureDetector d; + d.configure(makeConfig()); + const int N = 3; + const float cmd[N] = {0.3f, 0.3f, 0.3f}; + const bool rev[N] = {false, false, false}; + hrt_abstime now = 0; + + for (float t = 0.f; t < 3.f; t += kDt) { + now += us(kDt); + const float cur[N] = {kBase, 0.f, 0.f}; // 0 healthy, 1 & 2 open + d.update(N, now, cmd, cur, rev); + } + + EXPECT_FALSE(d.status(0).failed); + EXPECT_TRUE(d.status(1).failed); + EXPECT_TRUE(d.status(2).failed); + EXPECT_EQ(d.firstFailed(), 1); +}