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

Author: tetano02

README.md

@@ -125,6 +125,48 @@ echo 'export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_
 
 Reload your terminal with `source ~/.bashrc` (or `source ~/.zshrc` on macOS).
 
+### Servo Channels (16/32)
+
+`ArduPilotPlugin` auto-detects 16 vs 32 output channels from the SITL packet
+magic value (`18458` for 16 channels, `29569` for 32 channels).
+
+The SDF parameter `<have_32_channels>` is retained for compatibility and debug
+purposes, but no longer needs to match `SERVO_32_ENABLE` exactly at startup.
+If a mismatch is detected at runtime, the plugin logs a warning and overrides
+the value from incoming packets.
+
+#### Tested
+
+Manual verification was run with Gazebo logs captured to a file:
+
+```bash
+gz sim -v4 -r iris_runway.sdf 2>&1 | tee /tmp/gz.log
+```
+
+Then, in MAVProxy:
+
+```text
+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
+```
+
+And in a shell:
+
+```bash
+rg -n "ArduPilot|magic|Overriding|Connected" /tmp/gz.log
+```
+
+Expected behavior:
+
+- with `0 -> 1`, Gazebo logs a mismatch warning and overrides to 32 channels.
+- with `1 -> 0`, Gazebo logs a mismatch warning and overrides to 16 channels.
+- during SITL reboot, transient `ArduPilot controller has reset` warnings can appear.
+
 ## Usage
 
 ### 1. Iris quad-copter

include/ArduPilotPlugin.hh

@@ -29,9 +29,8 @@ namespace sim
 {
 namespace systems
 {
-/// \todo(srmainwaring) handle 16 or 32 based on magic
-
 // The servo packet received from ArduPilot SITL. Defined in SIM_JSON.h.
+// Channel count (16 vs 32) is auto-detected at runtime from the magic field.
 struct servo_packet_16 {
     uint16_t magic;         // 18458 expected magic value
     uint16_t frame_rate;
@@ -87,7 +86,8 @@ class ArduPilotPluginPrivate;
 /// <lock_step>           set true to enforce lock-step simulation
 /// <no_time_sync>        set true to prevent SITL from trying to sync
 ///                       with wall-time
-/// <have_32_channels>    set true if 32 channels are enabled
+/// <have_32_channels>    optional compatibility/debug hint; runtime value is
+///                       auto-detected from packet magic and may be overridden
 ///
 class GZ_SIM_VISIBLE ArduPilotPlugin:
   public gz::sim::System,

src/ArduPilotPlugin.cc

@@ -1437,7 +1437,7 @@ ssize_t getServoPacket(
     int counter = 0;
     while (true)
     {
-        TServoPacket last_pkt;
+        TServoPacket last_pkt{};
         auto recvSize_last = _sock.recv(&last_pkt, sizeof(TServoPacket), 0ul);
         if (recvSize_last == -1)
         {
@@ -1482,42 +1482,17 @@ bool gz::sim::systems::ArduPilotPlugin::ReceiveServoPacket()
         waitMs = 1;
     }
 
-    // 16 / 32 channel compatibility
-    uint16_t pkt_magic{0};
-    uint16_t pkt_frame_rate{0};
-    uint16_t pkt_frame_count{0};
-    std::array<uint16_t, 32> pkt_pwm;
-    ssize_t recvSize{-1};
-    if (this->dataPtr->have32Channels)
-    {
-      servo_packet_32 pkt;
-      recvSize = getServoPacket(
-          this->dataPtr->sock,
-          this->dataPtr->fcu_address,
-          this->dataPtr->fcu_port_out,
-          waitMs,
-          this->dataPtr->modelName,
-          pkt);
-      pkt_magic = pkt.magic;
-      pkt_frame_rate = pkt.frame_rate;
-      pkt_frame_count = pkt.frame_count;
-      std::copy(std::begin(pkt.pwm), std::end(pkt.pwm), std::begin(pkt_pwm));
-    }
-    else
-    {
-      servo_packet_16 pkt;
-      recvSize = getServoPacket(
-          this->dataPtr->sock,
-          this->dataPtr->fcu_address,
-          this->dataPtr->fcu_port_out,
-          waitMs,
-          this->dataPtr->modelName,
-          pkt);
-      pkt_magic = pkt.magic;
-      pkt_frame_rate = pkt.frame_rate;
-      pkt_frame_count = pkt.frame_count;
-      std::copy(std::begin(pkt.pwm), std::end(pkt.pwm), std::begin(pkt_pwm));
-    }
+    // Always receive into the largest packet buffer (servo_packet_32, 72 bytes).
+    // The channel count is determined afterward from the magic field; this removes
+    // the need to keep <have_32_channels> in sync with the SITL configuration.
+    servo_packet_32 pkt{};
+    ssize_t recvSize = getServoPacket(
+        this->dataPtr->sock,
+        this->dataPtr->fcu_address,
+        this->dataPtr->fcu_port_out,
+        waitMs,
+        this->dataPtr->modelName,
+        pkt);
 
     // didn't receive a packet, increment timeout count if online, then return
     if (recvSize == -1)
@@ -1547,6 +1522,75 @@ bool gz::sim::systems::ArduPilotPlugin::ReceiveServoPacket()
         return false;
     }
 
+    // Infer 16 vs 32 channels directly from the magic value in the packet.
+    // Values come from SIM_JSON.h in the ArduPilot SITL library.
+    constexpr uint16_t magic_16 = 18458;
+    constexpr uint16_t magic_32 = 29569;
+    const size_t recvSizeBytes = static_cast<size_t>(recvSize);
+    bool detected32Channels;
+    if (pkt.magic == magic_32)
+    {
+        detected32Channels = true;
+    }
+    else if (pkt.magic == magic_16)
+    {
+        detected32Channels = false;
+    }
+    else
+    {
+        gzwarn << "Incorrect protocol magic "
+            << pkt.magic << " (expected "
+            << magic_16 << " or " << magic_32 << "), dropping packet.\n";
+        return false;
+    }
+
+    if (detected32Channels && recvSizeBytes != sizeof(servo_packet_32))
+    {
+        gzwarn << "[" << this->dataPtr->modelName << "] "
+            << "Got 32-channel magic with unexpected packet size "
+            << recvSize << " (expected " << sizeof(servo_packet_32)
+            << "), dropping packet.\n";
+        return false;
+    }
+
+    if (!detected32Channels && recvSizeBytes != sizeof(servo_packet_16))
+    {
+        gzwarn << "[" << this->dataPtr->modelName << "] "
+            << "Got 16-channel magic with unexpected packet size "
+            << recvSize << " (expected " << sizeof(servo_packet_16)
+            << "), dropping packet.\n";
+        return false;
+    }
+
+    // If the SDF-configured value disagrees with the auto-detected one, override
+    // it and log a warning.  <have_32_channels> is retained for backward
+    // compatibility and debug purposes only.
+    if (detected32Channels != this->dataPtr->have32Channels)
+    {
+        gzwarn << "[" << this->dataPtr->modelName << "] "
+            << "<have_32_channels>=" << this->dataPtr->have32Channels
+            << " does not match packet magic"
+            << " (detected " << (detected32Channels ? 32 : 16) << " channels)."
+            << " Overriding from packet.\n";
+        this->dataPtr->have32Channels = detected32Channels;
+    }
+
+    const uint16_t pkt_frame_rate = pkt.frame_rate;
+    const uint32_t pkt_frame_count = pkt.frame_count;
+
+    // Build a unified 32-element PWM array, zero-initialised.
+    // For a 16-channel packet the upper 16 slots remain at zero.
+    std::array<uint16_t, 32> pkt_pwm{};
+    if (detected32Channels)
+    {
+        std::copy(std::begin(pkt.pwm), std::end(pkt.pwm), std::begin(pkt_pwm));
+    }
+    else
+    {
+        std::copy(std::begin(pkt.pwm), std::begin(pkt.pwm) + 16,
+            std::begin(pkt_pwm));
+    }
+
 #if DEBUG_JSON_IO
     int max_servo_channels = this->dataPtr->have32Channels ? 32 : 16;
 
@@ -1555,7 +1599,7 @@ bool gz::sim::systems::ArduPilotPlugin::ReceiveServoPacket()
     oss << "recv " << recvSize << " bytes from "
         << this->dataPtr->fcu_address << ":"
         << this->dataPtr->fcu_port_out << "\n";
-    // oss << "magic: " << pkt_magic << "\n";
+    // oss << "magic: " << pkt.magic << "\n";
     // oss << "frame_rate: " << pkt_frame_rate << "\n";
     oss << "frame_count: " << pkt_frame_count << "\n";
     // oss << "pwm: [";
@@ -1566,18 +1610,6 @@ bool gz::sim::systems::ArduPilotPlugin::ReceiveServoPacket()
     gzdbg << "\n" << oss.str();
 #endif
 
-    // check magic, return if invalid
-    constexpr uint16_t magic_16 = 18458;
-    constexpr uint16_t magic_32 = 29569;
-    uint16_t magic = this->dataPtr->have32Channels ? magic_32 : magic_16;
-    if (magic != pkt_magic)
-    {
-        gzwarn << "Incorrect protocol magic "
-            << pkt_magic << " should be "
-            << magic << "\n";
-        return false;
-    }
-
     // the controller is online
     if (!this->dataPtr->arduPilotOnline)
     {