mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-07-24 15:27:41 +08:00
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.
This commit is contained in:
gguidone
@@ -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)
|
||||
|
||||
40
src/lib/motor_failure_detector/CMakeLists.txt
Normal file
40
src/lib/motor_failure_detector/CMakeLists.txt
Normal file
@@ -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)
|
||||
135
src/lib/motor_failure_detector/MotorFailureDetector.cpp
Normal file
135
src/lib/motor_failure_detector/MotorFailureDetector.cpp
Normal file
@@ -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 <cmath>
|
||||
|
||||
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<float>(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;
|
||||
}
|
||||
100
src/lib/motor_failure_detector/MotorFailureDetector.hpp
Normal file
100
src/lib/motor_failure_detector/MotorFailureDetector.hpp
Normal file
@@ -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 <drivers/drv_hrt.h>
|
||||
#include <lib/hysteresis/hysteresis.h>
|
||||
#include <mathlib/math/filter/AlphaFilter.hpp>
|
||||
|
||||
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<float> _residual_lpf[kMaxMotors];
|
||||
systemlib::Hysteresis _hyst[kMaxMotors];
|
||||
hrt_abstime _last_us{0};
|
||||
};
|
||||
295
src/lib/motor_failure_detector/MotorFailureDetectorTest.cpp
Normal file
295
src/lib/motor_failure_detector/MotorFailureDetectorTest.cpp
Normal file
@@ -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 <gtest/gtest.h>
|
||||
#include <MotorFailureDetector.hpp>
|
||||
|
||||
#include <cmath>
|
||||
#include <functional>
|
||||
|
||||
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<float(float)> ¤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);
|
||||
}
|
||||
Reference in New Issue
Block a user