Takeoff node — evolution v1 → v5 and 4 rules for controlling ArduPilot via MAVROS

Context

Before looking at the code, here are the three communication mechanisms we used:

1. Topics — asynchronous publish. Like radio: the sender doesn’t know who’s listening.

takeoff_node ─publish(PoseStamped)─→ /mavros/setpoint_position/local ─→ MAVLink SET_POSITION_TARGET_LOCAL_NED ─→ ArduPilot

2. Services — synchronous request/response via call_async.

takeoff_node ─call_async(arm=True)─→ /mavros/cmd/arming ─→ MAVLink COMMAND_LONG / ARM_DISARM ─→ ArduPilot ─COMMAND_ACK─→

3. MAVLink — ArduPilot’s binary protocol. Each message is a packet of bytes with a specific ID. MAVROS sits between our Python and ArduPilot as a translator.

v1 — naive approach → command spam

The idea was: always send setpoints (ArduPilot requires a continuous command stream); on connect, do takeoff once.

self.timer = self.create_timer(0.1, self.timer_callback)  # 10 Hz
def timer_callback(self):
    self.setpoint_pub.publish(self.target)  # ALWAYS
    if not self.takeoff_done:
        self.takeoff_done = True
        self.create_timer(2.0, self.do_takeoff)  # ← BUG

Bug 1 — nested create_timer repeats. create_timer in ROS 2 is not JS’s setTimeout. It’s a persistent, repeating timer. do_arm was called once per second, forever. Logs: Arming... Takeoff to 1.5m! Arming... Takeoff to 1.5m! .... ArduPilot was getting arm/disarm so fast it disarmed itself out of confusion.

Bug 2 — setpoint vs takeoff conflict. The drone is on the ground (z=0.2), we send “fly to z=1.5”. ArduPilot is not armed → ignores. After arming, it sees a conflict with its internal “I’m on the ground” state and disarms for safety.

MAVProxy log:

ARMED
AP: Disarming motors   ← immediately after arm
DISARMED

v2 — State Machine, same bug

Removed nested timers — one timer plus a counter plus an explicit step.

self.counter = 0
self.create_timer(0.1, self.timer_callback)
def timer_callback(self):
    self.counter += 1
    if self.step == 0 and self.counter > 30:   # 3 s
        self.step = 1                          # once
        ...

Cleaner. But setpoint still goes out from tick 0 — before arm. Same DISARM right after ARM. Infinite retry loop.

tick 0-29:   setpoint z=1.5 → ArduPilot ignores (not armed)
tick 30:     SetMode GUIDED → OK
tick 50:     Arm → ACCEPTED
tick 51-70:  setpoint continues
             ArduPilot: "armed + setpoint, but no NAV_TAKEOFF" → DISARM

In ArduPilot GUIDED mode there are two distinct scenarios:

• Correct: arm → NAV_TAKEOFF → altitude climb → then setpoint for holding.
• Ours: setpoint z=1.5 → arm → no NAV_TAKEOFF → ArduPilot confused → DISARM.

We were using setpoint_position out of its intended use. It’s for an already flying drone. On the ground, ArduPilot either ignores it or treats it as a conflict.

v3 — added CommandTOL, conflict persisted

CommandTOL is a ROS 2 wrapper for the MAVLink NAV_TAKEOFF (cmd=22) — exactly what MAVProxy does for takeoff 2.

# tick 70: CommandTOL(altitude=1.5)

But the order:

t=0s:    setpoint z=1.5 (drone on ground)
t=3s:    GUIDED
t=5s:    ARM → ACCEPTED
t=5s:    setpoint z=1.5 continues (10 Hz!)
t=7s:    NAV_TAKEOFF altitude=1.5 → ACCEPTED
         ArduPilot: "two command sources — setpoint+takeoff conflict → DISARM"

ArduPilot prefers safety. Two tools at once → it shuts down.

v4 — removed CommandTOL, same DISARM

Hypothesis: since CommandTOL and setpoint conflict, try without CommandTOL — ArduPilot should climb to the setpoint after arming.

Hypothesis wrong. Setpoint in GUIDED is not a takeoff command — it’s a hold-position command for an already-flying drone. Logs again:

ARMED → AP: Disarming motors → DISARMED  (repeated)

Key insight: ArduPilot doesn’t keep motors armed indefinitely. If there’s no movement command N seconds after arm — autodisarm on inactivity.

Also: I added an armed_once flag in __init__ and never used it anywhere. Typical iterative-development mistake — scaffolding without wiring. In v5 I replaced it with a phase string — state is immediately visible.

v5 — final: phase strict separation

The same line, evolved across all versions:

# v1..v4:
def loop(self):
    self.sp_pub.publish(self.target)  # ALWAYS, from tick 0
# v5:
def loop(self):
    elif self.phase == 'hover':
        self.sp_pub.publish(self.target)  # only in hover

Phases:

'wait'     → send nothing
'guided'   → send nothing
'arm'      → send nothing
'takeoff'  → only CommandTOL
'hover'    → only setpoint

Full graph:

wait → tick>30 → SetMode(GUIDED) → guided
     → tick>50 → Arm(True)      → arm
     → tick>70 AND state.armed → CommandTOL(1.5) → takeoff
     → tick>100               → hover (publish setpoint)

if armed becomes False during arm:
    tick = 55, phase = 'guided'  ← rollback one step

Final table of the 5 versions

Version	Setpoint before arm	CommandTOL	Timers	Result
v1	YES	NO	nested	command spam
v2	YES	NO	one + counter	DISARM immediately
v3	YES	YES	one + counter	conflict
v4	YES	NO	one + tick	DISARM immediately
v5 ✅	NO (only in hover)	YES	one + phase	FLIES

Common pattern in the 4 broken ones: setpoint sent before arm.

Four rules (extracted from system failures)

1. Setpoint — only for a flying drone. Don’t publish while the drone is on the ground.
2. Strict order: guided → arm → takeoff → setpoint. No parallel commands.
3. One timer, explicit phases. Nested create_timer is an antipattern.
4. Check state, not just tick. Transition on state.mode == 'GUIDED' AND state.armed == True, not just on time.

These rules aren’t in the documentation. They were extracted from failure logs. That’s how robotics engineering actually works: the system tells you what’s wrong through its behavior, you read the logs, and you draw conclusions.

What’s next

v5 worked until the --mavros integration. Under MAVROS the tick-based FSM broke (the tick incremented before state.connected). The final v7 — event-driven — is described in takeoff-node-final.