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

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.
+        GREEN: all nominal, 4 marginal parameters assessed.
-        AMBER: core navigation intact, <= 1 parameter marginal.
+        AMBER: core navigation intact, <= 1 of those 4 parameters marginal.
-        RED:   navigation sensor failure, >= 2 marginal, or any safety-critical lost.
+        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}%'