[youtalk]

What this does

This makes the VisionPilot heartbeat one more source of the SI supervisor from #66. The supervisor watches /safety_island/vp_heartbeat with a SourceWatch. A gap longer than source_timeout_heartbeat_s (0.5 s) latches SI_STOP in supervisorStaleReason(), the same path as every other source. The stop goes out through publishSiStop(), so it is an ApprovedRequest with a fault_id. Only the operator re-enable clears it.

The SI_STOP payload now ramps the speed target from the ego speed at the latch: v = max(0, v0 - decel * t). The acceleration demand stays -decel for the whole stop, so the ramp reaching 0 is never a brake release. HOLD keeps the plain standstill payload.

The X5H build compiles in two defaults: SI_DEFAULT_LEGACY_CONTROL_CMD="stop_only" and SI_DEFAULT_STOP_DECEL_MPS2=3.0f. The SI then stays silent on control_cmd until it stops the car, and it stops at 3.0 m/s².

Why

If VisionPilot stops talking to the safety island, the safety island must stop the vehicle. In the demo no trajectory arrives, so the heartbeat is the only sign that the driving stack is alive. The first revision of this pull request wrote its own stop command beside ApprovedRequest. That made two writers, and ApprovedRequest did not show the stop. This revision has one writer.

With the ramp, the speed target and the acceleration demand describe the same stop. A speed-tracking actuator, such as CARLA's Ackermann controller, follows the speed target and uses the acceleration only as a limit.

The X5H has no environment and loads no node parameters, so its two defaults are compiled in. The demo's command arbiter prefers any fresh CR52 command, so the SI must stay silent until it stops the car. The demo's stop gate (D6) was validated at 3.0 m/s², with a stop distance limit derived from that value. The shared default of 1.5 m/s² matches Autoware's moderate_stop_service_acceleration. Autoware's emergency values are 2.4 (vehicle_cmd_gate) and 2.5 (mrm_emergency_stop_operator).

Behaviour details

• The heartbeat is not stale before its first sample. A board that boots without VisionPilot never stops a car that nobody drives.
• If a heartbeat was seen, re-enable requires it to be fresh. A rig without VisionPilot can still re-enable.
• The ramp reads the odometry only after its first sample. A negative or NaN speed gives a target of 0.
• stop_only and 3.0 m/s² are set only on actuation_x5h. Every other target keeps always and 1.5 m/s². The startup line prints the stop deceleration, so the console shows which value a firmware image carries.

How this was verified

• ./build.sh --platform freertos-posix succeeds. The controller smoke reaches Actuation Safety Island is Live, and the startup line reads heartbeat 0.50s; stop 1.50 m/s^2.
• ./build.sh --platform freertos-posix --unit-test succeeds, and all six host test binaries exit 0. supervision_latch_test now covers the ramp: the floor at 0, and a negative or NaN ego speed.
• ./build.sh --platform freertos-x5h succeeds at the top of this stack. SI_DEFAULT_LEGACY_CONTROL_CMD="stop_only" and SI_DEFAULT_STOP_DECEL_MPS2=3.0f are in the actuation_x5h compile flags only.

No hardware was used for this pull request.

What this depends on

This is based on #71, which re-lands #47. #47 merged into feat/freertos-x5h-netif after #45 had already squash-merged that branch into main, so its content never reached main.

Refs: autowarefoundation/openadkit#147

[oguzkaganozt]

Hi @youtalk, thanks for this stack. The stop profile and the rpmsg-si channel are what the demo needs. I'd like to fit them into #66 (green in CI) instead of running them beside it, so the SI has one output.

The problem. #66 makes the SI publish one supervised output, ApprovedRequest, with per-source watchdogs and a stop latch that only an explicit /control/safety_island/reenable clears. #57 also stops the car on a stale heartbeat, but writes its own command straight to control_cmd/CAN. Together that is two writers, and ApprovedRequest would not show the stop.

Proposal: the heartbeat and the fault flag become two more inputs to the #66 supervisor.

• Keep: the ramp (v = max(0, v0 - decel * t); CARLA's Ackermann controller follows the speed target), holding -decel for the whole stop and never 0.0, the has_odometry_/has_steering_ guards, the 0.5 s threshold as a parameter, stop_profile_test.
• Heartbeat: a SourceWatch checked in supervisorStaleReason(). "Not stale until first seen" matches your arming rule.
• Fault flag: si_channel_fault() calls latchFault(...) on the rising edge. fault=0 must not clear the latch: in #151 fault=1 is an operator injection, and a message should not release a stop.
• Stop output: through publishSiStop(), so it is an ApprovedRequest with a fault_id. The ramp goes into the payload it builds (stoppedPayload()).
• Clearing: only on /reenable, once every source is fresh (see my follow-up for what that means for the demo).

For the D5/D6 bench session

• Heartbeat gap. On the CPU rig VisionPilot's longest gap between commands was 627–682 ms at 8.8 Hz, so 0.5 s would false-trip there. On the NPU it should be shorter. Please note the longest gap when D5 runs; the threshold is a parameter.
• Cycle id. The node relays /vehicle/throttle_cmd 1:1 and the legacy Float64 topics carry no cycle id, so a VisionPilot that keeps republishing the same command keeps the heartbeat alive. DrivingCommand on feat/vp-si-interface has a cycle the SI could check. I did not find it in feat/x5h-carla-npu.

Not covered. A stop path that still works when the follower hangs (#64). Neither #57 nor #66 has it: both run in the MPC's timer.

Logistics. The stack sits on feat/freertos-x5h-netif, which was squash-merged in #45, so it needs a rebase onto main.

I can draft the rework on top of #66 if that is easier for you.

[oguzkaganozt]

Follow-up on #66 and the demo. #66 passes CI and the CARLA rig, but I have not run it with the demo topology. Four things the #57 rework needs:

1. Build with SI_LEGACY_CONTROL_CMD=stop_only. On main the SI is silent when idle, and the demo depends on that: the arbiter prefers a fresh CR52 command, and D6 passes when a CR52 control_cmd appears. feat(si): supervised single output with startup mode selection and native VisionPilot ingress #66 publishes HOLD/NORMAL on control_cmd every cycle by default. With stop_only only SI_STOP goes there; on the rig I saw 0 messages while driving and 200 during the stop. The X5H has no environment, so this is a compile-time default (SI_DEFAULT_LEGACY_CONTROL_CMD, no CMake option yet, SI: hardening and test backlog from the #66 review #69). I have not tried it on the X5H build.
2. A way to clear the latch. After a stop the SI stays in SI_STOP until /control/safety_island/reenable is published, and every source must be fresh within 5 s of it. bridge-demo.yaml has no entry for it. The booth script needs to publish it after the VisionPilot reset, and at bring-up for D7.
3. A supervision configuration for the demo's inputs. Re-enable needs odometry, acceleration, steering, operation mode and the selected input to have each been seen. The demo bridge carries only odometry, steering and the heartbeat, and no trajectory arrives, so the latch could never clear. The demo needs the heartbeat as the selected source, without acceleration and operation mode. This is new work on top of feat(si): supervised single output with startup mode selection and native VisionPilot ingress #66; I can write it if you prefer.
4. Watch the new watchdogs in D6. An odometry or acceleration gap over 0.4 s, or a steering gap over 0.5 s, now latches SI_STOP (main only withheld output). The operation mode has no age limit. The limits are compile-time constants today: the code declares them as source_timeout_*_s parameters, but nothing loads node parameters on the target.

I read bridge-demo.yaml at the head of #151 (46d41c3).

[youtalk]

I reworked the stack as you proposed and rebased it onto main. Here is each point.

Proposal

• Heartbeat: watch_heartbeat_ is a SourceWatch checked in supervisorStaleReason(), with source_timeout_heartbeat_s (0.5 s) as a parameter. If a heartbeat was seen, re-enable requires it to be fresh. A rig without VisionPilot can still re-enable.
• Fault flag (feat(freertos-x5h): add the rpmsg-si channel with a heartbeat and a fault latch #58): the control tick calls latchFault("si fault input") on the rising edge of si_channel_fault(), and fault=0 does not clear the latch. I added one condition: re-enable also waits until the input is back at 0. Without it, a re-enable while fault=1 is still held resumes driving, and no new edge latches again.
• Stop output: through publishSiStop(). The ramp is in stoppedPayload() while the latch is set, from the ego speed at the latch. StopControl still holds -decel for the whole stop. The has_odometry_ guard moved into latchFault(), and stoppedPayload() already guards the steering. The ramp therefore applies to every SI_STOP, not only to the heartbeat stop. Please run your E2E stop gate once with it. Your actuator maps SI_STOP to the brake directly, so I expect the 500 ms gate to hold, but I have not read that code.
• stop_profile_test: the class is gone, because the supervisor now owns arming, latching and clearing. The ramp is a pure helper, stopRampVelocity() in supervision_latch.hpp, and its cases are in supervision_latch_test.
• Deceleration: nothing loads node parameters on the target, so I added SI_DEFAULT_STOP_DECEL_MPS2, set to 3.0 on actuation_x5h only. D6 passed at 3.0, and its stop distance limit comes from that value. Every other target keeps 1.5. A question: 1.5 matches Autoware's moderate_stop_service_acceleration, and Autoware's emergency values are 2.4 (vehicle_cmd_gate) and 2.5 (mrm_emergency_stop_operator). Is 1.5 deliberate for an SI stop, or do you want a higher shared default?

Follow-up

1. Done in feat(controller): stop the vehicle when the driving stack's heartbeat goes stale #57: actuation_x5h compiles SI_DEFAULT_LEGACY_CONTROL_CMD="stop_only". The define is in the X5H build flags, which I checked. I have not run it on the board yet.
2. and 4. The demo stays on its current firmware for now. When it moves to this firmware, I will add /control/safety_island/reenable to bridge-demo.yaml and the booth script, and watch the new watchdogs in D6.
3. Yes, please write Configure SI startup and required inputs per deployment #70. I will rebase on it.

Bench notes

• Heartbeat gap: I will record the longest gap on the NPU in the next bench session. The threshold stays 0.5 s until then.
• Cycle id: agreed. The heartbeat shows that VisionPilot's loop runs, not that it sees new frames. The demo build does not publish DrivingCommand yet. After it does, the VP_CONTROL identity check covers this, and the heartbeat watch can go.
• Keep SI fault response running when the follower hangs #64: agreed, out of scope here.

Logistics

#47 merged into feat/freertos-x5h-netif 17 seconds after #45 had squash-merged that branch, so its content never reached main. #58 needs it. #71 re-lands it unchanged at the bottom of this stack: #71, then #57, #58 and #59.

[oguzkaganozt]

LGTM!