mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
feat(motor_failure_detector): add current-residual detection (#27711)
* 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 = current_slope_a*u + current_idle_a) and trips on a low-pass-filtered
residual held past a trip band 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.
Band and persistence are configurable per residual direction. Undercurrent is the
fault-consistent side -- an open circuit, a shed propeller or a desync makes a
motor draw less than commanded -- while overcurrent covers a winding short,
mechanical jam or demagnetization. They are separate because the benign residual
is far from symmetric: a fast throttle increase draws acceleration current the
static model cannot predict, so overcurrent excursions are large and unbounded,
whereas an undercurrent residual is floored at -I_exp. A single band sized to
reject the overcurrent transients can therefore end up wider than the largest
undercurrent fault a motor can physically produce, which makes a full current
collapse undetectable rather than merely slow. Set undercurrent_threshold_a <= 0
to keep one symmetric band on the residual magnitude.
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 per direction (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.
Includes an 18-case gtest spanning the failure-mode spectrum and edge cases:
healthy-never-trips (incl. high throttle), open-circuit and overcurrent trips,
severe prop-loss undercurrent, the missed ~50% partial loss (documents the
detection floor), the per-direction bands (a tighter undercurrent band catching a
collapse one symmetric band misses, band independence, per-side persistence),
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.
* refactor(motor_failure_detector): resolve the undercurrent band at configure time
An unset undercurrent threshold now mirrors the overcurrent band in
configure(), so update() runs the same two bands in every configuration and
no longer carries a second threshold test that reads like a repeat of the
disable gate. A symmetric band is the two-band case with equal bands.
Also name esc_status CONNECTED_ESC_MAX at kMaxMotors so the constant is
findable, use the PX4 time literals for the persistence conversions, and
drop the excluded temporary.
* feat(commander): use current-residual detector for the ESC motor-failure check
Replace the flat, instantaneous current check in EscChecks::checkMotorStatus with
the MotorFailureDetector library (added in the parent PR). Per motor it predicts
the expected ESC current from the commanded thrust
(I_exp = MOTFAIL_C2T*u + MOTFAIL_IDLE) and trips on the low-pass-filtered
residual, with an independent band and hold time per direction: MOTFAIL_UNDER held
for MOTFAIL_UND_TIME below the expectation, MOTFAIL_OVER held for MOTFAIL_OVR_TIME
above it.
Because the two directions need separate bands, the single band and hold time are
renamed to name the direction they act on: MOTFAIL_OFF -> MOTFAIL_OVER and
MOTFAIL_TIME -> MOTFAIL_OVR_TIME, migrated in param_translation.cpp. The old band
applied to both directions, so it carries over to the overcurrent side, and
MOTFAIL_UNDER defaults to 0, which keeps a single symmetric band -- an existing
vehicle behaves as before. MOTFAIL_IDLE also defaults to 0, and the whole check
stays gated by FD_ACT_EN (off by default).
The detector is driven off esc_status.timestamp, so a telemetry stall freezes the
filter instead of integrating stale current; the separate 400 ms ESC telemetry
timeout stays in checkEscOnline(). It is reconfigured from parameters while
disarmed, so a parameter change never resets a latched failure mid-flight. The
motor_failure_mask still feeds failure_detector_status via
FailureDetector::publishStatus, leaving the ControlAllocator reaction path
untouched.
The mask reports the detector's sticky latch alone, not whether the motor is
currently evaluable. That preserves the invariant the previous check spelled out
-- "Only set, never clear mid-air: stopping the motor in response could make it
appear healthy again" -- because handling a failure makes exactly that motor
unevaluable: the control allocator drops it, which publishes a NAN command,
which excludes it from the detector. Re-deriving the mask from the exclusion
state clears it, the allocator restores all motors, and the next check re-trips.
Measured in SIH on a hexarotor with a missing propeller: 83 mask transitions in
nine seconds, with a reversed recovery motor swinging between idle and near-full
reverse at the 10 Hz check rate. An ESC going offline after a real failure
retracts the report the same way. The latch is only ever set on a tick where the
motor was evaluable, so reporting it alone cannot report a motor that was never
judged.
* 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
* docs(motor_failure_detector): document the current-residual motor-failure check
Add a configuration page for motor failure detection, next to the other
actuator topics, and a developer page for the offline replay tool:
- what the check compares and why the under/overcurrent bands are separate
- the new MOTFAIL_* parameters, including the rename migration
- how to fit the current model and pick the bands from the vehicle's own
logs with mfd_replay / batch_replay.sh
The "Motor Failure Trigger" section in the failsafe page described the
pre-rename parameters, the unfiltered symmetric comparison and claimed the
failure was non-latching, so it is reduced to a summary of the trigger and
its action, pointing at the new page for the thresholds.
* docs(docs): Subedit for simpler introductions and terminology
* docs(docs): Improve upgrading from v1.18 section
* feat(motor_failure_detector): add current-model fitting tool
mfd_fit derives MOTFAIL_C2T and MOTFAIL_IDLE from recorded logs, so the model
and the thresholds are taken from the same data.
Like mfd_replay it reads ESC current, motor command and arming state through
ulog_cpp by field name, so one binary handles any log version. Each log is a
single streaming pass that accumulates only the sums a least-squares line needs,
so memory does not grow with the log or with the number of logs, and a whole
corpus can be fitted in one invocation.
Alongside the parameters it reports the residual spread the trip bands have to
clear, and each motor's bias against the shared model.
* docs(docs): document motor failure detection tuning and the fitting tool
Follow-up to review of the detection and replay pages:
- fix the link left behind by the Failure Response -> Failure Action rename
- drop the Multicopter badge: the check has no vehicle-type gate, so a VTOL's
multirotor motors and its forward-flight motor are both checked
- describe the model-fitting step without the statistics jargon
- state that only healthy logs may be used, for the fit and the thresholds
- document mfd_fit, and reduce the release note to a pointer
* feat(motor_failure_detector): report residual extremes and add a band sweep
The replay tool printed the largest residual magnitude, which hid whether an
excursion was above or below the model. Since the two trip bands are chosen per
direction, that left the reader running one band at a time to find out. It now
prints the signed minimum and maximum instead, so each band's headroom is
readable straight from the table.
Add sweep_matrix.sh alongside batch_replay.sh: it sweeps one band against its
hold time over a set of logs and prints how many logs false-trip at each pair,
so the lowest usable band per hold time is the first zero in a row.
Both tools now number motors from 1, matching the "Motor N ..." the vehicle
reports, rather than from 0.
* docs(docs): rework the motor failure calibration walkthrough
Follow a real calibration through both pages and fix what it ran into:
- the replay page's worked example is one vehicle end to end, with every
number and pasted output taken from that corpus
- the fitting tool, the single-log run and the band sweeps are ordered the way
the work is actually done, with replaying a whole fleet last
- the sweeps are shown as a matrix of false trips per (hold time, band), and
the grids are derived from the residual extremes rather than guessed
- commands match the output they produce, and say what they need: mfd_replay
without --set uses a fallback calibration that belongs to another vehicle
- vehicle_status is optional, not required, for the replay
- drop the repetition: batch_replay.sh, the hover-current ceiling and the
phase-sensitivity note were each explained two or three times
* fix(motor_failure_detector): separate configure and reset with the appropriate triggers
* refactor(motor_failure_detector): clarify leading edge hysteresis as trigger time compared to "persistence"
* fix(escCheck): encapsulate _motor_failure_direction handling into MotorFailureDetector
* fix(astyle): exclude ulog_cpp submodule form check
* fix(motor_failure_detector_tools): fail log replay when no log is present
* refactor(motor_failure_detector): Improve documentation
* fix(escCheck): check load per ESC not per motor function
This was already done in my original implementation. It supports checking multiple ESCs mapped to the same motor function individually. It's also consistent with other ESC checks that run for every ESC that reports telemetry.
* fix(motor_failure_detector): run update() per motor and consider individual telemetry timestamps
* fix(motor_failure_detector): simplify output interface to getters of actually used results
* fix(motor_failure_detector): inline what doesn't need to be separate
* fix(motor_failure_detector): make the bands independent and drop special cases
- Each trip band is now independent: a threshold <= 0 disables only that band. The undercurrent band no longer copies the overcurrent one.
- Drop the MOTFAIL_OFF/MOTFAIL_TIME parameter migration. The new parameters start at their defaults.
- Treat a non-finite current like a non-finite command (not evaluated, filter and debounce reset). No driver reports NaN current, so the path only existed for its test.
- Compute the residual as max(current, 0) - I_expected. Same as before, but written such that it's obvious: regeneration counts as 0A.
- Drop unnecessary trigger time parameter clamp to >= 0 before converting to unsigned microseconds, metadate already decalres that as out of range.
- Count each ESC sample once by its own timestamp in mfd_fit, like the motor_failure_detector.
* fix(motor_failure_detector): check all still considered online ESC telemetry
* fix(motor_failure_detector): declare all link dependencies
* fix(motor_failure_detector_tools): reuse log reading logic to reduce duplication
* fix(motor_failure_detector): replace reversible flag getting passed into the detector for separate handling
Having a NAN command already handles that.
* test(escChecks): add unit tests for load based failure detection
---------
Co-authored-by: Hamish Willee <hamishwillee@gmail.com>
Co-authored-by: Matthias Grob <maetugr@gmail.com>
Co-authored-by: Andrea Bernasconi <andrea.bernasconi@auterion.com>
This commit is contained in:
25 files changed
+2539
-90
No files matched your search
@@ -118,3 +118,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
|
||||
@@ -38,6 +38,7 @@ exec find boards msg src platforms test \
|
||||
-path src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client-v3 -prune -o \
|
||||
-path src/lib/cdrstream/cyclonedds -prune -o \
|
||||
-path src/lib/cdrstream/rosidl -prune -o \
|
||||
-path src/lib/ulog_cpp -prune -o \
|
||||
-path src/modules/zenoh/zenoh-pico -prune -o \
|
||||
-path boards/modalai/voxl2/libfc-sensor-api -prune -o \
|
||||
-path boards/modalai/voxl2/src/lib/mpa/libmodal-json -prune -o \
|
||||
|
||||
+3
-1
@@ -317,7 +317,6 @@
|
||||
- [Sensor Thermal Compensation](advanced_config/sensor_thermal_calibration.md)
|
||||
- [Actuators](actuators/index.md)
|
||||
- [Actuator Allocation](config/actuators.md)
|
||||
- [Motor Failure Recovery](config/motor_failure_recovery.md)
|
||||
- [ESC Calibration](advanced_config/esc_calibration.md)
|
||||
- [ESCs & Motors](peripherals/esc_motors.md)
|
||||
- [ESC Protocols](esc/esc_protocols.md)
|
||||
@@ -333,6 +332,8 @@
|
||||
- [VESC Project ESCs](peripherals/vesc.md)
|
||||
- [Zubax Telega ESCs](dronecan/zubax_telega.md)
|
||||
- [Internal Combustion Engines](actuators/internal_combustion_engine.md)
|
||||
- [Motor Failure Detection](config/motor_failure_detection.md)
|
||||
- [Motor Failure Recovery](config/motor_failure_recovery.md)
|
||||
- [Manual Control](config/manual_control.md)
|
||||
- [Radio Control (RC)](getting_started/rc_transmitter_receiver.md)
|
||||
- [Radio Setup](config/radio.md)
|
||||
@@ -886,6 +887,7 @@
|
||||
- [Simulation Debugging](debug/simulation_debugging.md)
|
||||
- [Sending Debug Values](debug/debug_values.md)
|
||||
- [System-wide Replay](debug/system_wide_replay.md)
|
||||
- [Offline Motor Failure Replay](debug/motor_failure_replay.md)
|
||||
- [Profiling](debug/profiling.md)
|
||||
- [Binary Size Profiling](debug/binary_size_profiling.md)
|
||||
- [Logging](dev_log/logging.md)
|
||||
|
||||
+3
-1
@@ -314,7 +314,6 @@
|
||||
- [Sensor Thermal Compensation](/advanced_config/sensor_thermal_calibration.md)
|
||||
- [Actuators](/actuators/index.md)
|
||||
- [Actuator Allocation](/config/actuators.md)
|
||||
- [Motor Failure Recovery](/config/motor_failure_recovery.md)
|
||||
- [ESC Calibration](/advanced_config/esc_calibration.md)
|
||||
- [ESCs & Motors](/peripherals/esc_motors.md)
|
||||
- [ESC Protocols](/esc/esc_protocols.md)
|
||||
@@ -329,6 +328,8 @@
|
||||
- [Vertiq Motor/ESC Modules](/peripherals/vertiq.md)
|
||||
- [VESC Project ESCs](/peripherals/vesc.md)
|
||||
- [Zubax Telega ESCs](/dronecan/zubax_telega.md)
|
||||
- [Motor Failure Detection](/config/motor_failure_detection.md)
|
||||
- [Motor Failure Recovery](/config/motor_failure_recovery.md)
|
||||
- [Manual Control](/config/manual_control.md)
|
||||
- [Radio Control (RC)](/getting_started/rc_transmitter_receiver.md)
|
||||
- [Radio Setup](/config/radio.md)
|
||||
@@ -871,6 +872,7 @@
|
||||
- [Simulation Debugging](/debug/simulation_debugging.md)
|
||||
- [Sending Debug Values](/debug/debug_values.md)
|
||||
- [System-wide Replay](/debug/system_wide_replay.md)
|
||||
- [Offline Motor Failure Replay](/debug/motor_failure_replay.md)
|
||||
- [Profiling](/debug/profiling.md)
|
||||
- [Binary Size Profiling](/debug/binary_size_profiling.md)
|
||||
- [Logging](/dev_log/logging.md)
|
||||
|
||||
@@ -3,11 +3,12 @@
|
||||
This section contains topics about the core actuators used for flight control (ESC/motors, and servos), and how they are assigned to the flight controller outputs, configured, and calibrated.
|
||||
|
||||
- [Actuator Allocation](../config/actuators.md) — Configure flight controller outputs for specific functions and ESC/servo types.
|
||||
- [Motor Failure Recovery](../config/motor_failure_recovery.md) — How allocation is reconfigured if a motor fails.
|
||||
|
||||
- [ESCs & Motors](../peripherals/esc_motors.md) — ESCs such as [DShot](../peripherals/dshot.md) (recommended) and DroneCAN.
|
||||
- [ESC Calibration](../advanced_config/esc_calibration.md) — Calibration for PWM ESC (not required for DShot/CAN ESC/servos).
|
||||
- [Internal Combustion Engines](../actuators/internal_combustion_engine.md) — Petrol/nitro engines, including start/stop sequencing and idle RPM control.
|
||||
- [Motor Failure Detection](../config/motor_failure_detection.md) — Detect a motor that stops producing thrust, from the current its ESC reports.
|
||||
- [Motor Failure Recovery](../config/motor_failure_recovery.md) — How allocation is reconfigured if a motor fails.
|
||||
|
||||
## See Also
|
||||
|
||||
|
||||
@@ -0,0 +1,241 @@
|
||||
# Motor Failure Detection
|
||||
|
||||
<Badge type="tip" text="main (PX4 v2.0)" />
|
||||
|
||||
PX4 can detect a motor that has stopped producing the thrust it was commanded to give, as caused by a stalled motor, a lost or broken propeller, a seized bearing, a rubbing blade, and so on.
|
||||
|
||||
The mechanism works by comparing the current each ESC reports against the current expected for the command that motor was given.
|
||||
A motor whose current stays too far from that expectation for long enough is flagged as failed.
|
||||
The failure is reported to the ground station, and can be [acted on](#failure-action) by removing the motor from the control allocation or by running a failsafe action.
|
||||
|
||||
Failure detection is supported for DShot and DroneCAN ESC provided the following [requirements](#requirements) are met.
|
||||
It is disabled by default ([FD_ACT_EN](#FD_ACT_EN)).
|
||||
|
||||
::: warning
|
||||
The thresholds are airframe specific, and the defaults are not a working configuration.
|
||||
Values tuned for one vehicle will either false-trip or go blind on another, because both the expected current and the size of the healthy current fluctuations depend on the motors, propellers, ESCs and battery.
|
||||
Always calibrate on logs from the vehicle you are configuring: see [Choosing Parameter Values](#choosing-parameter-values).
|
||||
:::
|
||||
|
||||
## Requirements
|
||||
|
||||
- ESCs that report **per-motor current**, and are configured so that PX4 knows which motor each ESC drives.
|
||||
|
||||
For DShot ESC this needs either a [telemetry wire](../peripherals/dshot.md#esc-telemetry) or [Extended DShot Telemetry](../peripherals/dshot.md#extended-dshot-telemetry-edt).
|
||||
[DroneCAN ESC](../dronecan/escs.md) report current as part of their status.
|
||||
ESC that report no current, or only report RPM, cannot be checked.
|
||||
|
||||
- Motors, not servos: only outputs assigned to a motor function are checked.
|
||||
- No vehicle-type restriction: a VTOL's multirotor motors and its forward-flight motor are both checked, in every flight mode.
|
||||
Only the [failure actions](#failure-action) are multirotor-specific, since they assume the vehicle has rotors to spare.
|
||||
- Recorded flight logs from the vehicle, to calibrate the model and the thresholds.
|
||||
|
||||
## How Detection Works
|
||||
|
||||
For every armed motor, the check runs the following four steps at 10 Hz in [commander](../modules/modules_system.md#commander):
|
||||
|
||||
1. **Compute expected current.**
|
||||
|
||||
For each motor, the current expected for the command that the control allocator sent to it:
|
||||
|
||||
```text
|
||||
I_expected = MOTFAIL_C2T * u + MOTFAIL_IDLE
|
||||
```
|
||||
|
||||
where `u` is the normalized motor command in the range 0 to 1.
|
||||
The _commanded_ signal is used rather than the measured RPM, so the expectation stays valid even when telemetry degrades.
|
||||
|
||||
2. **Compute residual.**
|
||||
|
||||
The difference between reported and expected current, `I_measured - I_expected`, is low-pass filtered with a fixed 0.2 s time constant.
|
||||
The filter suppresses single noisy samples, so that the thresholds can be set close to the healthy fluctuation band.
|
||||
|
||||
3. **Compute two trip bands.**
|
||||
|
||||
The filtered residual fluctuates around zero even on a healthy motor, from noise and model error.
|
||||
A trip happens when it leaves that band and stays out, with a separate width and trigger time per direction:
|
||||
|
||||
```text
|
||||
undercurrent: LPF(I_measured - I_expected) < -MOTFAIL_UNDER held for MOTFAIL_UND_TIME
|
||||
overcurrent: LPF(I_measured - I_expected) > MOTFAIL_OVER held for MOTFAIL_OVR_TIME
|
||||
```
|
||||
|
||||
Undercurrent means the motor drew less than expected, as a stalled or damaged one would.
|
||||
Overcurrent means it drew more, which healthy motors also do during sharp throttle increases.
|
||||
|
||||
4. **Latch the fail flag if either band trips.**
|
||||
|
||||
A motor that satisfies either condition is flagged as failed, and reported as `Motor N undercurrent detected` or `Motor N overcurrent detected`.
|
||||
The flag is held until the vehicle disarms.
|
||||
It is not cleared in flight, because reacting to a motor failure changes what that motor is commanded to do, which removes the evidence the check runs on: a motor that has been switched off would immediately look healthy again.
|
||||
|
||||
Special cases:
|
||||
|
||||
- A motor is not judged until its ESC has reported a non-zero current at least once, so ESC that only report while spinning do not trip the check on the ground.
|
||||
- A motor is skipped while its command is `NaN` (the allocation has switched it off), and while it is in reversible mode.
|
||||
- The filter runs on the ESC sample timestamp, so an interruption in ESC telemetry freezes it instead of integrating stale current.
|
||||
A gap longer than 0.4 s, the time after which an ESC counts as offline, resets the filter and cancels a pending trip.
|
||||
Complete loss of ESC telemetry is a separate check, enabled with [COM_ARM_CHK_ESCS](../advanced_config/parameter_reference.md#COM_ARM_CHK_ESCS).
|
||||
|
||||
::: details Why the Bands Are Not Symmetric
|
||||
|
||||
The overcurrent and undercurrent directions do not carry the same information, so a single threshold for both serves neither well.
|
||||
|
||||
**The fault side is bounded.**
|
||||
Current cannot fall below zero, so the largest undercurrent residual that can physically exist is `I_expected` itself.
|
||||
On a vehicle drawing 8 A per motor in hover, a rotor that stops producing thrust at hover throttle is a residual of at most 8 A, and a partial loss is proportionally smaller.
|
||||
|
||||
**The nuisance side is not bounded.**
|
||||
A fast throttle increase draws acceleration current that a static command-to-current model cannot predict.
|
||||
The largest residuals seen in perfectly healthy flight are therefore positive, and there is no ceiling on them.
|
||||
|
||||
A single band has to clear those positive excursions, and can end up above the largest negative signal a real fault can produce.
|
||||
That makes a stopped motor structurally undetectable rather than merely slow to detect.
|
||||
Splitting the bands lets the undercurrent side sit where the faults are, while the overcurrent side stays above the transients.
|
||||
|
||||
:::
|
||||
|
||||
## Failure Action
|
||||
|
||||
Two parameters configure what happens when a failure is detected:
|
||||
|
||||
- [CA_FAILURE_MODE](#CA_FAILURE_MODE) selects what the [control allocator](../concept/control_allocation.md) does, such as removing the failed motor from the allocation.
|
||||
- [COM_ACT_FAIL_ACT](#COM_ACT_FAIL_ACT) selects a failsafe action, in the same way as the other [failsafes](../config/safety.md#motor-failure-trigger).
|
||||
|
||||
A quadrotor that loses a rotor cannot hold yaw, so the realistic responses there are landing or termination.
|
||||
Vehicles with more rotors than the minimum can keep flying with one removed.
|
||||
|
||||
## Choosing Parameter Values
|
||||
|
||||
There are two things to calibrate, in this order: the current model ([MOTFAIL_C2T](#MOTFAIL_C2T) and [MOTFAIL_IDLE](#MOTFAIL_IDLE)), then the two bands and their trigger times ([MOTFAIL_UNDER](#MOTFAIL_UNDER)/[MOTFAIL_UND_TIME](#MOTFAIL_UND_TIME) and [MOTFAIL_OVER](#MOTFAIL_OVER)/[MOTFAIL_OVR_TIME](#MOTFAIL_OVR_TIME)).
|
||||
The goal is the smallest bands and shortest trigger times that still never trip across a set of healthy flights.
|
||||
That is a tradeoff: a wider band held longer is more robust against false positives; a narrower band held for less time reacts sooner when a motor really does fail.
|
||||
|
||||
Both stages are fitted from the vehicle's own logs, which have to come from _healthy_ flights covering the throttle range it really uses, including climbs and aggressive manoeuvres.
|
||||
Those flights are what set the achievable thresholds.
|
||||
|
||||
1. **Find how command maps to current.**
|
||||
|
||||
The model has to say what current a healthy motor draws for a given command, so what you need is the relationship between the commanded value `u` and the reported ESC current, collected over all motors and all logs.
|
||||
Use only flights in which every motor was healthy: a log from failure testing, or one with a real fault in it, describes the fault rather than the vehicle and shifts the model away from the rest of the fleet.
|
||||
|
||||
Plotting the two against each other shows that relationship, and fitting a straight line through the points is a convenient way to reduce it to the two numbers the detector wants: the slope of the line goes into [MOTFAIL_C2T](#MOTFAIL_C2T), and its value at zero command into [MOTFAIL_IDLE](#MOTFAIL_IDLE).
|
||||
|
||||
The [`mfd_fit` tool](../debug/motor_failure_replay.md#fitting-the-current-model) does this directly from the logs and prints the two values to set.
|
||||
|
||||
One model is shared by all motors.
|
||||
Differences in current draw between motors are small compared with the width of the trip bands, so there is no practical benefit in having separate models.
|
||||
|
||||
2. **Set the bands from the healthy residual.**
|
||||
|
||||
Use the [Offline Motor Failure Replay Tool](../debug/motor_failure_replay.md) to replay the logs through the detector and find the lowest band that produces no trips anywhere in the set, then add margin on top.
|
||||
|
||||
Around 30% above the lowest false-positive-free value is a reasonable starting point, so a set that first stays clean at 3.0 A might be configured at 4.0 A.
|
||||
|
||||
3. **Check the trigger time as well as the band.**
|
||||
|
||||
A trip needs the residual to stay outside the band for the whole trigger time, so a longer trigger time allows a lower band.
|
||||
How much it allows is very different in the two directions.
|
||||
|
||||
The table below is from 6 healthy flights of one 6-rotor vehicle drawing about 8 A per motor in hover, and gives the lowest band that never trips on any motor of any of those flights:
|
||||
|
||||
Undercurrent:
|
||||
|
||||
| [MOTFAIL_UND_TIME](#MOTFAIL_UND_TIME) | Lowest [MOTFAIL_UNDER](#MOTFAIL_UNDER) |
|
||||
| ------------------------------------- | -------------------------------------- |
|
||||
| 0.1 s | 3.5 A |
|
||||
| 0.3 s | 3.0 A |
|
||||
| 0.5 s | 3.0 A |
|
||||
| 0.7 s | 3.0 A |
|
||||
| 1.0 s | 2.5 A |
|
||||
|
||||
Overcurrent:
|
||||
|
||||
| [MOTFAIL_OVR_TIME](#MOTFAIL_OVR_TIME) | Lowest [MOTFAIL_OVER](#MOTFAIL_OVER) |
|
||||
| ------------------------------------- | ------------------------------------ |
|
||||
| 0.4 s | 6.0 A |
|
||||
| 0.7 s | 3.0 A |
|
||||
| 1.0 s | 2.0 A |
|
||||
| 1.5 s | 1.5 A |
|
||||
| 2.5 s | 1.0 A |
|
||||
|
||||
The values are specific to that vehicle, but the pattern is general.
|
||||
The overcurrent band gains a lot from waiting longer, from 6.0 A at 0.4 s down to 1.0 A at 2.5 s, because the healthy current transients that set it are large but brief.
|
||||
The undercurrent band gains much less, 3.5 A down to 2.5 A over ten times the trigger time, and most of what it gains is already there at 0.3 s, because what limits it is steady model error and waiting does not make that go away.
|
||||
A long trigger time is affordable on the overcurrent side, where the faults are thermal and a few seconds cost nothing, and not on the undercurrent side, where a stopped rotor has to be caught in about a second.
|
||||
|
||||
4. **Work out what the configuration can detect.**
|
||||
|
||||
A fault is only visible once it moves the current further than the band, so the undercurrent band and the expected current together set the smallest loss that can be seen, as a fraction of what the motor was drawing:
|
||||
|
||||
```text
|
||||
smallest detectable loss = MOTFAIL_UNDER / I_expected
|
||||
```
|
||||
|
||||
Continuing the example, the 4.0 A band chosen in step 2 against 8 A of hover current is 50%: a rotor losing less than roughly half its current still looks healthy at hover.
|
||||
The fraction gets worse as throttle drops, because `I_expected` shrinks with it.
|
||||
A complete stop is the same sum with the whole current missing, so it is detectable wherever `I_expected` on its own exceeds the band, which for these numbers is any command above about half the hover command.
|
||||
|
||||
If that floor is too high for the faults you care about, the only way to lower it is a better current model, not a smaller band: the band is already at the smallest width that avoids false trips on healthy motors, so it can't be reduced further.
|
||||
|
||||
5. **Verify on the whole set.**
|
||||
|
||||
Include logs that were not used for the fit, and confirm that no motor trips.
|
||||
|
||||
::: tip
|
||||
A _total_ failure is caught far more easily than the floor in step 4 suggests, because it amplifies its own signal.
|
||||
|
||||
When a rotor stops producing thrust, the controller commands it harder to recover the missing lift, usually until it saturates.
|
||||
`I_expected` follows the command up while the real current stays near zero, so the residual keeps growing rather than settling at the value it had at hover.
|
||||
|
||||
In one flight test, a rotor that lost its propeller in hover was commanded to full throttle within about 200 ms, which took its residual to nearly 3x the trip band; the failure was flagged 1 s after it happened.
|
||||
|
||||
A _partial_ loss gets none of this.
|
||||
The vehicle still flies, so the command stays near hover and the residual stays small, which is why the floor in step 4 governs partial faults and not total ones.
|
||||
:::
|
||||
|
||||
## Limitations
|
||||
|
||||
- **Partial failures near hover are the hard case.**
|
||||
The residual is bounded by the expected current, so a small loss at low throttle can be indistinguishable from model error.
|
||||
- **Calibration is per airframe**, and has to be revisited when motors, propellers, ESCs or battery chemistry change.
|
||||
- **Only current is used.**
|
||||
A motor that draws the expected current but produces no thrust, such as one with a slipping propeller adapter, is not detected.
|
||||
- **The flag is latched** until disarm, so a fault that recovers stays flagged for the rest of the flight.
|
||||
- **Detection is not instantaneous.**
|
||||
The 10 Hz check rate, the 0.2 s filter and the trigger time all add up.
|
||||
Expect a few hundred milliseconds at best, and about a second in practice for a complete loss.
|
||||
|
||||
## Parameters
|
||||
|
||||
| Parameter | Description |
|
||||
| ----------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------ |
|
||||
| <a id="FD_ACT_EN"></a>[FD_ACT_EN](../advanced_config/parameter_reference.md#FD_ACT_EN) | Enable motor failure detection. Disabled by default. |
|
||||
| <a id="MOTFAIL_C2T"></a>[MOTFAIL_C2T](../advanced_config/parameter_reference.md#MOTFAIL_C2T) | Current model slope: expected current per unit of motor command (A). |
|
||||
| <a id="MOTFAIL_IDLE"></a>[MOTFAIL_IDLE](../advanced_config/parameter_reference.md#MOTFAIL_IDLE) | Current model offset: expected current at zero command (A). |
|
||||
| <a id="MOTFAIL_UNDER"></a>[MOTFAIL_UNDER](../advanced_config/parameter_reference.md#MOTFAIL_UNDER) | Undercurrent trip band (A): how far below the expected current a motor may run.<br>Set to `0` to disable undercurrent detection. |
|
||||
| <a id="MOTFAIL_UND_TIME"></a>[MOTFAIL_UND_TIME](../advanced_config/parameter_reference.md#MOTFAIL_UND_TIME) | Time (s) the undercurrent band must be exceeded before the motor is flagged. |
|
||||
| <a id="MOTFAIL_OVER"></a>[MOTFAIL_OVER](../advanced_config/parameter_reference.md#MOTFAIL_OVER) | Overcurrent trip band (A): how far above the expected current a motor may run.<br>Set to `0` to disable overcurrent detection. |
|
||||
| <a id="MOTFAIL_OVR_TIME"></a>[MOTFAIL_OVR_TIME](../advanced_config/parameter_reference.md#MOTFAIL_OVR_TIME) | Time (s) the overcurrent band must be exceeded before the motor is flagged. |
|
||||
| <a id="CA_FAILURE_MODE"></a>[CA_FAILURE_MODE](../advanced_config/parameter_reference.md#CA_FAILURE_MODE) | What the control allocator does on a failure, such as removing the failed motor from the allocation. |
|
||||
| <a id="COM_ACT_FAIL_ACT"></a>[COM_ACT_FAIL_ACT](../advanced_config/parameter_reference.md#COM_ACT_FAIL_ACT) | Failsafe action on a detected motor failure, such as warning, hold, return, land, or terminate. |
|
||||
|
||||
## Upgrading From PX4 v1.18
|
||||
|
||||
In PX4 v1.18 and earlier, the check compared the _instantaneous_ current residual against a single symmetric threshold, `±MOTFAIL_OFF`, held for `MOTFAIL_TIME`.
|
||||
|
||||
From PX4 main (v2.0), the residual is low-pass filtered before it is compared, so a sustained deviation is needed rather than isolated samples.
|
||||
The single threshold is also replaced by two independent bands: [MOTFAIL_UNDER](#MOTFAIL_UNDER)/[MOTFAIL_UND_TIME](#MOTFAIL_UND_TIME) for undercurrent and [MOTFAIL_OVER](#MOTFAIL_OVER)/[MOTFAIL_OVR_TIME](#MOTFAIL_OVR_TIME) for overcurrent.
|
||||
The current model also gains an offset term ([MOTFAIL_IDLE](#MOTFAIL_IDLE)), and the [offline replay tool](../debug/motor_failure_replay.md) is available to calibrate the new parameters from logs.
|
||||
|
||||
`MOTFAIL_OFF` and `MOTFAIL_TIME` are not migrated: after the upgrade, all of the new parameters start at their defaults.
|
||||
To keep the v1.18 shape of the check, set [MOTFAIL_OVER](#MOTFAIL_OVER) and [MOTFAIL_UNDER](#MOTFAIL_UNDER) to the old `MOTFAIL_OFF`, and [MOTFAIL_OVR_TIME](#MOTFAIL_OVR_TIME) and [MOTFAIL_UND_TIME](#MOTFAIL_UND_TIME) to the old `MOTFAIL_TIME`.
|
||||
|
||||
The thresholds are worth re-deriving rather than copied: see [Choosing Parameter Values](#choosing-parameter-values).
|
||||
|
||||
## See Also
|
||||
|
||||
- [Offline Motor Failure Replay](../debug/motor_failure_replay.md) — replaying recorded logs through the detector to calibrate and verify it.
|
||||
- [Safety Configuration (Failsafes)](../config/safety.md#motor-failure-trigger) — the failure detector and its other triggers.
|
||||
- [Control Allocation (Mixing)](../concept/control_allocation.md) — how a removed motor changes the actuator setpoints.
|
||||
- [Actuator Configuration and Testing](../config/actuators.md) — geometry, motor order and bidirectional motors.
|
||||
+11
-17
@@ -420,27 +420,21 @@ The relevant parameters are shown below:
|
||||
|
||||
### Motor Failure Trigger
|
||||
|
||||
The failure detector can be configured to detect a motor failure while armed (and trigger an associated action) if the ESC current falls outside expected threshold for more than [MOTFAIL_TIME](#MOTFAIL_TIME) seconds.
|
||||
A motor flagged by this current check stays flagged until the vehicle disarms, because the reaction to a failure may stop the motor and thereby make it look healthy again.
|
||||
The failure detector can be configured to detect a failed motor (or a lost or broken propeller, a seized bearing, a rubbing blade, and so on).
|
||||
More specifically, it detects a motor that has stopped producing the thrust it was commanded, by comparing the current each ESC reports against the current expected for that motor's command.
|
||||
A motor whose current stays too far below or above that expectation for long enough is flagged as failed, and stays flagged until the vehicle disarms, because the reaction to a failure may stop the motor and thereby make it look healthy again.
|
||||
|
||||
The undercurrent and overcurrent conditions are defined by:
|
||||
The trigger needs ESC that report per-motor current, and thresholds calibrated for the specific airframe.
|
||||
For how the check works, and how to set the `MOTFAIL_*` thresholds, see [Motor Failure Detection](../config/motor_failure_detection.md).
|
||||
|
||||
```text
|
||||
undercurrent: {esc current} < {MOTFAIL_C2T} * {motor command [0,1]} - {MOTFAIL_OFF}
|
||||
overcurrent: {esc current} > {MOTFAIL_C2T} * {motor command [0,1]} + {MOTFAIL_OFF}
|
||||
```
|
||||
|
||||
The action on motor failure is set with [CA_FAILURE_MODE](#CA_FAILURE_MODE).
|
||||
Actions depend on the vehicle type, and might include warning the user, removing failed motors from control allocation, disabling symmetrically opposite motors, and so on.
|
||||
The action on motor failure is set with [COM_ACT_FAIL_ACT](../advanced_config/parameter_reference.md#COM_ACT_FAIL_ACT) and [CA_FAILURE_MODE](#CA_FAILURE_MODE).
|
||||
Actions depend on the vehicle type, and might include warning the user, running a failsafe, removing failed motors from control allocation, disabling symmetrically opposite motors, and so on.
|
||||
For more information see [Motor Failure Recovery](motor_failure_recovery.md).
|
||||
|
||||
| Parameter | Description |
|
||||
| -------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------- |
|
||||
| <a id="FD_ACT_EN"></a>[FD_ACT_EN](../advanced_config/parameter_reference.md#FD_ACT_EN) | Enable/disable the motor failure trigger completely. |
|
||||
| <a id="MOTFAIL_C2T"></a>[MOTFAIL_C2T](../advanced_config/parameter_reference.md#MOTFAIL_C2T) | Slope between normalized motor command [0–1] and expected steady-state current (FD_ACT_MOT_C2T at 100%) (A/%). |
|
||||
| <a id="MOTFAIL_OFF"></a>[MOTFAIL_OFF](../advanced_config/parameter_reference.md#MOTFAIL_OFF) | Under/over-current detection threshold offset (A). Added/subtracted to the expected current to form the upper and lower bounds. |
|
||||
| <a id="MOTFAIL_TIME"></a>[MOTFAIL_TIME](../advanced_config/parameter_reference.md#MOTFAIL_TIME) | Hysteresis time (s) for which the current threshold must remain exceeded before a motor failure is triggered. |
|
||||
| <a id="CA_FAILURE_MODE"></a>[CA_FAILURE_MODE](../advanced_config/parameter_reference.md#CA_FAILURE_MODE) | Failure mode. See [Motor Failure Recovery](motor_failure_recovery.md). |
|
||||
| Parameter | Description |
|
||||
| -------------------------------------------------------------------------------------------------------- | ---------------------------------------------------------------------- |
|
||||
| <a id="FD_ACT_EN"></a>[FD_ACT_EN](../advanced_config/parameter_reference.md#FD_ACT_EN) | Enable/disable the motor failure trigger completely. |
|
||||
| <a id="CA_FAILURE_MODE"></a>[CA_FAILURE_MODE](../advanced_config/parameter_reference.md#CA_FAILURE_MODE) | Failure mode. See [Motor Failure Recovery](motor_failure_recovery.md). |
|
||||
|
||||
### External Automatic Trigger System (ATS)
|
||||
|
||||
|
||||
@@ -0,0 +1,380 @@
|
||||
# Offline Motor Failure Replay
|
||||
|
||||
<Badge type="tip" text="main (PX4 v2.0)" />
|
||||
|
||||
Two command line tools calibrate the [motor failure detector](../config/motor_failure_detection.md) against a vehicle's own flight logs.
|
||||
`mfd_fit` derives the current model from a set of logs, and `mfd_replay` runs the real detector over a log and prints what it would have decided for each motor.
|
||||
|
||||
They exist because the [threshold parameters](../config/motor_failure_detection.md#choosing-parameter-values) (`MOTFAIL_*`) are specific to an airframe, so the only place to get them is that airframe's own flights.
|
||||
`mfd_replay` links the detector library itself, so a replay runs the same code that runs in flight.
|
||||
|
||||
One flight is rarely enough to characterise a vehicle, so usually we run the tools over a directory of logs.
|
||||
|
||||
The tools are used to:
|
||||
|
||||
- Confirm a candidate configuration never trips on a set of healthy flights.
|
||||
- Find the tightest thresholds a vehicle's data supports, by sweeping values and re-running.
|
||||
- Check after the fact whether the detector would have flagged a motor in a flight where something went wrong.
|
||||
|
||||
## Building
|
||||
|
||||
To build the tools:
|
||||
|
||||
1. Build PX4 SITL (to populate the build directory)
|
||||
|
||||
```bash
|
||||
make px4_sitl_default # once, if the build directory does not exist yet
|
||||
```
|
||||
|
||||
2. Build the tool binaries:
|
||||
|
||||
```bash
|
||||
cmake -DMFD_REPLAY_TOOL=ON build/px4_sitl_default
|
||||
ninja -C build/px4_sitl_default mfd_replay mfd_fit
|
||||
```
|
||||
|
||||
This produces `build/px4_sitl_default/mfd_replay` and `build/px4_sitl_default/mfd_fit`.
|
||||
`mfd_fit` derives the current model from logs; `mfd_replay` then runs the detector with it.
|
||||
|
||||
## Fitting the Current Model
|
||||
|
||||
The first step is to fit the current model: find the straight line that best predicts each motor's current from its command, since the detector flags a motor whose current strays too far from it.
|
||||
To do this, run `mfd_fit` once over every healthy log you have for the airframe.
|
||||
It fits the expected current `I_expected = MOTFAIL_C2T*u + MOTFAIL_IDLE` to all of them together, where `u` is the motor command, and prints the two parameter values plus the residual sigma you need later to [choose the bands](#choosing-the-bands).
|
||||
The more of the vehicle's flying the logs cover, the better the fit, and there is no limit on how many you pass: a hundred logs take under a second.
|
||||
|
||||
::: warning
|
||||
Fit on healthy flights only.
|
||||
The fit cannot tell a faulty motor from a healthy one, so a single failure-test log pulls the model toward that fault and shifts the residual of every healthy motor with it.
|
||||
:::
|
||||
|
||||
```bash
|
||||
./build/px4_sitl_default/mfd_fit ~/logs/healthy/*.ulg
|
||||
```
|
||||
|
||||
Typical output, for three healthy flights:
|
||||
|
||||
```text
|
||||
# 3 logs, 80697 armed samples
|
||||
# I_expected = 6.91*u + 0.37 A (residual sigma 0.50 A)
|
||||
# mean command 0.387, mean current 3.04 A
|
||||
|
||||
MOTFAIL_C2T 6.91
|
||||
MOTFAIL_IDLE 0.37
|
||||
|
||||
# per-motor mean residual against this model [A]
|
||||
# motor 1: -0.12 (sigma 0.64, n 13484)
|
||||
# ...
|
||||
# motor 6: -0.10 (sigma 0.46, n 13360)
|
||||
# worst per-motor bias 0.12 A -- the trip bands have to clear this before any fault.
|
||||
```
|
||||
|
||||
It contains:
|
||||
|
||||
- The fitted model, `I_expected = 6.91*u + 0.37 A`.
|
||||
`6.91` is the slope (expected current per unit of command, in A) and `0.37` is the offset (expected current at zero command, in A).
|
||||
- The same slope and offset as the two parameters to set, [MOTFAIL_C2T](../config/motor_failure_detection.md#MOTFAIL_C2T) `6.91` and [MOTFAIL_IDLE](../config/motor_failure_detection.md#MOTFAIL_IDLE) `0.37`.
|
||||
- The residual sigma, `0.50 A`: how far the samples scatter around the model.
|
||||
This is the healthy noise the trip bands have to clear, so it caps how tight they can be.
|
||||
- For each motor, its mean residual against the model (`-0.12 A` for motor 1), with that motor's own sigma and sample count `n`.
|
||||
- The worst per-motor bias, `0.12 A`: the largest of the per-motor mean residuals, ignoring sign.
|
||||
The detector uses one model for all motors, so this is error the bands absorb before any fault does.
|
||||
|
||||
To use it:
|
||||
|
||||
1. Set `MOTFAIL_C2T` and `MOTFAIL_IDLE` to the printed values.
|
||||
|
||||
On some vehicles the fitted offset comes out below zero.
|
||||
`MOTFAIL_IDLE` cannot be set below 0, so the tool adds a `NOTE` saying so: use 0 in that case.
|
||||
Nothing is lost by it, because the bands are then chosen against the residual that results, so they absorb the difference.
|
||||
|
||||
2. Keep the residual sigma for [Choosing the Bands](#choosing-the-bands).
|
||||
|
||||
Use the fitted offset, not one copied from another airframe.
|
||||
An offset that is too high by some amount lowers every healthy motor's residual by that same amount, which the undercurrent band then has to absorb.
|
||||
For example, replaying the healthy flight shown in [Running the Detector on One Log](#running-the-detector-on-one-log) with another airframe's offset of 2.88 A, instead of the fitted 0.37 A, raises the expected current by 2.51 A.
|
||||
Every residual drops by the same 2.51 A (motor 1's lowest goes from -1.79 A to -4.30 A), past the 2.25 A undercurrent band, so all six healthy motors are reported as failed:
|
||||
|
||||
```text
|
||||
# config (...): I_exp = 6.91*u + 2.88 A, trip if LPF(I-I_exp) < -2.25 for 0.70 s or > 1.00 for 2.50 s
|
||||
# motor min[A] max[A] trips verdict
|
||||
1 -4.30 +1.69 5/5 FAILED (all phases)
|
||||
...
|
||||
6 -4.75 +1.43 5/5 FAILED (all phases)
|
||||
```
|
||||
|
||||
## Running the Detector on One Log
|
||||
|
||||
```bash
|
||||
./build/px4_sitl_default/mfd_replay flight.ulg \
|
||||
--set MOTFAIL_C2T=6.91 --set MOTFAIL_IDLE=0.37 \
|
||||
--set MOTFAIL_UNDER=2.25 --set MOTFAIL_UND_TIME=0.7 \
|
||||
--set MOTFAIL_OVER=1.0 --set MOTFAIL_OVR_TIME=2.5
|
||||
```
|
||||
|
||||
Run without the `--set` flags it uses a built-in fallback calibration, which belongs to a different vehicle and will report failures on healthy flights.
|
||||
|
||||
The log must contain `esc_status` (per-motor current) and `actuator_motors` (the motor commands).
|
||||
`vehicle_status` is used to evaluate only the armed part of the flight; without it the tool says so and evaluates everything.
|
||||
Fields are resolved by name from the log's own format, so one binary reads logs from any PX4 version, with 8 or 12 ESCs, without rebuilding.
|
||||
A log that is not a ULog, or that is missing a needed field, is reported as an error rather than silently misread.
|
||||
|
||||
Output for a healthy flight with a calibrated configuration:
|
||||
|
||||
```text
|
||||
# flight.ulg (5 phases, 728 armed samples each)
|
||||
# config (built-in calibration, overrides: MOTFAIL_C2T MOTFAIL_IDLE MOTFAIL_OVER MOTFAIL_OVR_TIME MOTFAIL_UNDER MOTFAIL_UND_TIME): I_exp = 6.91*u + 0.37 A, trip if LPF(I-I_exp) < -2.25 for 0.70 s or > 1.00 for 2.50 s
|
||||
# motor min[A] max[A] trips verdict
|
||||
1 -1.79 +4.20 0/5 ok
|
||||
2 -3.09 +2.11 0/5 ok
|
||||
3 -1.79 +2.67 0/5 ok
|
||||
4 -2.13 +2.38 0/5 ok
|
||||
5 -2.35 +2.78 0/5 ok
|
||||
6 -2.24 +3.94 0/5 ok
|
||||
```
|
||||
|
||||
| Column | Meaning |
|
||||
| --------- | ---------------------------------------------------------------------------------------------------------------------------------------------------- |
|
||||
| `motor` | Motor index, counting from 1, as mapped from each ESC's actuator function. |
|
||||
| `min[A]` | Deepest the filtered residual went below the model, over the whole flight and every sampling phase. This is what the undercurrent band has to clear. |
|
||||
| `max[A]` | Highest it went above the model, likewise. This is what the overcurrent band has to clear. |
|
||||
| `trips` | How many of the sampling phases flagged this motor. |
|
||||
| `verdict` | `ok`, `FAILED (all phases)`, or `FAILED (sampling-dependent!)` when only some phases flagged it. |
|
||||
|
||||
Motors are numbered from 1, the same as in the `Motor 1 undercurrent detected` message the vehicle reports.
|
||||
|
||||
Exit codes make the tool usable as a check in a script: `0` if every motor stayed healthy, `1` if any motor was flagged, `2` if the log could not be used.
|
||||
|
||||
### The Sampling Phase Sweep
|
||||
|
||||
Every replay runs five detectors side by side over the same log (the `5 phases` in the header and the `/5` in `trips`), because a single one can miss a trip that would happen on the vehicle.
|
||||
|
||||
On the vehicle the check runs at 10 Hz, so it sees only a subset of the ESC samples, and _which_ subset depends on the alignment between the check and the telemetry stream.
|
||||
That alignment cannot be reconstructed from a log.
|
||||
|
||||
Each detector therefore starts its 10 Hz grid at a different offset, and the tool reports the spread.
|
||||
A single-phase answer would be a lower bound on the trip risk, not a verdict.
|
||||
|
||||
Read the result as follows:
|
||||
|
||||
- `0/5`: the configuration did not trip on this flight, and `min`/`max` say by how much it cleared each band.
|
||||
- `0/5` but with a note printed below the table: one of the extremes moved by more than 30% between phases, so this run happened not to trip and the same flight could trip on the vehicle.
|
||||
- `1/5` … `4/5` (`sampling-dependent!`): treat this as a false positive, not as a pass.
|
||||
- `5/5`: it would have tripped.
|
||||
|
||||
### Where the Configuration Comes From
|
||||
|
||||
The configuration in the `# config` header line is the set of `MOTFAIL_*` values the replay ran the detector with.
|
||||
For each parameter, in order of precedence:
|
||||
|
||||
1. `--set NAME=value` on the command line.
|
||||
2. The value recorded in the log, if the log contains the `MOTFAIL_*` parameters.
|
||||
3. A built-in fallback.
|
||||
|
||||
The header line always prints the configuration that was used, and names any parameter that came from `--set`.
|
||||
|
||||
::: warning
|
||||
The built-in fallback is an example fit for one particular vehicle, not a recommended default.
|
||||
On another airframe it will report failures on perfectly healthy flights.
|
||||
Always replay with the configuration you are actually evaluating.
|
||||
:::
|
||||
|
||||
Overriding one knob at a time is how the band sweeps below are done:
|
||||
|
||||
```bash
|
||||
./build/px4_sitl_default/mfd_replay flight.ulg --set MOTFAIL_UNDER=1.75 --set MOTFAIL_UND_TIME=0.7
|
||||
```
|
||||
|
||||
The accepted names are [MOTFAIL_C2T](../config/motor_failure_detection.md#MOTFAIL_C2T), [MOTFAIL_IDLE](../config/motor_failure_detection.md#MOTFAIL_IDLE), [MOTFAIL_UNDER](../config/motor_failure_detection.md#MOTFAIL_UNDER), [MOTFAIL_UND_TIME](../config/motor_failure_detection.md#MOTFAIL_UND_TIME), [MOTFAIL_OVER](../config/motor_failure_detection.md#MOTFAIL_OVER) and [MOTFAIL_OVR_TIME](../config/motor_failure_detection.md#MOTFAIL_OVR_TIME).
|
||||
|
||||
## Choosing the Bands
|
||||
|
||||
After [fitting the current model](#fitting-the-current-model) you have `MOTFAIL_C2T`, `MOTFAIL_IDLE` and the residual sigma, which you use with `mfd_replay` to choose the remaining four parameters: the undercurrent and overcurrent bands and their trigger times.
|
||||
|
||||
The goal is the tightest bands that never trip on healthy flight, so the logs have to cover the throttle range the vehicle really flies, including climbs and aggressive manoeuvres: the largest deviation across them is what sets the achievable threshold.
|
||||
As with the fit, every log has to come from a flight where all motors were healthy.
|
||||
|
||||
The steps below use [`batch_replay.sh`](#replaying-a-fleet-of-logs) to replay a whole set of logs at once.
|
||||
|
||||
**1. Read the healthy residual.**
|
||||
|
||||
Replay each log with the fitted model and both bands disabled (`0`), so nothing trips and the `min`/`max` columns report the full excursion.
|
||||
Each log prints its own header and table; one of them:
|
||||
|
||||
```bash
|
||||
for log in ~/logs/healthy/*.ulg; do
|
||||
./build/px4_sitl_default/mfd_replay "$log" \
|
||||
--set MOTFAIL_C2T=6.91 --set MOTFAIL_IDLE=0.37 --set MOTFAIL_OVER=0 --set MOTFAIL_UNDER=0
|
||||
done
|
||||
```
|
||||
|
||||
```text
|
||||
# motor min[A] max[A] trips verdict
|
||||
1 -1.36 +7.84 0/5 ok
|
||||
2 -1.32 +2.72 0/5 ok
|
||||
3 -1.85 +1.93 0/5 ok
|
||||
4 -1.37 +7.72 0/5 ok
|
||||
5 -2.17 +1.11 0/5 ok
|
||||
6 -1.55 +1.96 0/5 ok
|
||||
```
|
||||
|
||||
Read the two columns separately, because they set the two sweeps.
|
||||
The deepest `min` across all logs and motors is what the undercurrent band has to clear, and the highest `max` is what the overcurrent band has to clear.
|
||||
On this vehicle they are very different, about −2.2 A against +7.8 A, which is the whole reason for two bands.
|
||||
|
||||
**2. Sweep each band against its trigger time.**
|
||||
|
||||
`sweep_matrix.sh` sweeps one direction while disabling the other, and prints
|
||||
how many logs false-trip at each (trigger time, band) pair.
|
||||
The first `0` in a row is the lowest band that trigger time supports.
|
||||
|
||||
```bash
|
||||
export C2T=6.91 IDLE=0.37 # the model you fitted
|
||||
src/lib/motor_failure_detector/tools/sweep_matrix.sh ~/logs/healthy under
|
||||
src/lib/motor_failure_detector/tools/sweep_matrix.sh ~/logs/healthy over
|
||||
```
|
||||
|
||||
Set the grid from the `min`/`max` you just read, with `TRIGGER_TIMES` and `BANDS`:
|
||||
|
||||
- **Bands** run from the healthy noise up to that side's extreme.
|
||||
Roughly twice the residual sigma makes a sensible bottom, since below that every log trips.
|
||||
On the undercurrent side the top is the expected current at hover: a band above it cannot be reached even by a rotor that has stopped completely.
|
||||
The overcurrent side has no such ceiling, so start at its `max` and come down by spending trigger time instead of amplitude.
|
||||
- **Trigger times** come from the latency you can accept.
|
||||
A latch takes about `0.1 s` of sampling plus `0.2 s` of filter plus the trigger time, so a fault that destabilises the vehicle wants well under a second, while a thermal fault can wait seconds.
|
||||
|
||||
```bash
|
||||
TRIGGER_TIMES="0.1 0.3 0.5 0.7 1.0" BANDS="1.0 1.25 1.5 1.75 2.0 2.5 3.0" \
|
||||
src/lib/motor_failure_detector/tools/sweep_matrix.sh ~/logs/healthy under
|
||||
```
|
||||
|
||||
```text
|
||||
trigger[s] 1.0 1.25 1.5 1.75 2.0 2.5 3.0 <- MOTFAIL_UNDER [A] (logs tripped)
|
||||
0.1 3 2 2 2 2 1 0
|
||||
0.3 2 2 2 2 1 1 0
|
||||
0.5 2 2 1 1 1 0 0
|
||||
0.7 2 1 1 0 0 0 0
|
||||
1.0 1 1 1 0 0 0 0
|
||||
```
|
||||
|
||||
Each row gives one answer.
|
||||
Walk it from left to right and stop at the first `0`:
|
||||
|
||||
| trigger time | lowest band with no trips |
|
||||
| ------------ | ------------------------- |
|
||||
| 0.1 s | 3.0 A |
|
||||
| 0.3 s | 3.0 A |
|
||||
| 0.5 s | 2.5 A |
|
||||
| 0.7 s | 1.75 A |
|
||||
| 1.0 s | 1.75 A |
|
||||
|
||||
Now compare each floor against this side's ceiling, the expected current at hover, which is about 3.0 A here.
|
||||
|
||||
That makes the top two rows unusable rather than merely cautious: the smallest band that avoids false positives is already the band a total failure at hover cannot reach.
|
||||
The 0.5 s row sits just under the ceiling, which leaves almost nothing for a partial loss.
|
||||
By 0.7 s the floor has dropped to 1.75 A, which leaves real room below the ceiling, and a longer trigger time gains nothing.
|
||||
That is what picks 0.7 s here, rather than taste.
|
||||
|
||||
**Sweep coarse, then refine.**
|
||||
The first grid only has to show which zone is interesting; a second, finer pass finds the edge.
|
||||
|
||||
The overcurrent side is the example.
|
||||
Run on the default grid, `sweep_matrix.sh ~/logs/healthy over` puts every zero in the 2.5 s row, which says the floor is 0.5 A or lower and the trigger time that reaches it is 2.5 s or shorter, but not what either actually is.
|
||||
Re-running with finer trigger times and smaller bands:
|
||||
|
||||
```bash
|
||||
TRIGGER_TIMES="1.5 1.75 2.0 2.25 2.5" BANDS="0.2 0.3 0.5 0.75 1.0 1.5 2.0 3.0" \
|
||||
src/lib/motor_failure_detector/tools/sweep_matrix.sh ~/logs/healthy over
|
||||
```
|
||||
|
||||
```text
|
||||
trigger[s] 0.2 0.3 0.5 0.75 1.0 1.5 2.0 3.0 <- MOTFAIL_OVER [A] (logs tripped)
|
||||
1.5 3 3 2 1 1 1 1 1
|
||||
1.75 2 1 0 0 0 0 0 0
|
||||
2.0 2 1 0 0 0 0 0 0
|
||||
2.25 2 1 0 0 0 0 0 0
|
||||
2.5 2 1 0 0 0 0 0 0
|
||||
```
|
||||
|
||||
Two things are readable now.
|
||||
The floor is 0.5 A, since 0.3 A still trips a log.
|
||||
And it arrives abruptly between 1.5 s and 1.75 s: at 1.5 s even a 3.0 A band trips, at 1.75 s a 0.5 A band is clean, and every longer trigger time is identical.
|
||||
|
||||
That second part is physics rather than a coincidence of the grid.
|
||||
The benign excursion that sets this side must last between 1.5 s and 1.75 s, so a trigger time past it rejects the excursion completely while a trigger time just short of it rejects nothing.
|
||||
|
||||
Choosing from a pair of tables like these:
|
||||
|
||||
- On the **fault side**, latency decides the trigger time, and the band takes some margin above the floor.
|
||||
How much is your call, and it depends on how much data the floor came from.
|
||||
A floor measured on three flights is a sample rather than the worst case, so something like 30% hedges against the next flight going deeper, turning a 1.75 A floor into 2.25 A.
|
||||
With hundreds of flights behind it, the measured floor is already close to the real one and you can sit nearer to it.
|
||||
- On the **nuisance side** there is no ceiling from physics, so the band is set by what you want to catch rather than by what a fault can reach.
|
||||
Pick the smallest over-draw worth flagging, find the shortest trigger time whose floor is below it, then go a little longer than that.
|
||||
The floor drops at whatever trigger time outlasts the benign excursion, so the shortest trigger time that works is sitting on its edge.
|
||||
The extra trigger time costs only latency, and these faults are thermal.
|
||||
|
||||
::: warning
|
||||
If you write your own sweep instead of using the script, write it in `bash` and pass the `--set` flags
|
||||
literally or through an array.
|
||||
In `zsh` an unquoted variable holding `--set A=1 --set B=2` arrives as a single argument, so the tool
|
||||
prints a usage error and exits without a verdict table, and a loop that only counts failures reads that
|
||||
as "nothing tripped".
|
||||
Assert that a table was produced before trusting a zero, as the script's `grep -q` guard does.
|
||||
:::
|
||||
|
||||
**3. Verify.**
|
||||
|
||||
Re-run the whole set of flight logs with the chosen configuration, including logs that were not used for the fit, and confirm every motor is `ok` with no phase-dependent verdicts.
|
||||
Then check the detection floor the configuration implies: the undercurrent band divided by the per-motor hover current is the smallest current loss that can be seen at hover.
|
||||
|
||||
## Replaying a Fleet of Logs
|
||||
|
||||
A single flight doesn't usually contain enough information to calculate thresholds, so the usual unit of work is a set of flight logs.
|
||||
`batch_replay.sh` runs the tool over every `.ulg` in a directory and summarizes the verdicts:
|
||||
|
||||
```bash
|
||||
src/lib/motor_failure_detector/tools/batch_replay.sh ~/logs/healthy --set MOTFAIL_UNDER=1.75 --set MOTFAIL_UND_TIME=0.7
|
||||
```
|
||||
|
||||
```text
|
||||
OK flight1.ulg
|
||||
OK flight2.ulg
|
||||
OK flight3.ulg
|
||||
----
|
||||
total=3 OK=3 FAIL=0 SKIP=0
|
||||
```
|
||||
|
||||
Anything after the directory is passed straight through to `mfd_replay`.
|
||||
`SKIP` counts logs with no usable ESC data.
|
||||
Run it from the repository root, or set `MFD_REPLAY` to the path of the binary.
|
||||
|
||||
::: details Replaying thousands of logs
|
||||
`batch_replay.sh` works through the logs one at a time, which is fine for tens of them and slow for thousands.
|
||||
For a large set, run `mfd_replay` itself with one process per log, and give each its own output file so the verdict tables do not interleave:
|
||||
|
||||
```bash
|
||||
mkdir -p /tmp/verdicts
|
||||
ls ~/logs/healthy/*.ulg | xargs -P"$(nproc)" -I{} \
|
||||
sh -c './build/px4_sitl_default/mfd_replay "$1" > /tmp/verdicts/$(basename "$1").txt' _ {}
|
||||
|
||||
grep -l FAILED /tmp/verdicts/*.txt # the logs that tripped
|
||||
```
|
||||
|
||||
This gives the full per-motor table for every log instead of the one-line summary, which is what you want when you then have to look at _why_ something tripped.
|
||||
:::
|
||||
|
||||
## What the Replay Does Not Reproduce
|
||||
|
||||
- **The sampling phase**, which is why the sweep exists.
|
||||
- **Real faults**, unless the log contains one.
|
||||
There is no fault injection: to study the true-positive side, replay a log from a flight where a motor really failed, or modify a log so that the current of one motor reflects the fault you want to study.
|
||||
- **Parameter changes during the log.**
|
||||
On the vehicle the detector is reset whenever it is disarmed and reconfigured on parameter change; the replay configures it once, resets it on disarm and evaluates only the armed samples.
|
||||
- **Anything outside the current check.**
|
||||
The ESC telemetry timeout, the arming checks and the failure response all live elsewhere in the firmware and are not part of this tool.
|
||||
|
||||
## See Also
|
||||
|
||||
- [Motor Failure Detection](../config/motor_failure_detection.md) — what the detector does and how it is configured on the vehicle.
|
||||
- [Flight Log Analysis](../dev_log/flight_log_analysis.md) — general tooling for working with ULog files.
|
||||
@@ -62,6 +62,8 @@ Please continue reading for [upgrade instructions](#upgrade-guide).
|
||||
[SENS_GNSS_PRIME](../advanced_config/parameter_reference.md#SENS_GNSS_PRIME) now defaults to `-1` (Auto) instead of `0`, and saved values are not changed: with Auto the moving base of a moving base pair is the primary receiver, and without one, a receiver that passes the strict checks ([GNSS_REQ_EPH](../advanced_config/parameter_reference.md#GNSS_REQ_EPH) and the other `GNSS_REQ_*` parameters) is used, then one with an RTK fixed solution.
|
||||
To keep preferring the receiver on the main serial port, set `SENS_GNSS_PRIME` to `0`.
|
||||
In flight the primary receiver is now also left when it has failed the strict checks for 10 s while the other receiver passed them. ([PX4-Autopilot#28798](https://github.com/PX4/PX4-Autopilot/pull/28798))
|
||||
- **Re-check the motor failure trigger thresholds.**
|
||||
`MOTFAIL_OFF` and `MOTFAIL_TIME` are replaced by a separate band and trigger time per direction ([MOTFAIL_OVER](../advanced_config/parameter_reference.md#MOTFAIL_OVER)/[MOTFAIL_OVR_TIME](../advanced_config/parameter_reference.md#MOTFAIL_OVR_TIME) and [MOTFAIL_UNDER](../advanced_config/parameter_reference.md#MOTFAIL_UNDER)/[MOTFAIL_UND_TIME](../advanced_config/parameter_reference.md#MOTFAIL_UND_TIME)), and are not migrated: the new parameters start at their defaults. The check now compares a low-pass filtered current residual instead of the instantaneous value, so the thresholds are worth re-deriving: see [Motor Failure Detection > Upgrading from PX4 v1.18](../config/motor_failure_detection.md#upgrading-from-px4-v1-18). ([PX4-Autopilot#27917](https://github.com/PX4/PX4-Autopilot/pull/27917))
|
||||
|
||||
## Other changes
|
||||
|
||||
@@ -103,6 +105,7 @@ Please continue reading for [upgrade instructions](#upgrade-guide).
|
||||
Returns to home if the estimated flight time to home is within the remaining battery time; otherwise returns to the closest rally point.
|
||||
Falls back to the closest safe point (home or rally) if battery time remaining is unavailable.
|
||||
- [Secure boot](../advanced_config/bootloader_secure_boot.md#multi-part-signed-images) now supports applications that can boot securely from SD card or eMMC. ([PX4-Autopilot#28228](https://github.com/PX4/PX4-Autopilot/pull/28228))
|
||||
- [Motor failure detection](../config/motor_failure_detection.md) now compares a low-pass filtered current residual against a separate trip band and trigger time for the undercurrent and overcurrent directions, instead of one symmetric band on the instantaneous current, and the current model gains an offset term. See [Upgrading from PX4 v1.18](../config/motor_failure_detection.md#upgrading-from-px4-v1-18). ([PX4-Autopilot#27711](https://github.com/PX4/PX4-Autopilot/pull/27711), [PX4-Autopilot#27917](https://github.com/PX4/PX4-Autopilot/pull/27917))
|
||||
|
||||
### Estimation
|
||||
|
||||
@@ -122,7 +125,7 @@ Please continue reading for [upgrade instructions](#upgrade-guide).
|
||||
|
||||
### Debug & Logging
|
||||
|
||||
- TBD
|
||||
- [Offline motor failure replay](../debug/motor_failure_replay.md): a host tool that runs the real motor failure detector over a recorded `.ulg`, for calibrating and verifying the [motor failure detection](../config/motor_failure_detection.md) thresholds against a vehicle's own flights. ([PX4-Autopilot#27917](https://github.com/PX4/PX4-Autopilot/pull/27917))
|
||||
|
||||
### Ethernet
|
||||
|
||||
|
||||
@@ -64,6 +64,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(open_drone_id EXCLUDE_FROM_ALL)
|
||||
add_subdirectory(perf EXCLUDE_FROM_ALL)
|
||||
|
||||
@@ -0,0 +1,63 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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})
|
||||
target_link_libraries(MotorFailureDetector PUBLIC hysteresis mathlib)
|
||||
|
||||
px4_add_unit_gtest(SRC MotorFailureDetectorTest.cpp LINKLIBS MotorFailureDetector)
|
||||
|
||||
# Offline log tools: mfd_replay drives the real detector over a recorded ulog, mfd_fit derives the
|
||||
# current model from one (host only). Opt-in (off by default) via -DMFD_REPLAY_TOOL=ON so a normal
|
||||
# build never fetches/builds them or their ulog_cpp submodule.
|
||||
set(MFD_REPLAY_TOOL OFF CACHE BOOL "Build the host-only MotorFailureDetector offline log tools")
|
||||
|
||||
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 ulog_cpp::ulog_cpp)
|
||||
|
||||
add_executable(mfd_fit EXCLUDE_FROM_ALL tools/mfd_fit.cpp)
|
||||
target_link_libraries(mfd_fit PRIVATE MotorFailureDetector ulog_cpp::ulog_cpp)
|
||||
endif()
|
||||
@@ -0,0 +1,105 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
void MotorFailureDetector::configure(const Config &cfg)
|
||||
{
|
||||
_cfg = cfg;
|
||||
|
||||
for (int i = 0; i < kMaxMotors; ++i) {
|
||||
_hyst_over[i].set_hysteresis_time_from(false, _cfg.overcurrent_trigger_time_s * 1_s); // false = rising-edge debounce time
|
||||
_hyst_under[i].set_hysteresis_time_from(false, _cfg.undercurrent_trigger_time_s * 1_s);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorFailureDetector::reset()
|
||||
{
|
||||
for (int i = 0; i < kMaxMotors; ++i) {
|
||||
_failure[i] = Failure::None;
|
||||
_residual_lpf[i].reset(0.f);
|
||||
_hyst_over[i].set_state_and_update(false, 0); // no falling-edge delay: clears immediately, keeps the debounce times
|
||||
_hyst_under[i].set_state_and_update(false, 0);
|
||||
_last_us[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void MotorFailureDetector::update(int i, hrt_abstime now_us, float command, float current)
|
||||
{
|
||||
if (!validIndex(i)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Same sample as last time (the ESC has not reported since): nothing new to evaluate.
|
||||
if (now_us == _last_us[i]) {
|
||||
return;
|
||||
}
|
||||
|
||||
const float dt = static_cast<float>(now_us - _last_us[i]) * 1e-6f;
|
||||
_last_us[i] = now_us;
|
||||
|
||||
// Cannot evaluate (non-finite command or current, or dt > kMaxGap incl. the first call)
|
||||
// -> reset filter + debounce so re-entry starts clean; the latch is kept.
|
||||
if (!std::isfinite(command) || !std::isfinite(current) || dt > kMaxGap) {
|
||||
_residual_lpf[i].reset(0.f);
|
||||
_hyst_over[i].set_state_and_update(false, now_us); // cancel any pending trip
|
||||
_hyst_under[i].set_state_and_update(false, now_us);
|
||||
return;
|
||||
}
|
||||
|
||||
const float i_expected = _cfg.current_slope_a * command + _cfg.current_idle_a;
|
||||
|
||||
// A motor cannot draw negative current: regeneration counts as 0 A, not as a fault.
|
||||
_residual_lpf[i].setParameters(dt, kResidualLpfTau);
|
||||
_residual_lpf[i].update(fmaxf(current, 0.f) - i_expected);
|
||||
|
||||
// Each band (a threshold <= 0 disables it) must hold for its trigger time (Hysteresis); the decision is then latched.
|
||||
const float residual_lpf = _residual_lpf[i].getState();
|
||||
const bool over = (_cfg.overcurrent_threshold_a > 0.f) && (residual_lpf >= _cfg.overcurrent_threshold_a);
|
||||
const bool under = (_cfg.undercurrent_threshold_a > 0.f) && (-residual_lpf >= _cfg.undercurrent_threshold_a);
|
||||
_hyst_over[i].set_state_and_update(over, now_us);
|
||||
_hyst_under[i].set_state_and_update(under, now_us);
|
||||
|
||||
if (_failure[i] == Failure::None) {
|
||||
if (_hyst_under[i].get_state()) {
|
||||
_failure[i] = Failure::Undercurrent;
|
||||
|
||||
} else if (_hyst_over[i].get_state()) {
|
||||
_failure[i] = Failure::Overcurrent;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,109 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 the low-passed
|
||||
* residual holds past a trip band for the trigger time.
|
||||
*
|
||||
* Band and trigger time are per direction: benign transients are much larger on the
|
||||
* overcurrent side, so one shared band makes the undercurrent side -- where a stopped motor
|
||||
* or a lost prop shows up -- inherit a limit the fault itself never reaches.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/hysteresis/hysteresis.h>
|
||||
#include <mathlib/math/filter/AlphaFilter.hpp>
|
||||
|
||||
class MotorFailureDetector
|
||||
{
|
||||
public:
|
||||
// Same as esc_status CONNECTED_ESC_MAX, hardcoded to keep the library uORB-free.
|
||||
static constexpr int kMaxMotors = 12;
|
||||
|
||||
// Step longer than this [s] => data gap (resets filters + debounce).
|
||||
// Equal to the ESC-offline timeout: every ESC that is still online gets evaluated.
|
||||
static constexpr float kMaxGap = 0.4f;
|
||||
|
||||
// Time constant of the residual low-pass filter [s]
|
||||
static constexpr float kResidualLpfTau = 0.2f;
|
||||
|
||||
// One model for all motors
|
||||
struct Config {
|
||||
float current_slope_a; ///< expected current per unit thrust command [A]
|
||||
float current_idle_a; ///< expected current at zero command [A]
|
||||
float overcurrent_threshold_a; ///< trip band above expected current [A]; <= 0 disables this band
|
||||
float overcurrent_trigger_time_s; ///< time above the overcurrent band before latching [s]
|
||||
float undercurrent_threshold_a; ///< trip band below expected current [A]; <= 0 disables this band
|
||||
float undercurrent_trigger_time_s; ///< time below the undercurrent band before latching [s]
|
||||
};
|
||||
|
||||
enum class Failure : uint8_t { None, Undercurrent, Overcurrent };
|
||||
|
||||
MotorFailureDetector() { reset(); }
|
||||
|
||||
/** Set the model, bands and trigger times. Keeps filter, debounce and latch state. */
|
||||
void configure(const Config &cfg);
|
||||
|
||||
/** Reset all per-motor state (filters, debounce, latch). */
|
||||
void reset();
|
||||
|
||||
/**
|
||||
* Advance motor i to its sample timestamp now_us [us] (dt derived per motor; a repeated
|
||||
* timestamp is the same sample again and is ignored). A non-finite command or current is not
|
||||
* evaluated (the model only describes forward thrust, so pass NAN for a reversible motor);
|
||||
* neither is a step > kMaxGap, e.g. after the motor was not updated for a while. Both reset
|
||||
* filter and debounce, the latch is kept. configure() first.
|
||||
*/
|
||||
void update(int i, hrt_abstime now_us, float command, float current);
|
||||
|
||||
/** Failure of motor i, latched at the first trip and kept until reset(). */
|
||||
Failure failure(int i) const { return validIndex(i) ? _failure[i] : Failure::None; }
|
||||
|
||||
/** Low-pass filtered residual I - I_expected of motor i [A]. */
|
||||
float residualFiltered(int i) const { return validIndex(i) ? _residual_lpf[i].getState() : 0.f; }
|
||||
|
||||
private:
|
||||
static bool validIndex(int i) { return 0 <= i && i < kMaxMotors; }
|
||||
|
||||
Config _cfg{};
|
||||
Failure _failure[kMaxMotors] {};
|
||||
AlphaFilter<float> _residual_lpf[kMaxMotors];
|
||||
systemlib::Hysteresis _hyst_over[kMaxMotors];
|
||||
systemlib::Hysteresis _hyst_under[kMaxMotors];
|
||||
hrt_abstime _last_us[kMaxMotors] {};
|
||||
};
|
||||
@@ -0,0 +1,455 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
{
|
||||
using Failure = MotorFailureDetector::Failure;
|
||||
|
||||
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: current_slope_a*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.current_slope_a = 21.f;
|
||||
cfg.overcurrent_threshold_a = 5.f;
|
||||
cfg.overcurrent_trigger_time_s = 0.5f;
|
||||
cfg.undercurrent_threshold_a = 5.f;
|
||||
cfg.undercurrent_trigger_time_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)
|
||||
{
|
||||
for (float t = 0.f; t < seconds; t += dt) {
|
||||
now += us(dt);
|
||||
d.update(0, now, u, current_fn(t));
|
||||
}
|
||||
}
|
||||
|
||||
// Sustained open failure at fixed dt for >> trigger time; did it latch? Isolates the kMaxGap boundary.
|
||||
bool latchesAtCadence(float dt)
|
||||
{
|
||||
MotorFailureDetector d;
|
||||
d.configure(makeConfig()); // trigger time 0.5 s
|
||||
hrt_abstime now = 0;
|
||||
run(d, now, 3.f, dt, 0.3f, [](float) { return 0.f; }); // 3 s >> trigger time
|
||||
return d.failure(0) != Failure::None;
|
||||
}
|
||||
} // 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_EQ(d.failure(0), Failure::None);
|
||||
EXPECT_LT(std::fabs(d.residualFiltered(0)), 0.1f);
|
||||
}
|
||||
|
||||
// Commanded high but current collapses to ~0 -> trips after the trigger time.
|
||||
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_EQ(d.failure(0), Failure::Undercurrent);
|
||||
EXPECT_LT(d.residualFiltered(0), -5.f);
|
||||
}
|
||||
|
||||
// A tighter undercurrent band catches a current collapse that the (wider) overcurrent band alone
|
||||
// misses -- the point of splitting: the fault side is not held hostage by overcurrent transients.
|
||||
TEST(MotorFailureDetectorTest, UndercurrentBandCatchesWhatSymmetricMisses)
|
||||
{
|
||||
MotorFailureDetector::Config cfg = makeConfig();
|
||||
cfg.overcurrent_threshold_a = 8.f; // wider than the fault the motor can produce
|
||||
cfg.undercurrent_threshold_a = 8.f; // symmetric: one band for both directions
|
||||
const auto half_current = [](float) { return kBase * 0.5f; }; // residual ~ -3.15 A
|
||||
|
||||
MotorFailureDetector symmetric;
|
||||
symmetric.configure(cfg);
|
||||
hrt_abstime now = 0;
|
||||
run(symmetric, now, 3.f, kDt, 0.3f, half_current);
|
||||
ASSERT_EQ(symmetric.failure(0), Failure::None); // one wide band cannot see it
|
||||
|
||||
cfg.undercurrent_threshold_a = 2.f;
|
||||
cfg.undercurrent_trigger_time_s = 0.5f;
|
||||
MotorFailureDetector asymmetric;
|
||||
asymmetric.configure(cfg);
|
||||
now = 0;
|
||||
run(asymmetric, now, 3.f, kDt, 0.3f, half_current);
|
||||
EXPECT_EQ(asymmetric.failure(0), Failure::Undercurrent);
|
||||
}
|
||||
|
||||
// The bands are independent: an overcurrent excursion below the (wide) overcurrent band must not
|
||||
// trip just because the tight undercurrent band would have caught the same magnitude.
|
||||
TEST(MotorFailureDetectorTest, UndercurrentBandDoesNotArmTheOvercurrentSide)
|
||||
{
|
||||
MotorFailureDetector::Config cfg = makeConfig();
|
||||
cfg.overcurrent_threshold_a = 8.f;
|
||||
cfg.undercurrent_threshold_a = 2.f;
|
||||
cfg.undercurrent_trigger_time_s = 0.5f;
|
||||
|
||||
MotorFailureDetector d;
|
||||
d.configure(cfg);
|
||||
hrt_abstime now = 0;
|
||||
run(d, now, 3.f, kDt, 0.3f, [](float) { return kBase + 4.f; }); // +4 A: over the under band, under the over band
|
||||
EXPECT_EQ(d.failure(0), Failure::None);
|
||||
EXPECT_GT(d.residualFiltered(0), 2.f);
|
||||
}
|
||||
|
||||
// Each side keeps its own trigger time: a collapse latches on the undercurrent time, not the
|
||||
// overcurrent one.
|
||||
TEST(MotorFailureDetectorTest, PerSideTriggerTime)
|
||||
{
|
||||
MotorFailureDetector::Config cfg = makeConfig();
|
||||
cfg.overcurrent_trigger_time_s = 5.f; // far longer than the run below
|
||||
cfg.undercurrent_threshold_a = 2.f;
|
||||
cfg.undercurrent_trigger_time_s = 0.2f;
|
||||
|
||||
MotorFailureDetector d;
|
||||
d.configure(cfg);
|
||||
hrt_abstime now = 0;
|
||||
run(d, now, 1.f, kDt, 0.3f, [](float) { return 0.f; });
|
||||
EXPECT_EQ(d.failure(0), Failure::Undercurrent); // latched on 0.2 s, not 5 s
|
||||
}
|
||||
|
||||
// A fault that crosses the threshold but recovers before the trigger time 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 trigger time
|
||||
ASSERT_GT(std::fabs(d.residualFiltered(0)), makeConfig().overcurrent_threshold_a); // genuinely over-threshold...
|
||||
run(d, now, 3.f, kDt, 0.3f, [](float) { return kBase; }); // ...then recovers before latching
|
||||
EXPECT_EQ(d.failure(0), Failure::None);
|
||||
}
|
||||
|
||||
// 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_EQ(d.failure(0), Failure::Overcurrent);
|
||||
EXPECT_GT(d.residualFiltered(0), 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_EQ(d.failure(0), Failure::Undercurrent);
|
||||
EXPECT_LT(d.residualFiltered(0), -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_EQ(d.failure(0), Failure::None);
|
||||
EXPECT_GT(std::fabs(d.residualFiltered(0)), 0.f); // residual present...
|
||||
EXPECT_LT(std::fabs(d.residualFiltered(0)), makeConfig().overcurrent_threshold_a); // ...but below trip
|
||||
}
|
||||
|
||||
// A descending vehicle windmills its props and the ESCs regenerate, reporting negative current --
|
||||
// more negative than any fault can be, since a dead motor draws 0 A. The residual floor keeps that
|
||||
// from latching where the band is out of reach, while a stop, whose residual sits exactly at the
|
||||
// floor, still latches wherever the band is reachable at all.
|
||||
TEST(MotorFailureDetectorTest, RegenerationBoundedButStopStillTrips)
|
||||
{
|
||||
MotorFailureDetector::Config cfg = makeConfig();
|
||||
cfg.undercurrent_threshold_a = 4.f; // above I_exp at u = 0.15 (3.15 A)
|
||||
cfg.undercurrent_trigger_time_s = 0.5f;
|
||||
|
||||
MotorFailureDetector regen;
|
||||
regen.configure(cfg);
|
||||
hrt_abstime now = 0;
|
||||
run(regen, now, 3.f, kDt, 0.15f, [](float) { return -5.f; }); // current flowing back into the pack
|
||||
EXPECT_EQ(regen.failure(0), Failure::None);
|
||||
// Floored at -I_exp (-3.15 A), not at the raw -8.15 A that would have crossed the band.
|
||||
EXPECT_GE(regen.residualFiltered(0), -cfg.current_slope_a * 0.15f - 0.01f);
|
||||
|
||||
MotorFailureDetector stopped;
|
||||
stopped.configure(cfg);
|
||||
now = 0;
|
||||
run(stopped, now, 3.f, kDt, 0.3f, [](float) { return 0.f; }); // I_exp 6.3 A, band reachable
|
||||
EXPECT_EQ(stopped.failure(0), Failure::Undercurrent);
|
||||
}
|
||||
|
||||
// A data gap mid-build must RESET the trigger-time debounce, not just avoid latching in one step.
|
||||
TEST(MotorFailureDetectorTest, GapResetsTriggerTimer)
|
||||
{
|
||||
MotorFailureDetector d;
|
||||
d.configure(makeConfig()); // trigger time 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_EQ(d.failure(0), Failure::None);
|
||||
|
||||
now += us(0.5f); // > kMaxGap -> gap resets filters + debounce
|
||||
d.update(0, now, 0.3f, 0.f);
|
||||
|
||||
run(d, now, 0.45f, kDt, 0.3f, [](float) { return 0.f; }); // rebuild from zero, < 0.5 s -> must NOT latch
|
||||
EXPECT_EQ(d.failure(0), Failure::None);
|
||||
}
|
||||
|
||||
// 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). An outage longer than kMaxGap
|
||||
// (the ESC-offline timeout) still resets.
|
||||
TEST(MotorFailureDetectorTest, TenHzFeedLatchesButRealGapStillResets)
|
||||
{
|
||||
static_assert(MotorFailureDetector::kMaxGap > 0.1f, "kMaxGap must exceed the ~10 Hz feed interval");
|
||||
|
||||
EXPECT_TRUE(latchesAtCadence(0.105f)); // 10 Hz feed
|
||||
EXPECT_TRUE(latchesAtCadence(0.35f)); // slow but online ESC
|
||||
EXPECT_FALSE(latchesAtCadence(0.5f)); // outage
|
||||
}
|
||||
|
||||
// Exclusion gate: a NaN command (motor off, or reversible) is never evaluated.
|
||||
TEST(MotorFailureDetectorTest, ExcludedGate)
|
||||
{
|
||||
MotorFailureDetector d;
|
||||
d.configure(makeConfig());
|
||||
hrt_abstime now = 0;
|
||||
run(d, now, 5.f, kDt, NAN, [](float) { return 150.f; });
|
||||
EXPECT_EQ(d.failure(0), Failure::None);
|
||||
EXPECT_EQ(d.residualFiltered(0), 0.f); // excluded: reset every step
|
||||
}
|
||||
|
||||
// A threshold <= 0 disables that band (a default-constructed Config disables both): never latches.
|
||||
TEST(MotorFailureDetectorTest, DisabledBandNeverLatches)
|
||||
{
|
||||
MotorFailureDetector unconfigured; // zero Config
|
||||
hrt_abstime now0 = 0;
|
||||
run(unconfigured, now0, 5.f, kDt, 0.5f, [](float) { return 123.f; });
|
||||
EXPECT_EQ(unconfigured.failure(0), Failure::None);
|
||||
|
||||
MotorFailureDetector::Config cfg = makeConfig();
|
||||
cfg.overcurrent_threshold_a = 0.f;
|
||||
MotorFailureDetector over_off;
|
||||
over_off.configure(cfg);
|
||||
hrt_abstime now = 0;
|
||||
run(over_off, now, 5.f, kDt, 0.3f, [](float) { return 2.5f * kBase; }); // overcurrent
|
||||
EXPECT_EQ(over_off.failure(0), Failure::None);
|
||||
|
||||
cfg = makeConfig();
|
||||
cfg.undercurrent_threshold_a = 0.f;
|
||||
MotorFailureDetector under_off;
|
||||
under_off.configure(cfg);
|
||||
now = 0;
|
||||
run(under_off, now, 5.f, kDt, 0.3f, [](float) { return 0.f; }); // undercurrent
|
||||
EXPECT_EQ(under_off.failure(0), Failure::None);
|
||||
}
|
||||
|
||||
// A non-finite current cannot be evaluated: like a NaN command it resets filter + debounce.
|
||||
TEST(MotorFailureDetectorTest, NonFiniteCurrentNotEvaluated)
|
||||
{
|
||||
MotorFailureDetector d;
|
||||
d.configure(makeConfig()); // trigger time 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_EQ(d.failure(0), Failure::None);
|
||||
run(d, now, 1.0f, kDt, 0.3f, [](float) { return NAN; }); // like a NaN command: reset, no latch
|
||||
EXPECT_EQ(d.failure(0), Failure::None);
|
||||
EXPECT_EQ(d.residualFiltered(0), 0.f);
|
||||
}
|
||||
|
||||
// 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()); // trigger time 0.5 s
|
||||
hrt_abstime now = 0;
|
||||
d.update(0, now, 0.3f, 0.f); // seed the clock
|
||||
now += us(10.f); // 10 s >> kMaxGap, single step -> gap, no latch
|
||||
d.update(0, now, 0.3f, 0.f);
|
||||
EXPECT_EQ(d.failure(0), Failure::None);
|
||||
}
|
||||
|
||||
// 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 (< trigger time)
|
||||
run(d, now, 1.f, kDt, NAN, [](float) { return 0.f; }); // excluded (motor commanded off)
|
||||
ASSERT_EQ(d.residualFiltered(0), 0.f); // excluded: reset every step
|
||||
run(d, now, 3.f, kDt, 0.3f, [](float) { return kBase; }); // healthy again
|
||||
EXPECT_EQ(d.failure(0), Failure::None);
|
||||
EXPECT_LT(std::fabs(d.residualFiltered(0)), 0.5f); // reseeded, not stale
|
||||
}
|
||||
|
||||
// The latch outlives exclusion. Handling a failure makes that motor unevaluable -- dropped from
|
||||
// allocation or driven reversible, both a NAN command -- and that must not retract its own trigger.
|
||||
TEST(MotorFailureDetectorTest, LatchSurvivesExclusion)
|
||||
{
|
||||
MotorFailureDetector d;
|
||||
d.configure(makeConfig());
|
||||
hrt_abstime now = 0;
|
||||
run(d, now, 3.f, kDt, 0.3f, [](float) { return 0.f; }); // latch
|
||||
ASSERT_EQ(d.failure(0), Failure::Undercurrent);
|
||||
run(d, now, 1.f, kDt, NAN, [](float) { return kBase; });
|
||||
EXPECT_EQ(d.residualFiltered(0), 0.f); // excluded: reset every step
|
||||
EXPECT_EQ(d.failure(0), Failure::Undercurrent);
|
||||
}
|
||||
|
||||
// reset() clears the sticky failure latch (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_EQ(d.failure(0), Failure::Undercurrent);
|
||||
d.reset();
|
||||
EXPECT_EQ(d.failure(0), Failure::None);
|
||||
}
|
||||
|
||||
// configure() (parameter change, possibly in flight) keeps the latch; only reset() clears it.
|
||||
TEST(MotorFailureDetectorTest, ConfigureKeepsLatch)
|
||||
{
|
||||
MotorFailureDetector d;
|
||||
d.configure(makeConfig());
|
||||
hrt_abstime now = 0;
|
||||
run(d, now, 3.f, kDt, 0.3f, [](float) { return 0.f; });
|
||||
ASSERT_EQ(d.failure(0), Failure::Undercurrent);
|
||||
d.configure(makeConfig());
|
||||
EXPECT_EQ(d.failure(0), Failure::Undercurrent);
|
||||
}
|
||||
|
||||
// Each motor keeps its own clock: one that is not updated for > kMaxGap restarts clean, even while
|
||||
// the others keep being updated (a caller skips motors it has no data for).
|
||||
TEST(MotorFailureDetectorTest, SkippedMotorRestartsClean)
|
||||
{
|
||||
MotorFailureDetector d;
|
||||
d.configure(makeConfig()); // trigger time 0.5 s
|
||||
hrt_abstime now = 0;
|
||||
|
||||
for (float t = 0.f; t < 0.45f; t += kDt) { // fault building on motor 0, not latched
|
||||
now += us(kDt);
|
||||
d.update(0, now, 0.3f, 0.f);
|
||||
d.update(1, now, 0.3f, kBase);
|
||||
}
|
||||
|
||||
for (float t = 0.f; t < 0.5f; t += kDt) { // motor 0 skipped > kMaxGap, motor 1 continues
|
||||
now += us(kDt);
|
||||
d.update(1, now, 0.3f, kBase);
|
||||
}
|
||||
|
||||
for (float t = 0.f; t < 0.45f; t += kDt) { // motor 0 resumes: rebuild from zero, < 0.5 s
|
||||
now += us(kDt);
|
||||
d.update(0, now, 0.3f, 0.f);
|
||||
}
|
||||
|
||||
EXPECT_EQ(d.failure(0), Failure::None);
|
||||
}
|
||||
|
||||
// A repeated sample timestamp is the same ESC sample again (esc_status republished for another ESC):
|
||||
// it is ignored, whatever command and current come with it.
|
||||
TEST(MotorFailureDetectorTest, RepeatedSampleIgnored)
|
||||
{
|
||||
MotorFailureDetector d;
|
||||
d.configure(makeConfig());
|
||||
hrt_abstime now = 0;
|
||||
run(d, now, 0.2f, kDt, 0.3f, [](float) { return 0.f; }); // fault building, residual far from 0
|
||||
const float residual_before = d.residualFiltered(0);
|
||||
ASSERT_LT(residual_before, -1.f);
|
||||
|
||||
for (int k = 0; k < 1000; ++k) { d.update(0, now, NAN, 0.f); } // same timestamp, would otherwise reset
|
||||
|
||||
EXPECT_EQ(d.failure(0), Failure::None);
|
||||
EXPECT_EQ(d.residualFiltered(0), residual_before);
|
||||
}
|
||||
|
||||
// Motors latch independently.
|
||||
TEST(MotorFailureDetectorTest, MultiMotorIndependent)
|
||||
{
|
||||
MotorFailureDetector d;
|
||||
d.configure(makeConfig());
|
||||
const float current[3] = {kBase, 0.f, 0.f}; // 0 healthy, 1 & 2 open
|
||||
hrt_abstime now = 0;
|
||||
|
||||
for (float t = 0.f; t < 3.f; t += kDt) {
|
||||
now += us(kDt);
|
||||
|
||||
for (int i = 0; i < 3; ++i) { d.update(i, now, 0.3f, current[i]); }
|
||||
}
|
||||
|
||||
EXPECT_EQ(d.failure(0), Failure::None);
|
||||
EXPECT_EQ(d.failure(1), Failure::Undercurrent);
|
||||
EXPECT_EQ(d.failure(2), Failure::Undercurrent);
|
||||
}
|
||||
|
||||
// The failure kind is the first band that tripped: a later excursion to the other side does not
|
||||
// overwrite it, so the report stays consistent for the rest of the flight.
|
||||
TEST(MotorFailureDetectorTest, FailureKindKeepsFirstTrip)
|
||||
{
|
||||
MotorFailureDetector d;
|
||||
d.configure(makeConfig());
|
||||
hrt_abstime now = 0;
|
||||
run(d, now, 3.f, kDt, 0.3f, [](float) { return 0.f; }); // undercurrent latches
|
||||
ASSERT_EQ(d.failure(0), Failure::Undercurrent);
|
||||
run(d, now, 3.f, kDt, 0.3f, [](float) { return 2.5f * kBase; }); // then sustained overcurrent
|
||||
ASSERT_GT(d.residualFiltered(0), 5.f);
|
||||
EXPECT_EQ(d.failure(0), Failure::Undercurrent);
|
||||
}
|
||||
@@ -0,0 +1,244 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 EscLogReader.hpp
|
||||
*
|
||||
* Shared ULog input of the MotorFailureDetector host tools. Reads esc_status, actuator_motors and
|
||||
* vehicle_status by field name from the log's own format, so one binary handles logs of any PX4
|
||||
* message version, and hands out ESC samples shaped the way escCheck.cpp feeds the detector.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "MotorFailureDetector.hpp"
|
||||
|
||||
#include <ulog_cpp/data_container.hpp>
|
||||
#include <ulog_cpp/exception.hpp>
|
||||
#include <ulog_cpp/reader.hpp>
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <cfloat>
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
/** One ESC slot's sample in an esc_status record, as escCheck would feed it to the detector. */
|
||||
struct EscSample {
|
||||
int slot; ///< ESC slot, the detector index
|
||||
int motor; ///< 0-based motor index (actuator_function - MOTOR1), selects the command
|
||||
uint64_t timestamp_us; ///< the ESC's own sample time
|
||||
bool fresh; ///< first record carrying this sample (esc_status repeats ESCs that did not report)
|
||||
float command; ///< latest actuator_motors control of that motor, NAN if it is reversible (like escCheck)
|
||||
float current; ///< [A]
|
||||
};
|
||||
|
||||
class EscLogReader : public ulog_cpp::DataContainer
|
||||
{
|
||||
public:
|
||||
static constexpr int kMaxSlots = MotorFailureDetector::kMaxMotors;
|
||||
|
||||
EscLogReader() : ulog_cpp::DataContainer(ulog_cpp::DataContainer::StorageConfig::Header) {}
|
||||
|
||||
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 = std::min(_esc_array_f->arrayLength(), kMaxSlots);
|
||||
_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_slot_timestamp_f = _esc_array_f->nestedField("timestamp");
|
||||
|
||||
} else if (msg.messageName() == "actuator_motors") {
|
||||
_actuator_id = msg.msgId();
|
||||
_actuator_fmt = it->second;
|
||||
_actuator_control_f = _actuator_fmt->field("control");
|
||||
_control_channels = std::min(_actuator_control_f->arrayLength(), kMaxSlots);
|
||||
_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 data(const ulog_cpp::Data &record) override
|
||||
{
|
||||
const uint16_t id = record.msgId();
|
||||
|
||||
if (id == _status_id) {
|
||||
const bool armed = ulog_cpp::TypedDataView(record, *_status_fmt)[_status_arming_f].as<int>() == kArmed;
|
||||
|
||||
if (_armed && !armed) { disarmed(); }
|
||||
|
||||
_armed = armed;
|
||||
return;
|
||||
}
|
||||
|
||||
if (id == _actuator_id) {
|
||||
ulog_cpp::TypedDataView view(record, *_actuator_fmt);
|
||||
const uint32_t reversible_mask = view[_actuator_reversible_f].as<uint32_t>();
|
||||
const auto control = view[_actuator_control_f];
|
||||
|
||||
for (int m = 0; m < _control_channels; ++m) {
|
||||
_command_latest[m] = control[m].as<float>();
|
||||
_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<int>() <= 0) { return; } // escCheck skips esc_status with esc_count <= 0
|
||||
|
||||
if (_have_status && !_armed) { return; } // escCheck only runs the detector while armed
|
||||
|
||||
std::array<EscSample, kMaxSlots> samples;
|
||||
int count = 0;
|
||||
const auto esc_arr = view[_esc_array_f];
|
||||
|
||||
for (int slot = 0; slot < _esc_slots; ++slot) {
|
||||
const auto esc = esc_arr[slot];
|
||||
const int function = esc[_esc_function_f].as<int>();
|
||||
|
||||
if (function < kMotor1 || function - kMotor1 >= kMaxSlots) { continue; }
|
||||
|
||||
const float current = esc[_esc_current_f].as<float>();
|
||||
|
||||
// Like escCheck: an ESC is not judged until it has reported current once.
|
||||
if (current > FLT_EPSILON) { _has_reported_current[slot] = true; }
|
||||
|
||||
if (!_has_reported_current[slot]) { continue; }
|
||||
|
||||
const int motor = function - kMotor1;
|
||||
const uint64_t timestamp_us = esc[_esc_slot_timestamp_f].as<uint64_t>();
|
||||
const float command = _reversible_latest[motor] ? NAN : _command_latest[motor];
|
||||
samples[count++] = {slot, motor, timestamp_us, timestamp_us != _last_sample_us[slot], command, current};
|
||||
_last_sample_us[slot] = timestamp_us;
|
||||
}
|
||||
|
||||
escRecord(view[_esc_timestamp_f].as<uint64_t>(), samples.data(), count);
|
||||
}
|
||||
|
||||
bool haveStatus() const { return _have_status; }
|
||||
|
||||
protected:
|
||||
/** An armed esc_status record: its publication time and the samples of every ESC escCheck would feed. */
|
||||
virtual void escRecord(uint64_t record_us, const EscSample *samples, int count) = 0;
|
||||
|
||||
/** The vehicle disarmed, where escCheck resets the detector. */
|
||||
virtual void disarmed() {}
|
||||
|
||||
private:
|
||||
static constexpr int kMotor1 = 101; // esc_report::ACTUATOR_FUNCTION_MOTOR1
|
||||
static constexpr int kArmed = 2; // vehicle_status::ARMING_STATE_ARMED
|
||||
|
||||
bool _armed = false;
|
||||
bool _have_status = false;
|
||||
std::array<float, kMaxSlots> _command_latest {}; // 0 until an actuator_motors arrives, like the runtime
|
||||
std::array<bool, kMaxSlots> _reversible_latest {};
|
||||
std::array<bool, kMaxSlots> _has_reported_current {}; // per ESC slot, never reset (like escCheck)
|
||||
std::array<uint64_t, kMaxSlots> _last_sample_us {}; // per ESC slot
|
||||
|
||||
// Each message's id and layout, and handles to the fields read, resolved once from the log's legend.
|
||||
uint16_t _esc_id = 0xffff; // 0xffff = message type not seen yet
|
||||
uint16_t _actuator_id = 0xffff;
|
||||
uint16_t _status_id = 0xffff;
|
||||
std::shared_ptr<ulog_cpp::MessageFormat> _esc_fmt;
|
||||
std::shared_ptr<ulog_cpp::MessageFormat> _actuator_fmt;
|
||||
std::shared_ptr<ulog_cpp::MessageFormat> _status_fmt;
|
||||
int _esc_slots = 0;
|
||||
int _control_channels = 0;
|
||||
|
||||
std::shared_ptr<ulog_cpp::Field> _esc_timestamp_f;
|
||||
std::shared_ptr<ulog_cpp::Field> _esc_array_f;
|
||||
std::shared_ptr<ulog_cpp::Field> _esc_count_f;
|
||||
std::shared_ptr<ulog_cpp::Field> _esc_function_f;
|
||||
std::shared_ptr<ulog_cpp::Field> _esc_current_f;
|
||||
std::shared_ptr<ulog_cpp::Field> _esc_slot_timestamp_f;
|
||||
std::shared_ptr<ulog_cpp::Field> _actuator_control_f;
|
||||
std::shared_ptr<ulog_cpp::Field> _actuator_reversible_f;
|
||||
std::shared_ptr<ulog_cpp::Field> _status_arming_f;
|
||||
};
|
||||
|
||||
/** Stream a whole log through reader. Returns false on a fatal error (already reported to stderr). */
|
||||
inline bool readLog(const char *ulog_path, const std::shared_ptr<EscLogReader> &reader)
|
||||
{
|
||||
FILE *file = fopen(ulog_path, "rb");
|
||||
|
||||
if (!file) { std::fprintf(stderr, "cannot open %s\n", ulog_path); return false; }
|
||||
|
||||
std::array<uint8_t, 4096> buf;
|
||||
int bytes_read;
|
||||
ulog_cpp::Reader ulog_reader{reader};
|
||||
|
||||
while ((bytes_read = fread(buf.data(), 1, buf.size(), file)) > 0) {
|
||||
try {
|
||||
ulog_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 false;
|
||||
}
|
||||
|
||||
if (reader->hadFatalError()) { break; }
|
||||
}
|
||||
|
||||
fclose(file);
|
||||
|
||||
if (reader->hadFatalError()) { std::fprintf(stderr, "fatal parse error in %s\n", ulog_path); return false; }
|
||||
|
||||
return true;
|
||||
}
|
||||
+31
@@ -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 <log-dir> [--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 <log-dir> [--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"
|
||||
@@ -0,0 +1,218 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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_fit.cpp
|
||||
*
|
||||
* Fits the MotorFailureDetector current model, I_expected = MOTFAIL_C2T * u + MOTFAIL_IDLE, from
|
||||
* recorded .ulg files, and prints the parameter values to set.
|
||||
*
|
||||
* Reads the logs through EscLogReader like mfd_replay. Every log is a single streaming pass that only
|
||||
* accumulates the six sums a least-squares line needs, so memory is constant and unrelated to log size
|
||||
* or corpus size: pass the whole corpus at once and the fit is pooled over all of it, which is what
|
||||
* the detector wants (one model shared by every motor).
|
||||
*/
|
||||
|
||||
#include "EscLogReader.hpp"
|
||||
|
||||
#include <array>
|
||||
#include <cfloat>
|
||||
#include <cmath>
|
||||
#include <cstdio>
|
||||
#include <memory>
|
||||
|
||||
namespace
|
||||
{
|
||||
constexpr int kMM = MotorFailureDetector::kMaxMotors;
|
||||
|
||||
/**
|
||||
* Sums for a least-squares fit of I = slope * u + idle.
|
||||
*
|
||||
* Only these six numbers are needed, so they can be accumulated per motor and pooled by addition;
|
||||
* no sample is ever stored.
|
||||
*/
|
||||
struct Sums {
|
||||
double n{0};
|
||||
double u{0};
|
||||
double uu{0};
|
||||
double i{0};
|
||||
double ui{0};
|
||||
double ii{0};
|
||||
|
||||
void add(double command, double current)
|
||||
{
|
||||
n += 1.0;
|
||||
u += command;
|
||||
uu += command * command;
|
||||
i += current;
|
||||
ui += command * current;
|
||||
ii += current * current;
|
||||
}
|
||||
|
||||
void operator+=(const Sums &o)
|
||||
{
|
||||
n += o.n; u += o.u; uu += o.uu; i += o.i; ui += o.ui; ii += o.ii;
|
||||
}
|
||||
|
||||
/** Solve for the line. Returns false if the commands span too little range to define a slope. */
|
||||
bool solve(double &slope, double &idle) const
|
||||
{
|
||||
const double det = n * uu - u * u;
|
||||
|
||||
if (n < 2.0 || std::fabs(det) < 1e-9) { return false; }
|
||||
|
||||
slope = (n * ui - u * i) / det;
|
||||
idle = (i - slope * u) / n;
|
||||
return true;
|
||||
}
|
||||
|
||||
/** Standard deviation of the residual about the given line (the spread the trip bands must clear). */
|
||||
double residualStdDev(double slope, double idle) const
|
||||
{
|
||||
const double sq = ii - 2.0 * slope * ui - 2.0 * idle * i
|
||||
+ slope * slope * uu + 2.0 * slope * idle * u + idle * idle * n;
|
||||
return (n > 1.0 && sq > 0.0) ? std::sqrt(sq / n) : 0.0;
|
||||
}
|
||||
};
|
||||
|
||||
class Fit : public EscLogReader
|
||||
{
|
||||
public:
|
||||
bool seen(int m) const { return _seen[m]; }
|
||||
const Sums &motor(int m) const { return _per_motor[m]; }
|
||||
|
||||
protected:
|
||||
void escRecord(uint64_t /*record_us*/, const EscSample *samples, int count) override
|
||||
{
|
||||
for (int k = 0; k < count; ++k) {
|
||||
const EscSample &s = samples[k];
|
||||
|
||||
// Count each ESC sample once. A motor the allocation switched off or in reversible mode (NAN),
|
||||
// or an ESC reporting no current describes nothing about the healthy forward model.
|
||||
if (!s.fresh || !std::isfinite(s.current) || s.current <= FLT_EPSILON || !std::isfinite(s.command)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
_seen[s.motor] = true;
|
||||
_per_motor[s.motor].add(s.command, s.current);
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
std::array<Sums, kMM> _per_motor {};
|
||||
std::array<bool, kMM> _seen {};
|
||||
};
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
const char *usage = "usage: %s <log.ulg> [<log.ulg> ...]\n"
|
||||
" Fits I_expected = MOTFAIL_C2T * u + MOTFAIL_IDLE, pooled over every log given.\n";
|
||||
|
||||
if (argc < 2) { std::fprintf(stderr, usage, argv[0]); return 2; }
|
||||
|
||||
std::array<Sums, kMM> per_motor {};
|
||||
std::array<bool, kMM> seen {};
|
||||
int logs_used = 0;
|
||||
|
||||
for (int arg = 1; arg < argc; ++arg) {
|
||||
if (argv[arg][0] == '-') { std::fprintf(stderr, usage, argv[0]); return 2; }
|
||||
|
||||
auto fit = std::make_shared<Fit>();
|
||||
|
||||
if (!readLog(argv[arg], fit)) { continue; } // reported; keep going so one bad log does not lose the corpus
|
||||
|
||||
bool any = false;
|
||||
|
||||
for (int m = 0; m < kMM; ++m) {
|
||||
if (!fit->seen(m)) { continue; }
|
||||
|
||||
seen[m] = true;
|
||||
any = true;
|
||||
per_motor[m] += fit->motor(m);
|
||||
}
|
||||
|
||||
if (any) { ++logs_used; }
|
||||
|
||||
else { std::fprintf(stderr, "%s: no armed ESC current samples\n", argv[arg]); }
|
||||
}
|
||||
|
||||
if (logs_used == 0) { std::fprintf(stderr, "no usable logs\n"); return 2; }
|
||||
|
||||
Sums pooled;
|
||||
|
||||
for (int m = 0; m < kMM; ++m) {
|
||||
if (seen[m]) { pooled += per_motor[m]; }
|
||||
}
|
||||
|
||||
double slope = 0.0;
|
||||
double idle = 0.0;
|
||||
|
||||
if (!pooled.solve(slope, idle)) {
|
||||
std::fprintf(stderr, "commands span too little range to fit a slope (was the vehicle flying?)\n");
|
||||
return 2;
|
||||
}
|
||||
|
||||
const double residual_sd = pooled.residualStdDev(slope, idle);
|
||||
const double mean_command = pooled.u / pooled.n;
|
||||
const double mean_current = pooled.i / pooled.n;
|
||||
|
||||
std::printf("# %d log%s, %.0f armed samples\n", logs_used, logs_used == 1 ? "" : "s", pooled.n);
|
||||
std::printf("# I_expected = %.2f*u + %.2f A (residual sigma %.2f A)\n", slope, idle, residual_sd);
|
||||
std::printf("# mean command %.3f, mean current %.2f A\n", mean_command, mean_current);
|
||||
std::printf("\nMOTFAIL_C2T %.2f\nMOTFAIL_IDLE %.2f\n", slope, idle);
|
||||
|
||||
if (idle < 0.0) {
|
||||
std::printf("\n# NOTE: the fitted offset is negative, and MOTFAIL_IDLE cannot be set below 0.\n"
|
||||
"# Use MOTFAIL_IDLE 0; an offset this small is within the residual anyway.\n");
|
||||
}
|
||||
|
||||
// Per-motor bias against the shared model: how much a single model costs each motor. Large spread
|
||||
// here is the only argument for not sharing one model, and it comes straight out of the same sums.
|
||||
std::printf("\n# per-motor mean residual against this model [A]\n");
|
||||
double worst = 0.0;
|
||||
|
||||
for (int m = 0; m < kMM; ++m) {
|
||||
if (!seen[m]) { continue; }
|
||||
|
||||
const Sums &s = per_motor[m];
|
||||
const double bias = s.i / s.n - (slope * (s.u / s.n) + idle);
|
||||
// 1-based, matching the "Motor N ..." the vehicle reports; the arrays stay 0-based.
|
||||
std::printf("# motor %d: %+.2f (sigma %.2f, n %.0f)\n", m + 1, bias, s.residualStdDev(slope, idle), s.n);
|
||||
worst = std::fmax(worst, std::fabs(bias));
|
||||
}
|
||||
|
||||
std::printf("# worst per-motor bias %.2f A -- the trip bands have to clear this before any fault.\n", worst);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,407 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 each motor's
|
||||
* residual extremes + trip fraction -- a single-phase verdict is only a lower bound.
|
||||
*/
|
||||
|
||||
#include "EscLogReader.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cfloat>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace
|
||||
{
|
||||
constexpr int kMM = MotorFailureDetector::kMaxMotors;
|
||||
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 (phase-swept):
|
||||
// clean across the reference multirotor corpus. Illustrative for this airframe, not a shipped default.
|
||||
MotorFailureDetector::Config makeConfig()
|
||||
{
|
||||
MotorFailureDetector::Config cfg{};
|
||||
cfg.current_slope_a = 34.8f;
|
||||
cfg.current_idle_a = 2.88f;
|
||||
cfg.overcurrent_threshold_a = 5.0f;
|
||||
cfg.overcurrent_trigger_time_s = 0.4f;
|
||||
cfg.undercurrent_threshold_a = 5.0f;
|
||||
cfg.undercurrent_trigger_time_s = 0.4f;
|
||||
return cfg;
|
||||
}
|
||||
|
||||
// MOTFAIL_* params -> Config fields, mirroring escCheck.cpp.
|
||||
struct ParamBind {
|
||||
const char *name;
|
||||
float MotorFailureDetector::Config::*field;
|
||||
};
|
||||
|
||||
constexpr std::array kParamBinds {
|
||||
ParamBind{"MOTFAIL_C2T", &MotorFailureDetector::Config::current_slope_a},
|
||||
ParamBind{"MOTFAIL_IDLE", &MotorFailureDetector::Config::current_idle_a},
|
||||
ParamBind{"MOTFAIL_OVER", &MotorFailureDetector::Config::overcurrent_threshold_a},
|
||||
ParamBind{"MOTFAIL_OVR_TIME", &MotorFailureDetector::Config::overcurrent_trigger_time_s},
|
||||
ParamBind{"MOTFAIL_UNDER", &MotorFailureDetector::Config::undercurrent_threshold_a},
|
||||
ParamBind{"MOTFAIL_UND_TIME", &MotorFailureDetector::Config::undercurrent_trigger_time_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 EscLogReader
|
||||
{
|
||||
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<bool, kNumParams> &cli_set)
|
||||
: _cli_cfg(cli_cfg), _cli_set(cli_set)
|
||||
{
|
||||
for (auto &phase : _trip_time) { phase.fill(-1.0); }
|
||||
|
||||
for (auto &phase : _rmin) { phase.fill(0.f); }
|
||||
|
||||
for (auto &phase : _rmax) { phase.fill(0.f); }
|
||||
}
|
||||
|
||||
// Results per ESC slot, read after the whole log has been parsed.
|
||||
bool seen(int i) const { return _seen[i]; }
|
||||
int motor(int i) const { return _motor[i]; } ///< 0-based motor index the slot was last mapped to
|
||||
float residualMin(int phase, int i) const { return _rmin[phase][i]; }
|
||||
float residualMax(int phase, int i) const { return _rmax[phase][i]; }
|
||||
double tripTime(int phase, int i) const { return _trip_time[phase][i]; }
|
||||
int paramsFromLog() const { return _params_from_log; }
|
||||
long evaluated() const { return _evaluated; }
|
||||
const MotorFailureDetector::Config &config() const { return _cfg; }
|
||||
|
||||
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<float>());
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
protected:
|
||||
void disarmed() override
|
||||
{
|
||||
for (int p = 0; p < kNumPhases; ++p) {
|
||||
_det[p].reset();
|
||||
_next_eval_us[p] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void escRecord(uint64_t record_us, const EscSample *samples, int count) override
|
||||
{
|
||||
for (int k = 0; k < count; ++k) {
|
||||
_seen[samples[k].slot] = true;
|
||||
_motor[samples[k].slot] = samples[k].motor;
|
||||
}
|
||||
|
||||
// One parse, kNumPhases detectors: feed each phase whose 10 Hz grid this record 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] = record_us + (uint64_t)p * kRuntimeTickUs / kNumPhases;
|
||||
}
|
||||
|
||||
if (record_us < _next_eval_us[p]) { continue; }
|
||||
|
||||
_next_eval_us[p] += kRuntimeTickUs;
|
||||
|
||||
if (_start_us < 0) { _start_us = (double)record_us; }
|
||||
|
||||
const double elapsed_s = ((double)record_us - _start_us) / 1e6;
|
||||
|
||||
for (int k = 0; k < count; ++k) {
|
||||
const EscSample &s = samples[k];
|
||||
_det[p].update(s.slot, s.timestamp_us, s.command, s.current);
|
||||
|
||||
const float residual_lpf = _det[p].residualFiltered(s.slot);
|
||||
|
||||
if (!std::isnan(residual_lpf)) {
|
||||
_rmin[p][s.slot] = std::min(_rmin[p][s.slot], residual_lpf);
|
||||
_rmax[p][s.slot] = std::max(_rmax[p][s.slot], residual_lpf);
|
||||
}
|
||||
|
||||
if (_det[p].failure(s.slot) != MotorFailureDetector::Failure::None && _trip_time[p][s.slot] < 0) {
|
||||
_trip_time[p][s.slot] = elapsed_s;
|
||||
}
|
||||
}
|
||||
|
||||
if (p == 0) { ++_evaluated; }
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
MotorFailureDetector::Config _cli_cfg {};
|
||||
std::array<bool, kNumParams> _cli_set {};
|
||||
|
||||
// One detector instance per sampling phase; a single parse feeds all of them.
|
||||
std::array<MotorFailureDetector, kNumPhases> _det;
|
||||
MotorFailureDetector::Config _cfg = makeConfig();
|
||||
std::array<bool, kNumParams> _log_set {};
|
||||
int _params_from_log = 0;
|
||||
bool _configured = false;
|
||||
|
||||
std::array<std::array<float, kMM>, kNumPhases> _rmin;
|
||||
std::array<std::array<float, kMM>, kNumPhases> _rmax;
|
||||
std::array<std::array<double, kMM>, kNumPhases> _trip_time; // -1 until a trip (filled in the ctor)
|
||||
std::array<bool, kMM> _seen {}; // per ESC slot
|
||||
std::array<int, kMM> _motor {}; // per ESC slot
|
||||
double _start_us = -1.0;
|
||||
long _evaluated = 0; // phase-0 evaluation count (== per-phase sample count)
|
||||
std::array<uint64_t, kNumPhases> _next_eval_us {}; // per-phase next 10 Hz evaluation time (runtime-cadence match)
|
||||
};
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
const char *usage = "usage: %s <log.ulg> [--set MOTFAIL_NAME=value ...]\n";
|
||||
const char *ulog_path = nullptr;
|
||||
MotorFailureDetector::Config cli_cfg{};
|
||||
std::array<bool, kNumParams> 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 = std::make_shared<Replay>(cli_cfg, cli_set);
|
||||
|
||||
if (!readLog(ulog_path, replay)) { return 2; }
|
||||
|
||||
// Aggregate the per-phase results into each ESC's residual extremes + trip fraction. The
|
||||
// envelope (deepest and highest any phase saw) is reported; the per-phase spread of each extreme
|
||||
// is what says whether the answer depends on sampling.
|
||||
std::array<bool, kMM> seen_any{};
|
||||
std::array<float, kMM> res_min;
|
||||
std::array<float, kMM> res_max;
|
||||
std::array<float, kMM> res_min_shallowest;
|
||||
std::array<float, kMM> res_max_lowest;
|
||||
std::array<int, kMM> trip_count{};
|
||||
|
||||
res_min.fill(0.f);
|
||||
res_max.fill(0.f);
|
||||
res_min_shallowest.fill(-FLT_MAX);
|
||||
res_max_lowest.fill(FLT_MAX);
|
||||
|
||||
for (int i = 0; i < kMM; ++i) {
|
||||
if (!replay->seen(i)) { continue; }
|
||||
|
||||
seen_any[i] = true;
|
||||
|
||||
for (int p = 0; p < kNumPhases; ++p) {
|
||||
res_min[i] = std::min(res_min[i], replay->residualMin(p, i));
|
||||
res_max[i] = std::max(res_max[i], replay->residualMax(p, i));
|
||||
res_min_shallowest[i] = std::max(res_min_shallowest[i], replay->residualMin(p, i));
|
||||
res_max_lowest[i] = std::min(res_max_lowest[i], replay->residualMax(p, i));
|
||||
|
||||
if (replay->tripTime(p, i) >= 0) { ++trip_count[i]; }
|
||||
}
|
||||
}
|
||||
|
||||
for (const std::string &err : replay->parsingErrors()) { std::fprintf(stderr, "parse: %s\n", err.c_str()); }
|
||||
|
||||
bool any_motor = false;
|
||||
|
||||
for (int i = 0; i < kMM; ++i) { any_motor |= seen_any[i]; }
|
||||
|
||||
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<char, 160> 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::array<char, 48> under_desc {};
|
||||
std::array<char, 48> over_desc {};
|
||||
|
||||
if (cfg.undercurrent_threshold_a > 0.f) {
|
||||
std::snprintf(under_desc.data(), under_desc.size(), "< -%.2f for %.2f s",
|
||||
(double)cfg.undercurrent_threshold_a, (double)cfg.undercurrent_trigger_time_s);
|
||||
|
||||
} else {
|
||||
std::snprintf(under_desc.data(), under_desc.size(), "undercurrent off");
|
||||
}
|
||||
|
||||
if (cfg.overcurrent_threshold_a > 0.f) {
|
||||
std::snprintf(over_desc.data(), over_desc.size(), "> %.2f for %.2f s",
|
||||
(double)cfg.overcurrent_threshold_a, (double)cfg.overcurrent_trigger_time_s);
|
||||
|
||||
} else {
|
||||
std::snprintf(over_desc.data(), over_desc.size(), "overcurrent off");
|
||||
}
|
||||
|
||||
std::printf("# %s (%d phases, %ld %s)\n"
|
||||
"# config (%s): I_exp = %.2f*u + %.2f A, trip if LPF(I-I_exp) %s or %s\n",
|
||||
ulog_path, kNumPhases, replay->evaluated(), count_label, config_source.data(),
|
||||
(double)cfg.current_slope_a, (double)cfg.current_idle_a, under_desc.data(), over_desc.data());
|
||||
std::printf("# %-5s %8s %8s %-5s %s\n", "motor", "min[A]", "max[A]", "trips", "verdict");
|
||||
|
||||
bool any_failed = false;
|
||||
bool phase_dependent = false;
|
||||
|
||||
for (int i = 0; i < kMM; ++i) {
|
||||
if (!seen_any[i]) { continue; }
|
||||
|
||||
const char *verdict;
|
||||
|
||||
if (trip_count[i] == kNumPhases) {
|
||||
verdict = "FAILED (all phases)";
|
||||
any_failed = true;
|
||||
|
||||
} else if (trip_count[i] > 0) {
|
||||
verdict = "FAILED (sampling-dependent!)";
|
||||
any_failed = true;
|
||||
phase_dependent = true;
|
||||
|
||||
} else {
|
||||
verdict = "ok";
|
||||
}
|
||||
|
||||
// One side moving by >30% between phases means the sampling grid, not the flight, picked the answer.
|
||||
if (std::fabs(res_min[i]) > 1.3f * std::fabs(res_min_shallowest[i])
|
||||
|| res_max[i] > 1.3f * res_max_lowest[i]) {
|
||||
phase_dependent = true;
|
||||
}
|
||||
|
||||
std::array<char, 16> trips_str {};
|
||||
std::snprintf(trips_str.data(), trips_str.size(), "%d/%d", trip_count[i], kNumPhases);
|
||||
// 1-based, matching the "Motor N ..." the vehicle reports; the arrays stay 0-based.
|
||||
std::printf(" %5d %+8.2f %+8.2f %-5s %s\n",
|
||||
replay->motor(i) + 1, (double)res_min[i], (double)res_max[i], 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;
|
||||
}
|
||||
+57
@@ -0,0 +1,57 @@
|
||||
#!/usr/bin/env bash
|
||||
# Sweep one trip band against its trigger time and print the result as a matrix: each cell is how
|
||||
# many logs false-trip at that (trigger time, band) pair, so the first 0 in a row is the lowest band
|
||||
# that trigger time supports. The other direction is disabled, so each side is measured on its own.
|
||||
#
|
||||
# Usage: C2T=<slope> IDLE=<offset> sweep_matrix.sh <log-dir> under|over
|
||||
# env TRIGGER_TIMES="0.5 0.7 1.0" trigger times, one per row
|
||||
# env BANDS="1.5 1.75 2.0" bands, one per column -- scale these to the vehicle, the
|
||||
# defaults below suit a multirotor drawing a few amps per
|
||||
# motor in hover
|
||||
# env MFD_REPLAY=... binary location, as for batch_replay.sh
|
||||
#
|
||||
# Run from the repository root. Fit the model first (mfd_fit); the bands are deviations from it.
|
||||
set -uo pipefail
|
||||
|
||||
DIR="${1:?usage: C2T=.. IDLE=.. sweep_matrix.sh <log-dir> under|over}"
|
||||
SIDE="${2:-}"
|
||||
: "${C2T:?set C2T to the fitted MOTFAIL_C2T}"
|
||||
: "${IDLE:?set IDLE to the fitted MOTFAIL_IDLE}"
|
||||
|
||||
HERE="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
BATCH="$HERE/batch_replay.sh"
|
||||
|
||||
case "$SIDE" in
|
||||
under)
|
||||
read -ra TRIGGER_TIMES <<<"${TRIGGER_TIMES:-0.1 0.3 0.5 0.7 1.0}"
|
||||
read -ra BANDS <<<"${BANDS:-1.0 1.25 1.5 1.75 2.0 2.5 3.0}"
|
||||
OTHER_OFF=(--set MOTFAIL_OVER=0); BAND_P=MOTFAIL_UNDER; TRIGGER_TIME_P=MOTFAIL_UND_TIME ;;
|
||||
over)
|
||||
read -ra TRIGGER_TIMES <<<"${TRIGGER_TIMES:-0.4 0.7 1.0 1.5 2.5}"
|
||||
read -ra BANDS <<<"${BANDS:-0.5 1.0 2.0 4.0 6.0 8.0}"
|
||||
OTHER_OFF=(--set MOTFAIL_UNDER=0); BAND_P=MOTFAIL_OVER; TRIGGER_TIME_P=MOTFAIL_OVR_TIME ;;
|
||||
*)
|
||||
echo "error: side must be 'under' or 'over', got '$SIDE'" >&2; exit 2 ;;
|
||||
esac
|
||||
|
||||
echo "# I_exp = ${C2T}*u + ${IDLE} A, sweeping $BAND_P over $DIR"
|
||||
printf "%-11s" "trigger[s]"; printf "%7s" "${BANDS[@]}"; echo " <- $BAND_P [A] (logs tripped)"
|
||||
|
||||
for trigger_time in "${TRIGGER_TIMES[@]}"; do
|
||||
printf "%-11s" "$trigger_time"
|
||||
|
||||
for band in "${BANDS[@]}"; do
|
||||
out=$("$BATCH" "$DIR" --set MOTFAIL_C2T="$C2T" --set MOTFAIL_IDLE="$IDLE" \
|
||||
"${OTHER_OFF[@]}" --set "$BAND_P=$band" --set "$TRIGGER_TIME_P=$trigger_time")
|
||||
|
||||
# Without a single evaluated log (no summary, empty directory, or every log skipped) the
|
||||
# cell would read as "no trips".
|
||||
ok=$(sed -n 's/.* OK=\([0-9]*\).*/\1/p' <<<"$out")
|
||||
fail=$(sed -n 's/.* FAIL=\([0-9]*\).*/\1/p' <<<"$out")
|
||||
(( ${ok:-0} + ${fail:-0} > 0 )) || { echo " (no log evaluated -- check the log directory)"; exit 9; }
|
||||
|
||||
printf "%7s" "$fail"
|
||||
done
|
||||
|
||||
echo
|
||||
done
|
||||
Submodule
+1
Submodule src/lib/ulog_cpp added at af366a91cb.
@@ -83,7 +83,7 @@ px4_add_library(health_and_arming_checks ${health_and_arming_checks_srcs})
|
||||
|
||||
set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/esc_check_params.yaml)
|
||||
add_dependencies(health_and_arming_checks mode_util)
|
||||
target_link_libraries(health_and_arming_checks PRIVATE geo hysteresis px4_platform)
|
||||
target_link_libraries(health_and_arming_checks PRIVATE geo hysteresis px4_platform MotorFailureDetector)
|
||||
|
||||
px4_add_functional_gtest(SRC HealthAndArmingChecksTest.cpp
|
||||
LINKLIBS health_and_arming_checks mode_util
|
||||
|
||||
@@ -88,7 +88,10 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
|
||||
if (_param_fd_act_en.get() > 0) {
|
||||
updateEscsStatus(context, reporter, esc_status, now);
|
||||
mask |= checkMotorStatus(context, reporter, esc_status, now);
|
||||
mask |= checkMotorStatus(context, reporter, esc_status);
|
||||
|
||||
} else {
|
||||
_motor_failure_detector.reset();
|
||||
}
|
||||
|
||||
if (_param_esc_temp_warn_th.get() >= FLT_EPSILON) {
|
||||
@@ -272,27 +275,33 @@ void EscChecks::checkEscTemperature(Report &reporter, const esc_status_s &esc_st
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t EscChecks::checkMotorStatus(const Context &context, Report &reporter, const esc_status_s &esc_status, hrt_abstime now)
|
||||
uint16_t EscChecks::checkMotorStatus(const Context &context, Report &reporter, const esc_status_s &esc_status)
|
||||
{
|
||||
uint16_t mask = 0;
|
||||
|
||||
// Only check while armed
|
||||
if (context.status().arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
|
||||
actuator_motors_s actuator_motors{};
|
||||
_actuator_motors_sub.copy(&actuator_motors);
|
||||
|
||||
// Check individual ESC reports
|
||||
for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) {
|
||||
// The detector is indexed by ESC slot, so every ESC is checked even if several share a motor function.
|
||||
// The motor index (actuator_function - MOTOR1) selects the command and is what gets reported.
|
||||
static_assert(esc_status_s::CONNECTED_ESC_MAX <= MotorFailureDetector::kMaxMotors, "ESC slots exceed kMaxMotors");
|
||||
static_assert(esc_report_s::ACTUATOR_FUNCTION_MOTOR_MAX - esc_report_s::ACTUATOR_FUNCTION_MOTOR1
|
||||
< actuator_motors_s::NUM_CONTROLS, "ESC motor-function range exceeds actuator_motors controls");
|
||||
static_assert((hrt_abstime)(MotorFailureDetector::kMaxGap * 1e6f) == ESC_OFFLINE_TIMEOUT_US,
|
||||
"an ESC that is online but too slow for the detector would silently not be checked");
|
||||
|
||||
for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) {
|
||||
if (!math::isInRange(esc_status.esc[i].actuator_function,
|
||||
esc_report_s::ACTUATOR_FUNCTION_MOTOR1, esc_report_s::ACTUATOR_FUNCTION_MOTOR_MAX)) {
|
||||
continue; // Skip unmapped ESC status entries
|
||||
}
|
||||
|
||||
const uint8_t actuator_function_index = esc_status.esc[i].actuator_function - esc_report_s::ACTUATOR_FUNCTION_MOTOR1;
|
||||
const float current = esc_status.esc[i].esc_current;
|
||||
const uint8_t motor = esc_status.esc[i].actuator_function - esc_report_s::ACTUATOR_FUNCTION_MOTOR1;
|
||||
const float measured_current = esc_status.esc[i].esc_current;
|
||||
|
||||
// First wait for ESC telemetry reporting non-zero current. Before that happens, don't check it.
|
||||
if (current > FLT_EPSILON) {
|
||||
// Wait for the ESC to start reporting current before judging it.
|
||||
if (measured_current > FLT_EPSILON) {
|
||||
_esc_has_reported_current[i] = true;
|
||||
}
|
||||
|
||||
@@ -300,34 +309,22 @@ uint16_t EscChecks::checkMotorStatus(const Context &context, Report &reporter, c
|
||||
continue;
|
||||
}
|
||||
|
||||
// Current limits
|
||||
float thrust = 0.f;
|
||||
// The current model only describes forward thrust: a reversible motor is not evaluated.
|
||||
const bool reversible = (actuator_motors.reversible_flags >> motor) & 1;
|
||||
const float command = reversible ? NAN : actuator_motors.control[motor];
|
||||
|
||||
if (PX4_ISFINITE(actuator_motors.control[actuator_function_index])) {
|
||||
// Normalized motor thrust commands before thrust model factor is applied, NAN means motor is turned off -> 0 thrust
|
||||
thrust = fabsf(actuator_motors.control[actuator_function_index]);
|
||||
// Drive the detector off this ESC's own sample time: esc_status is published whenever any ESC
|
||||
// reports, so the other slots may still hold an older sample, which must not count again.
|
||||
_motor_failure_detector.update(i, esc_status.esc[i].timestamp, command, measured_current);
|
||||
|
||||
// The latch outlives handling the failure, which stops evaluating its own motor (NAN command).
|
||||
const MotorFailureDetector::Failure failure = _motor_failure_detector.failure(i);
|
||||
|
||||
if (failure != MotorFailureDetector::Failure::None) {
|
||||
mask |= (1u << motor);
|
||||
}
|
||||
|
||||
bool current_too_low = current < (thrust * _param_motfail_c2t.get()) - _param_motfail_off.get();
|
||||
bool current_too_high = current > (thrust * _param_motfail_c2t.get()) + _param_motfail_off.get();
|
||||
|
||||
_esc_undercurrent_hysteresis[i].set_hysteresis_time_from(false, _param_motfail_time.get() * 1_s);
|
||||
_esc_overcurrent_hysteresis[i].set_hysteresis_time_from(false, _param_motfail_time.get() * 1_s);
|
||||
|
||||
if (!_esc_undercurrent_hysteresis[i].get_state()) {
|
||||
// Only set, never clear mid-air: stopping the motor in response could make it appear healthy again
|
||||
_esc_undercurrent_hysteresis[i].set_state_and_update(current_too_low, now);
|
||||
}
|
||||
|
||||
if (!_esc_overcurrent_hysteresis[i].get_state()) {
|
||||
// Only set, never clear mid-air: stopping the motor in response could make it appear healthy again
|
||||
_esc_overcurrent_hysteresis[i].set_state_and_update(current_too_high, now);
|
||||
}
|
||||
|
||||
mask |= (static_cast<uint16_t>(_esc_undercurrent_hysteresis[i].get_state()) << actuator_function_index);
|
||||
mask |= (static_cast<uint16_t>(_esc_overcurrent_hysteresis[i].get_state()) << actuator_function_index);
|
||||
|
||||
if (_esc_undercurrent_hysteresis[i].get_state()) {
|
||||
if (failure == MotorFailureDetector::Failure::Undercurrent) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
@@ -336,15 +333,13 @@ uint16_t EscChecks::checkMotorStatus(const Context &context, Report &reporter, c
|
||||
*/
|
||||
reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::motors_escs,
|
||||
events::ID("check_motor_undercurrent"),
|
||||
events::Log::Critical, "Motor {1} undercurrent detected", actuator_function_index + 1);
|
||||
events::Log::Critical, "Motor {1} undercurrent detected", motor + 1);
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Motor failure: Motor %d undercurrent detected",
|
||||
actuator_function_index + 1);
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Motor failure: Motor %d undercurrent detected", motor + 1);
|
||||
}
|
||||
}
|
||||
|
||||
if (_esc_overcurrent_hysteresis[i].get_state()) {
|
||||
} else if (failure == MotorFailureDetector::Failure::Overcurrent) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
@@ -353,25 +348,35 @@ uint16_t EscChecks::checkMotorStatus(const Context &context, Report &reporter, c
|
||||
*/
|
||||
reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::motors_escs,
|
||||
events::ID("check_motor_overcurrent"),
|
||||
events::Log::Critical, "Motor {1} overcurrent detected", actuator_function_index + 1);
|
||||
events::Log::Critical, "Motor {1} overcurrent detected", motor + 1);
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Motor failure: Motor %d overcurrent detected",
|
||||
actuator_function_index + 1);
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Motor failure: Motor %d overcurrent detected", motor + 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} else { // Disarmed
|
||||
for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) {
|
||||
_esc_undercurrent_hysteresis[i].set_state_and_update(false, now);
|
||||
_esc_overcurrent_hysteresis[i].set_state_and_update(false, now);
|
||||
}
|
||||
} else {
|
||||
_motor_failure_detector.reset();
|
||||
}
|
||||
|
||||
return mask;
|
||||
}
|
||||
|
||||
void EscChecks::updateParams()
|
||||
{
|
||||
HealthAndArmingCheckBase::updateParams();
|
||||
|
||||
MotorFailureDetector::Config cfg{};
|
||||
cfg.current_slope_a = _param_motfail_c2t.get();
|
||||
cfg.current_idle_a = _param_motfail_idle.get();
|
||||
cfg.overcurrent_threshold_a = _param_motfail_over.get();
|
||||
cfg.overcurrent_trigger_time_s = _param_motfail_ovr_time.get();
|
||||
cfg.undercurrent_threshold_a = _param_motfail_under.get();
|
||||
cfg.undercurrent_trigger_time_s = _param_motfail_und_time.get();
|
||||
_motor_failure_detector.configure(cfg);
|
||||
}
|
||||
|
||||
void EscChecks::updateEscsStatus(const Context &context, Report &reporter, const esc_status_s &esc_status, hrt_abstime now)
|
||||
{
|
||||
if (context.status().arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
|
||||
|
||||
@@ -36,6 +36,7 @@
|
||||
#include "../Common.hpp"
|
||||
|
||||
#include <lib/hysteresis/hysteresis.h>
|
||||
#include <lib/motor_failure_detector/MotorFailureDetector.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/actuator_motors.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
@@ -48,6 +49,7 @@ public:
|
||||
~EscChecks() = default;
|
||||
|
||||
void checkAndReport(const Context &context, Report &reporter) override;
|
||||
void updateParams() override;
|
||||
|
||||
uint16_t getMotorFailureMask() const { return _motor_failure_mask; }
|
||||
bool getEscArmStatus() const { return _esc_arm_hysteresis.get_state(); }
|
||||
@@ -62,7 +64,7 @@ public:
|
||||
private:
|
||||
uint16_t checkEscOnline(const Context &context, Report &reporter, const esc_status_s &esc_status, hrt_abstime now);
|
||||
uint16_t checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status);
|
||||
uint16_t checkMotorStatus(const Context &context, Report &reporter, const esc_status_s &esc_status, hrt_abstime now);
|
||||
uint16_t checkMotorStatus(const Context &context, Report &reporter, const esc_status_s &esc_status);
|
||||
void updateEscsStatus(const Context &context, Report &reporter, const esc_status_s &esc_status, hrt_abstime now);
|
||||
void checkEscTemperature(Report &reporter, const esc_status_s &esc_status);
|
||||
|
||||
@@ -72,17 +74,19 @@ private:
|
||||
const hrt_abstime _start_time{hrt_absolute_time()};
|
||||
|
||||
uint16_t _motor_failure_mask = 0;
|
||||
bool _esc_has_reported_current[esc_status_s::CONNECTED_ESC_MAX] {};
|
||||
bool _esc_has_reported_current[esc_status_s::CONNECTED_ESC_MAX] {}; // true if ESC reported non-zero current before (some never report any)
|
||||
bool _esc_over_temp_warned{false};
|
||||
systemlib::Hysteresis _esc_undercurrent_hysteresis[esc_status_s::CONNECTED_ESC_MAX];
|
||||
systemlib::Hysteresis _esc_overcurrent_hysteresis[esc_status_s::CONNECTED_ESC_MAX];
|
||||
systemlib::Hysteresis _esc_arm_hysteresis;
|
||||
MotorFailureDetector _motor_failure_detector;
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
|
||||
(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_com_arm_chk_escs,
|
||||
(ParamBool<px4::params::FD_ACT_EN>) _param_fd_act_en,
|
||||
(ParamFloat<px4::params::MOTFAIL_C2T>) _param_motfail_c2t,
|
||||
(ParamFloat<px4::params::MOTFAIL_TIME>) _param_motfail_time,
|
||||
(ParamFloat<px4::params::MOTFAIL_OFF>) _param_motfail_off,
|
||||
(ParamFloat<px4::params::MOTFAIL_IDLE>) _param_motfail_idle,
|
||||
(ParamFloat<px4::params::MOTFAIL_OVER>) _param_motfail_over,
|
||||
(ParamFloat<px4::params::MOTFAIL_OVR_TIME>) _param_motfail_ovr_time,
|
||||
(ParamFloat<px4::params::MOTFAIL_UNDER>) _param_motfail_under,
|
||||
(ParamFloat<px4::params::MOTFAIL_UND_TIME>) _param_motfail_und_time,
|
||||
(ParamFloat<px4::params::ESC_TEMP_WARN_TH>) _param_esc_temp_warn_th);
|
||||
};
|
||||
@@ -39,6 +39,7 @@
|
||||
#include <px4_platform_common/param.h>
|
||||
#include <px4_platform_common/time.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/actuator_motors.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
|
||||
using namespace time_literals;
|
||||
@@ -59,12 +60,16 @@ public:
|
||||
const int32_t one = 1;
|
||||
param_set_no_notification(param_find("COM_ARM_CHK_ESCS"), &one);
|
||||
param_set_no_notification(param_find("FD_ACT_EN"), &one);
|
||||
|
||||
const float trigger_time = 0.2f; // keeps the motor failure tests short
|
||||
param_set_no_notification(param_find("MOTFAIL_UND_TIME"), &trigger_time);
|
||||
}
|
||||
|
||||
void TearDown() override
|
||||
{
|
||||
param_reset(param_find("COM_ARM_CHK_ESCS"));
|
||||
param_reset(param_find("FD_ACT_EN"));
|
||||
param_reset(param_find("MOTFAIL_UND_TIME"));
|
||||
}
|
||||
|
||||
// Publish a quad ESC status with every ESC fresh except `stale_index` whose last report is `age` old
|
||||
@@ -84,6 +89,44 @@ public:
|
||||
_esc_status_pub.publish(esc_status);
|
||||
}
|
||||
|
||||
// Fly armed for `duration` of simulated time: the motor failure detector runs on the ESC sample
|
||||
// timestamps, so they advance in kTick steps without waiting. All motors are commanded kCommand,
|
||||
// each ESC reports the current its motor draws at the default model (MOTFAIL_C2T * kCommand),
|
||||
// except ESC `failed_index` reporting 0 A. `functions` maps the ESC slots to motors,
|
||||
// `reversible_flags` is published per motor.
|
||||
void fly(hrt_abstime duration, int failed_index, uint16_t reversible_flags = 0,
|
||||
const uint8_t (&functions)[4] = kQuadFunctions)
|
||||
{
|
||||
if (_esc_time == 0) { _esc_time = hrt_absolute_time(); }
|
||||
|
||||
for (hrt_abstime t = 0; t < duration; t += kTick) {
|
||||
_esc_time += kTick;
|
||||
|
||||
actuator_motors_s actuator_motors{};
|
||||
actuator_motors.timestamp = _esc_time;
|
||||
actuator_motors.reversible_flags = reversible_flags;
|
||||
|
||||
for (int i = 0; i < 4; i++) { actuator_motors.control[i] = kCommand; }
|
||||
|
||||
_actuator_motors_pub.publish(actuator_motors);
|
||||
|
||||
esc_status_s esc_status{};
|
||||
esc_status.timestamp = _esc_time;
|
||||
esc_status.esc_count = 4;
|
||||
esc_status.esc_online_flags = 0b1111;
|
||||
esc_status.esc_armed_flags = 0b1111;
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
esc_status.esc[i].timestamp = esc_status.timestamp - i; // each ESC reports at its own time
|
||||
esc_status.esc[i].actuator_function = functions[i];
|
||||
esc_status.esc[i].esc_current = (i == failed_index) ? 0.f : kHealthyCurrent;
|
||||
}
|
||||
|
||||
_esc_status_pub.publish(esc_status);
|
||||
runCheck(true);
|
||||
}
|
||||
}
|
||||
|
||||
void runCheck(bool armed = false)
|
||||
{
|
||||
vehicle_status_s status{};
|
||||
@@ -100,7 +143,15 @@ public:
|
||||
_health_error_escs = reporter.healthResults().error & health_component_t::motors_escs;
|
||||
}
|
||||
|
||||
static constexpr hrt_abstime kTick = 50_ms;
|
||||
static constexpr float kCommand = 0.5f;
|
||||
static constexpr float kHealthyCurrent = 35.f * kCommand; // MOTFAIL_C2T default, MOTFAIL_IDLE 0
|
||||
static constexpr uint8_t kMotor1 = esc_report_s::ACTUATOR_FUNCTION_MOTOR1;
|
||||
static constexpr uint8_t kQuadFunctions[4] {kMotor1, kMotor1 + 1, kMotor1 + 2, kMotor1 + 3};
|
||||
|
||||
uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
|
||||
uORB::Publication<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
|
||||
hrt_abstime _esc_time{0}; // simulated ESC sample time, starts at the real time of the first fly()
|
||||
failsafe_flags_s _failsafe_flags{};
|
||||
bool _can_arm{true};
|
||||
bool _health_error_escs{false};
|
||||
@@ -167,3 +218,47 @@ TEST_F(EscChecksTest, EscOfflineIgnoredWithChecksDisabled)
|
||||
EXPECT_EQ(_check.getMotorFailureMask(), 0u);
|
||||
EXPECT_FALSE(_failsafe_flags.fd_motor_failure);
|
||||
}
|
||||
|
||||
// A motor whose current collapses while commanded is flagged once it stays under the band for the
|
||||
// trigger time; the failure is latched until disarm.
|
||||
TEST_F(EscChecksTest, MotorUndercurrentLatchesUntilDisarm)
|
||||
{
|
||||
fly(200_ms, -1); // healthy: every ESC reports current, so every ESC gets judged
|
||||
ASSERT_EQ(_check.getMotorFailureMask(), 0u);
|
||||
|
||||
fly(600_ms, 2);
|
||||
EXPECT_EQ(_check.getMotorFailureMask(), 0b0100u);
|
||||
EXPECT_TRUE(_failsafe_flags.fd_motor_failure);
|
||||
EXPECT_TRUE(_health_error_escs);
|
||||
|
||||
fly(300_ms, -1); // current back: still latched
|
||||
EXPECT_EQ(_check.getMotorFailureMask(), 0b0100u);
|
||||
|
||||
publishWithStaleEsc(-1, 0);
|
||||
runCheck(false); // disarm resets the detector
|
||||
EXPECT_EQ(_check.getMotorFailureMask(), 0u);
|
||||
EXPECT_FALSE(_failsafe_flags.fd_motor_failure);
|
||||
|
||||
fly(200_ms, -1); // the next flight starts without the old failure
|
||||
EXPECT_EQ(_check.getMotorFailureMask(), 0u);
|
||||
EXPECT_FALSE(_failsafe_flags.fd_motor_failure);
|
||||
}
|
||||
|
||||
// The current model only describes forward thrust: a reversible motor is not judged.
|
||||
TEST_F(EscChecksTest, ReversibleMotorNotJudged)
|
||||
{
|
||||
fly(200_ms, -1);
|
||||
fly(600_ms, 2, 0b0100);
|
||||
EXPECT_EQ(_check.getMotorFailureMask(), 0u);
|
||||
EXPECT_FALSE(_failsafe_flags.fd_motor_failure);
|
||||
}
|
||||
|
||||
// Every ESC is judged on its own, also when two ESCs share a motor function.
|
||||
TEST_F(EscChecksTest, EscSharingMotorFunctionJudgedSeparately)
|
||||
{
|
||||
const uint8_t functions[4] {kMotor1, kMotor1 + 1, kMotor1 + 2, kMotor1 + 2}; // ESCs 3 and 4 both drive motor 3
|
||||
fly(200_ms, -1, 0, functions);
|
||||
fly(600_ms, 2, 0, functions); // the first of the two fails, the second stays healthy
|
||||
EXPECT_EQ(_check.getMotorFailureMask(), 0b0100u);
|
||||
EXPECT_TRUE(_failsafe_flags.fd_motor_failure);
|
||||
}
|
||||
@@ -6,19 +6,15 @@ parameters:
|
||||
description:
|
||||
short: Enable Actuator Failure check
|
||||
long: |-
|
||||
If enabled, the HealthAndArmingChecks will verify that for motors, a minimum amount of ESC current per throttle
|
||||
level is being consumed.
|
||||
Otherwise this indicates an motor failure.
|
||||
If enabled, the HealthAndArmingChecks compare each motor's ESC current against the expected
|
||||
current for the commanded throttle, and flag a motor whose current stays too far from that
|
||||
expectation (too low or too high) for a set time.
|
||||
This check only works for ESCs that report current consumption.
|
||||
type: boolean
|
||||
default: 0
|
||||
MOTFAIL_C2T:
|
||||
description:
|
||||
short: Motor Failure Current/Throttle Scale
|
||||
long: |-
|
||||
Determines the slope between expected steady state current and linearized, normalized thrust command.
|
||||
E.g. FD_ACT_MOT_C2T A represents the expected steady state current at 100%.
|
||||
FD_ACT_LOW_OFF and FD_ACT_HIGH_OFF offset the threshold from that slope.
|
||||
short: Motor failure model current-vs-command slope
|
||||
type: float
|
||||
default: 35.0
|
||||
min: 0.0
|
||||
@@ -26,10 +22,20 @@ parameters:
|
||||
unit: A/%
|
||||
decimal: 2
|
||||
increment: 1
|
||||
MOTFAIL_OFF:
|
||||
MOTFAIL_IDLE:
|
||||
description:
|
||||
short: Under/over-current motor failure limit offset
|
||||
long: Check is triggered if `abs(FD_ACT_MOT_C2T * thrust - FD_ACT_OFF)` is above this value.
|
||||
short: Motor failure model idle current
|
||||
type: float
|
||||
default: 0.0
|
||||
min: 0.0
|
||||
max: 20.0
|
||||
unit: A
|
||||
decimal: 2
|
||||
increment: 1
|
||||
MOTFAIL_OVER:
|
||||
description:
|
||||
short: Motor failure overcurrent trip band
|
||||
long: Set 0 to disable overcurrent detection.
|
||||
type: float
|
||||
default: 10.0
|
||||
min: 0
|
||||
@@ -37,11 +43,10 @@ parameters:
|
||||
unit: A
|
||||
decimal: 2
|
||||
increment: 1
|
||||
MOTFAIL_TIME:
|
||||
MOTFAIL_OVR_TIME:
|
||||
description:
|
||||
short: Motor Failure Hysteresis Time
|
||||
long: Motor failure only triggers after current thresholds are exceeded for
|
||||
this time.
|
||||
short: Motor failure overcurrent trigger time
|
||||
long: Motor failure only triggers after MOTFAIL_OVER is exceeded for this time.
|
||||
type: float
|
||||
default: 1.0
|
||||
unit: s
|
||||
@@ -49,6 +54,28 @@ parameters:
|
||||
max: 10
|
||||
decimal: 2
|
||||
increment: 1
|
||||
MOTFAIL_UNDER:
|
||||
description:
|
||||
short: Motor failure undercurrent trip band
|
||||
long: Set 0 to disable undercurrent detection.
|
||||
type: float
|
||||
default: 10.0
|
||||
min: 0
|
||||
max: 30
|
||||
unit: A
|
||||
decimal: 2
|
||||
increment: 1
|
||||
MOTFAIL_UND_TIME:
|
||||
description:
|
||||
short: Motor failure undercurrent trigger time
|
||||
long: Motor failure only triggers after MOTFAIL_UNDER is exceeded for this time.
|
||||
type: float
|
||||
default: 1.0
|
||||
unit: s
|
||||
min: 0
|
||||
max: 10
|
||||
decimal: 2
|
||||
increment: 1
|
||||
- group: ESC
|
||||
definitions:
|
||||
ESC_TEMP_WARN_TH:
|
||||
|
||||
Reference in new issue
Block a user