diff --git a/src/rov_control/rov_control/failsafe_monitor.py b/src/rov_control/rov_control/failsafe_monitor.py index 3e4a7d5..e35ca89 100644 --- a/src/rov_control/rov_control/failsafe_monitor.py +++ b/src/rov_control/rov_control/failsafe_monitor.py @@ -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 = # --- 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}%'