administrator/rov-autonomy
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix(failsafe): FC comms loss now forces unconditional RED per DIR-7/DIR-12

Browse Source
This commit is contained in:
Grant 2026-07-11 11:56:38 +02:00
parent 2add8c9e68
commit 86b272802f
1 changed files with 105 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
src/rov_control/rov_control/failsafe_monitor.py
View File

@ -18,7 +18,11 @@ Priority order (highest wins when multiple conditions active):
2. Critical/emergency battery
3. Safety depth limit (300m)
4. Obstacle detected in RED state
5. Comms loss (assessed response)
5. Comms loss (assessed response) — flag_comms_loss now forces
assessment_state straight to RED in _assess_conditions() per DIR-7/
DIR-12 (FC comms loss is an unconditional RED, not a negotiable
"marginal" parameter). This priority level therefore always takes the
RED branch of _handle_comms_loss() — see that method's docstring.
6. Manual abort (assessed response)
7. Thruster anomaly (AMBER downgrade)
8. Low battery warning (informational)
@ -121,6 +125,19 @@ class FailsafeMonitor(Node):
# Sensor readings
self.battery_percent = 1.0 # 0.0–1.0
self.fcu_connected = False
# DIR-7: "the assessment MUST incorporate flight-controller
# connectivity and heartbeat age. ... A monitor with no input MUST
# NOT report GREEN — absence of data is not health." fcu_connected
# alone only reflects the most recent /mavros/state message; it
# cannot tell "never received a heartbeat" apart from "was connected,
# heartbeat just ticked over". This flag latches True the first time
# msg.connected is True (see _state_callback) and never resets. It is
# used in _assess_conditions to force flag_comms_loss True for the
# entire startup window before any FC data has ever arrived — closing
# the gap where the node would otherwise report GREEN for up to
# comms_timeout_s with zero FC input — and in _publish_status to pick
# the correct cause text ("Vehicle Disconnected" vs "Heartbeat Lost").
self.fcu_ever_connected = False
self.current_depth_m = 0.0
# Timing
@ -215,6 +232,12 @@ class FailsafeMonitor(Node):
# Reset comms loss tracking on each received heartbeat
self.last_heartbeat = self.get_clock().now()
self.comms_loss_start = None
# DIR-7: latch that real FC data has arrived at least once. Set
# True here only, never reset back to False — distinguishes "FC
# has never connected" (-> "Vehicle Disconnected") from "FC
# connected and then dropped" (-> "Heartbeat Lost") for the
# cause text in _publish_status.
self.fcu_ever_connected = True
def _altitude_callback(self, msg: Float64):
"""
@ -253,15 +276,43 @@ class FailsafeMonitor(Node):
def _assess_conditions(self):
"""
Evaluate all monitored parameters and derive assessment state.
GREEN: all nominal, 5+ parameters assessed.
AMBER: core navigation intact, <= 1 parameter marginal.
RED: navigation sensor failure, >= 2 marginal, or any safety-critical lost.
GREEN: all nominal, 4 marginal parameters assessed.
AMBER: core navigation intact, <= 1 of those 4 parameters marginal.
RED: navigation sensor failure, >= 2 marginal, any safety-critical
lost, OR flag_comms_loss.
flag_comms_loss is deliberately NOT one of the 4 "marginal"
parameters below (it was previously a 5th equally-weighted marginal
parameter, which meant comms loss alone only reached AMBER). Per
DIR-7 ("A dead or unreachable FC is a RED condition, not optional")
and DIR-12 ("treat loss of FC communications as a vehicle-health RED
regardless of how healthy the autonomy stack is"), it is applied as
its own unconditional OR term in the RED branch instead — see below.
"""
now = self.get_clock().now()
# --- Comms ---
# DIR-7 (quoted in full): "the assessment MUST incorporate flight-
# controller connectivity and heartbeat age. A dead or unreachable FC
# is a RED condition, not optional. A monitor with no input MUST NOT
# report GREEN — absence of data is not health."
elapsed_s = (now - self.last_heartbeat).nanoseconds / 1e9
self.flag_comms_loss = elapsed_s > self._comms_timeout_s
# flag_comms_loss has two independent triggers, either sufficient on
# its own:
# 1. elapsed_s > comms_timeout_s — the normal loss-of-link case:
# the FC connected at some point and its heartbeat has since
# aged out.
# 2. not fcu_ever_connected — the startup-window case: no FC data
# has EVER arrived, so elapsed_s is only measuring time since
# node start (self.last_heartbeat is initialised at construction,
# not from a real heartbeat). Without this term the node would
# report GREEN for up to comms_timeout_s after boot despite
# having zero FC input the entire time — precisely the "monitor
# with no input MUST NOT report GREEN" case DIR-7 prohibits.
self.flag_comms_loss = (
elapsed_s > self._comms_timeout_s
or not self.fcu_ever_connected
)
if self.flag_comms_loss and self.comms_loss_start is None:
self.comms_loss_start = now
@ -287,15 +338,26 @@ class FailsafeMonitor(Node):
# self.flag_obstacle = <sonar_node data>
# --- Derive assessment state ---
# flag_comms_loss is excluded from marginal_count on purpose (see the
# docstring above and the DIR-7/DIR-12 quotes there) — it is no
# longer one of the equally-weighted "marginal" parameters that only
# reach RED in combination. Folding it back in here would let FC
# comms health be traded off against unrelated marginal parameters,
# or sit at a mere AMBER when alone, which is exactly what DIR-12
# forbids ("regardless of how healthy the autonomy stack is").
marginal_count = sum([
self.flag_comms_loss,
self.flag_low_battery,
self.flag_depth_exceeded,
self.flag_obstacle,
self.flag_thruster_anomaly,
])
if self.flag_emergency_battery or self.flag_depth_exceeded or marginal_count >= 2:
# DIR-7 + DIR-12: flag_comms_loss is its own unconditional OR term,
# separate from marginal_count >= 2, so a comms loss alone is always
# RED — never merely AMBER, never dependent on a second marginal
# parameter also being true.
if (self.flag_emergency_battery or self.flag_depth_exceeded
or self.flag_comms_loss or marginal_count >= 2):
self.assessment_state = AssessmentState.RED
elif marginal_count == 1:
self.assessment_state = AssessmentState.AMBER
@ -334,6 +396,17 @@ class FailsafeMonitor(Node):
"""
Apply failsafe conditions in strict priority order per design spec.
Higher priority conditions override lower priority ones.
Priority 5 (comms loss) note: _assess_conditions() now sets
assessment_state to RED whenever flag_comms_loss is True (DIR-7:
"A dead or unreachable FC is a RED condition, not optional"; DIR-12:
"treat loss of FC communications as a vehicle-health RED regardless
of how healthy the autonomy stack is"). So by the time this method
runs, a genuine comms loss always has assessment_state == RED, and
_handle_comms_loss() below always takes its RED branch (immediate
RETURN_TO_SAFE). Its GREEN/AMBER hold-and-retry branches are dead
code for that case — retained as a structural fallback rather than
deleted, but not expected to execute.
"""
now = self.get_clock().now()
@ -404,6 +477,16 @@ class FailsafeMonitor(Node):
GREEN: hold 60s then RETURN_TO_SAFE.
AMBER: hold 30s then RETURN_TO_SAFE.
RED: RETURN_TO_SAFE immediately.
DIR-7/DIR-12: flag_comms_loss now forces assessment_state to RED
directly in _assess_conditions() (DIR-7: "A dead or unreachable FC is
a RED condition, not optional"; DIR-12: "treat loss of FC
communications as a vehicle-health RED regardless of how healthy the
autonomy stack is"). This method is therefore only ever entered with
assessment_state == RED for a genuine comms loss, so it always takes
the RED branch immediately below. The GREEN/AMBER branches (60s/30s
hold before returning to safe) are no longer reachable for a genuine
comms loss — they are retained as a structural fallback only.
"""
if self.assessment_state == AssessmentState.RED:
self._transition(FSMState.RETURN_TO_SAFE,
@ -507,7 +590,22 @@ class FailsafeMonitor(Node):
FSMState.ABORTED: FailsafeStatus.ACTION_NONE,
}.get(self.fsm_state, FailsafeStatus.ACTION_NONE)
# DIR-7 (quoted in full): "The status line MUST carry cause text such
# as 'Vehicle Disconnected' or 'Heartbeat Lost', not a bare colour."
# Prefix the message with the specific comms-loss cause so an
# operator (or the W1 widget) can distinguish an FC that has NEVER
# connected (fcu_ever_connected False — likely wiring/power/board)
# from one that connected and then dropped (heartbeat aged out past
# comms_timeout_s).
cause_prefix = ''
if self.flag_comms_loss:
cause_prefix = (
'Vehicle Disconnected | ' if not self.fcu_ever_connected
else 'Heartbeat Lost | '
)
msg.message = (
f'{cause_prefix}'
f'Assessment: {self._assessment_state_name()} | '
f'State: {self._fsm_state_name(self.fsm_state)} | '
f'Battery: {self.battery_percent*100:.0f}%'