ArduPilotPlugin: auto-detect 16/32 servo packet via magic

[tetano02]

Summary

This PR updates ArduPilotPlugin to auto-detect 16 vs 32 servo output channels from packet magic at runtime, instead of relying on static <have_32_channels> configuration.

Problem

Current behavior can drop valid packets when SITL channel mode (SERVO_32_ENABLE) does not match SDF <have_32_channels>, causing protocol magic mismatch warnings and actuator desync.

Changes

• Receive servo data using the maximum packet size (servo_packet_32) and detect protocol variant from magic:
  • 18458 -> 16 channels
  • 29569 -> 32 channels
• Keep <have_32_channels> for compatibility/debug, but override runtime state when packet magic disagrees.
• Normalize incoming PWM into std::array<uint16_t, 32>:
  • 32ch: copy all 32
  • 16ch: copy first 16, leave remaining entries zero
• Fix frame counter width handling (pkt_frame_count uses uint32_t, matching the
  packet format), avoiding false reset / missed-frame warnings caused by 16-bit wrap.
• Add packet size coherence checks:
  • 16ch magic must have 40-byte packet
  • 32ch magic must have 72-byte packet
• Remove strict-aliasing risk in 16ch extraction path (no reinterpret_cast to servo_packet_16).
• Initialize drained packet buffer in socket backlog handling to avoid uninitialized-copy behavior.
• Update documentation:
  • README.md: add Servo Channels behavior and tested procedure
  • ArduPilotPlugin.hh: clarify <have_32_channels> as optional compatibility/debug hint

Why this approach

• Uses protocol data already provided by SITL (SIM_JSON magic values) as source of truth.
• Removes configuration coupling between SDF and SITL runtime mode.
• Preserves backward compatibility for existing models that still set <have_32_channels>.
• Resolves the existing TODO in ArduPilotPlugin.hh: handle 16 or 32 based on magic.

Manual Testing

Gazebo with log capture:

gz sim -v4 -r iris_runway.sdf 2>&1 | tee /tmp/gz.log

MAVProxy sequence:

param show SERVO_32_ENABLE
param set SERVO_32_ENABLE 1
reboot
param show SERVO_32_ENABLE
param set SERVO_32_ENABLE 0
reboot
param show SERVO_32_ENABLE

Log check:

rg -n "ArduPilot|magic|Overriding|Connected" /tmp/gz.log

Observed in logs:

• <have_32_channels>=0 ... detected 32 channels ... Overriding from packet.
• <have_32_channels>=1 ... detected 16 channels ... Overriding from packet.
• ArduPilot controller has reset around SITL reboot (expected transient behavior).

Compatibility

No public API changes. Existing SDF files remain valid.

[Copilot]

Pull request overview

Updates ArduPilotPlugin to auto-detect whether incoming SITL servo packets contain 16 or 32 channels by inspecting the packet magic at runtime, reducing reliance on static SDF configuration that can drift from SITL settings.

Changes:

• Always receive into the 32-channel packet buffer and infer 16/32 channel mode from packet magic.
• Add size/magic coherence checks and normalize PWM data into a 32-element array.
• Update docs/comments to clarify <have_32_channels> is now a compatibility/debug hint.

Reviewed changes

Copilot reviewed 3 out of 3 changed files in this pull request and generated 2 comments.

File	Description
src/ArduPilotPlugin.cc	Implements runtime magic-based 16/32 detection, size checks, unified PWM normalization, and safer socket drain initialization.
include/ArduPilotPlugin.hh	Updates comments to reflect runtime channel auto-detection and re-scopes <have_32_channels> as a hint.
README.md	Documents the new 16/32 servo-channel behavior and a manual verification procedure.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.