Takeoff node v7 — event-driven FSM (final)

Closes TASK-021 21a (2026-05-09). Continues from Takeoff node evolution v1–v5. The final fix for the heartbeat unblocked integration with --mavros; afterward the old tick-based FSM broke, and this log is about migrating to event-driven.

Why v5/v6 (tick-based) broke under `--mavros`

In working v5, transitions were time-based: tick > 30 → GUIDED, tick > 50 → arm, tick > 70 → takeoff. Without --mavros, the drone was “already connected” by the time the node started — ticks lined up with reality.

Under --mavros it broke this way:

tick incremented before the state.connected check. MAVROS takes ~6 s to come up — during that time tick had passed 60.
On the first tick after connected, all three conditions (tick > 30, tick > 50, tick > 70) were satisfied simultaneously → set_mode(GUIDED) and arming() fired in adjacent ticks, and ArduPilot couldn’t keep up.
EKF Gyros inconsistent / Accels inconsistent blocked armed for another ~15 s. Each retry rolled tick = 55, phase = 'guided' — but set_mode(GUIDED) was not resent; only arming() was.
ArduPilot could exit GUIDED back into STABILIZE (on IMU re-init / prearm failure), and the FSM wouldn’t notice.
When armed finally took → NAV_TAKEOFF went into STABILIZE → FAILED → ArduPilot disarmed.

v7 architecture

PHASES = wait_connect → set_mode → arming → takeoff → hover

fsm_loop @ 1 Hz:
    wait_connect: wait for state.connected
    set_mode:     if state.mode != 'GUIDED' → SetMode(GUIDED), throttle 2 s
                  else → arming
    arming:       if state.mode != 'GUIDED' → rollback to set_mode
                  if state.armed → takeoff
                  else → CommandBool(True), throttle 2 s
    takeoff:      if !state.armed → rollback to arming
                  once: CommandTOL(2.0), record takeoff_at
                  after TAKEOFF_HOLD_S=5 s → hover
    hover:        log mode/armed once per second

hover_loop @ 10 Hz:
    publishes setpoint only when phase == 'hover'

Key differences from tick-based

Command → wait for state.* change → transition. No fixed delays.
Mode-revert detection. If ArduPilot reverts mode, we detect it, roll back to set_mode, and resend GUIDED.
service_is_ready() guard before each call_async. No lost calls when MAVROS plugins haven’t yet brought up their handlers.
Setpoint and FSM on separate timers:
- hover_loop @ 10 Hz — minimum 2 Hz required by ArduPilot to keep the link considered alive.
- fsm_loop @ 1 Hz — give ArduPilot time to react between commands, don’t spam services.

Smoke result (D2, headless)

$ ./help_scripts/launch.sh --full --mavros --auto takeoff --headless -d -log

[takeoff_node]: takeoff_node v7 started, waiting for FCU...
[takeoff_node]: phase: wait_connect → set_mode (FCU connected)        ← +5s
[takeoff_node]: → SetMode GUIDED
[takeoff_node]: phase: set_mode → arming (mode=GUIDED)                ← confirmed
[takeoff_node]: → Arming   (×8 retries, 24s — waiting for EKF tilt/yaw)
[takeoff_node]: phase: arming → takeoff (armed)
[takeoff_node]: → NAV_TAKEOFF 2.0m
[takeoff_node]: phase: takeoff → hover (takeoff hold elapsed)
[takeoff_node]: hover: mode=GUIDED armed=True

Live MAVROS state at the hover moment:

$ ros2 topic echo /mavros/state --once
connected: true     armed: true     guided: true
mode: GUIDED        system_status: 4 (MAV_STATE_ACTIVE)

$ ros2 topic echo /mavros/local_position/pose --once
position: x=0.0  y=0.0  z=2.0       ← exactly target

ArduPilot:

GUIDED> Mode GUIDED
AP: EKF3 IMU0 initialised
AP: Arm: Accels inconsistent  (×3)        ← EKF settling, not a bug
AP: Arm: Gyros inconsistent   (×4)
Got COMMAND_ACK: COMPONENT_ARM_DISARM: ACCEPTED
AP: Arming motors → ARMED
Got COMMAND_ACK: NAV_TAKEOFF: ACCEPTED

Full logs: ~/drone_media/sim/TASK-021/21a_*.log.

What you need to understand about “EKF settling”

Accels inconsistent / Gyros inconsistent for 15-25 s after SITL start — that’s normal for indoor (no GPS, no optical flow). EKF3 waits for IMU data to stabilize before signaling armed-ready. Not our bug — a feature of SITL IMU init plus EK3_SRC1_VELXY 5 (optical flow). We accounted for it via retry in the arming phase with a 2 s throttle.

Files

simulation/src/drone_sim/drone_sim/takeoff_node.py — v7 (event-driven).
simulation/src/drone_sim/launch/drone.launch.py — user entry point.

Usage

./help_scripts/launch.sh --full --mavros --auto takeoff --headless -d -log

The drone walks itself through wait_connect → GUIDED → ARM → NAV_TAKEOFF 2 m → hover. Cartographer / Nav2 can build on top of the flying drone after that.

Why v5/v6 (tick-based) broke under --mavros