SITL: optional shared-memory transport for ride-along JSON

When a ride-along master (--slave N) and its JSON slaves are all
started with --cluster, the fdm/servo exchange runs over the cluster
shared-memory segment instead of the UDP sockets; without --cluster
the UDP transport remains exactly as before. The wire content is
unchanged in both directions - the master's JSON line and the slave's
binary servo packet are byte-identical over either transport - so the
two paths are directly comparable.

Each instance's 4096-byte payload slot is partitioned: SwarmInfo keeps
offset 0, and bytes 512+ become a seq-numbered one-way channel (the
master publishes fdm in its own slot, each slave publishes servos in
its own slot; readers poll seq for change, and the per-slot robust
mutex makes every read internally consistent). write_payload() and
read_payload() gain a default-0 offset argument to address the
partition.

The master now also reports its achieved exchange rate every 2s of
wall time over either transport, so the two can be A/B compared:
measured master+1-slave on one host, UDP averaged 11.6k frames/s
(peak 13.9k) and shared memory 15.0k frames/s (peak 20.6k), ~30%
faster on average with ~50% higher peaks.

Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>

Author: davidbuzz

libraries/AP_HAL_SITL/SITL_SharedMem.cpp

@@ -704,20 +704,20 @@ uint8_t AP_SITL_SharedMem::get_instance_count() const
   mutex. EOWNERDEAD (a prior crashed run reusing this slot) is marked
   consistent and we proceed, since we are the sole writer for our slot.
 */
-void AP_SITL_SharedMem::write_payload(const void *data, uint32_t len)
+void AP_SITL_SharedMem::write_payload(const void *data, uint32_t len, uint32_t offset)
 {
-    if (_data == nullptr) {
+    if (_data == nullptr || offset >= AP_SITL_SHMEM_PAYLOAD_SIZE) {
         return;
     }
-    if (len > AP_SITL_SHMEM_PAYLOAD_SIZE) {
-        len = AP_SITL_SHMEM_PAYLOAD_SIZE;
+    if (len > AP_SITL_SHMEM_PAYLOAD_SIZE - offset) {
+        len = AP_SITL_SHMEM_PAYLOAD_SIZE - offset;
     }
     auto &slot = _data->instance[_instance_id];
 
 #ifdef __CYGWIN__
     // no cross-process lock exists on this platform - see
     // _init_segment_contents(); the copy is unguarded
-    memcpy(slot.payload, data, len);
+    memcpy(&slot.payload[offset], data, len);
 #else
     int ret = pthread_mutex_lock(&slot.payload_mutex);
 #if AP_SITL_SHMEM_ROBUST_MUTEX
@@ -736,7 +736,7 @@ void AP_SITL_SharedMem::write_payload(const void *data, uint32_t len)
         return;
     }
 
-    memcpy(slot.payload, data, len);
+    memcpy(&slot.payload[offset], data, len);
 
     pthread_mutex_unlock(&slot.payload_mutex);
 #endif  // __CYGWIN__
@@ -747,20 +747,21 @@ void AP_SITL_SharedMem::write_payload(const void *data, uint32_t len)
   mutex. EOWNERDEAD means the peer died mid-write; mark it consistent
   but treat the data as unreliable and return false.
 */
-bool AP_SITL_SharedMem::read_payload(uint8_t instance_id, void *data, uint32_t len) const
+bool AP_SITL_SharedMem::read_payload(uint8_t instance_id, void *data, uint32_t len, uint32_t offset) const
 {
-    if (_data == nullptr || instance_id >= AP_SITL_SHMEM_MAX_INSTANCES) {
+    if (_data == nullptr || instance_id >= AP_SITL_SHMEM_MAX_INSTANCES ||
+        offset >= AP_SITL_SHMEM_PAYLOAD_SIZE) {
         return false;
     }
-    if (len > AP_SITL_SHMEM_PAYLOAD_SIZE) {
-        len = AP_SITL_SHMEM_PAYLOAD_SIZE;
+    if (len > AP_SITL_SHMEM_PAYLOAD_SIZE - offset) {
+        len = AP_SITL_SHMEM_PAYLOAD_SIZE - offset;
     }
     auto &slot = _data->instance[instance_id];
 
 #ifdef __CYGWIN__
     // no cross-process lock exists on this platform - see
     // _init_segment_contents(); the copy is unguarded
-    memcpy(data, slot.payload, len);
+    memcpy(data, &slot.payload[offset], len);
     return true;
 #else
     int ret = pthread_mutex_lock(&slot.payload_mutex);
@@ -782,7 +783,7 @@ bool AP_SITL_SharedMem::read_payload(uint8_t instance_id, void *data, uint32_t l
     }
 
     if (!owner_died) {
-        memcpy(data, slot.payload, len);
+        memcpy(data, &slot.payload[offset], len);
     }
 
     pthread_mutex_unlock(&slot.payload_mutex);

libraries/AP_HAL_SITL/SITL_SharedMem.h

@@ -57,6 +57,39 @@
 // e.g. vehicle position/attitude or other user-defined telemetry.
 #define AP_SITL_SHMEM_PAYLOAD_SIZE  4096
 
+/*
+  the payload block is partitioned between its tenants at fixed offsets:
+    [0, AP_SITL_SHMEM_RIDE_OFFSET)    AP_SITL_SwarmInfo, published from
+                                      Aircraft::sync_frame_time()
+    [AP_SITL_SHMEM_RIDE_OFFSET, end)  ride-along JSON transport channel,
+                                      used by SIM_JSON_Master (master) and
+                                      SIM_JSON (slave) instead of the UDP
+                                      sockets when the ride-along pair was
+                                      started with --cluster
+  Both tenants are guarded by the same per-slot robust mutex.
+*/
+#define AP_SITL_SHMEM_RIDE_OFFSET   512
+#define AP_SITL_SHMEM_RIDE_SIZE     (AP_SITL_SHMEM_PAYLOAD_SIZE - AP_SITL_SHMEM_RIDE_OFFSET)
+
+/*
+  one-directional message channel living in the ride-along region of a
+  slot: the master publishes its fdm JSON line in its own slot, each
+  slave publishes its servo packet in its own slot. A publish rewrites
+  the whole record under the slot mutex and bumps seq last, so a single
+  read_payload() of the channel always yields an internally-consistent
+  record; readers poll seq for change.
+*/
+struct AP_SITL_RideAlongChannel {
+    uint32_t magic;     // which endpoint writes this slot's channel (below)
+    uint32_t seq;       // bumped once per publish
+    uint32_t len;       // valid bytes in data[]
+    uint8_t  data[AP_SITL_SHMEM_RIDE_SIZE - 12];
+};
+static_assert(sizeof(AP_SITL_RideAlongChannel) == AP_SITL_SHMEM_RIDE_SIZE,
+              "ride-along channel must exactly fill its payload region");
+#define AP_SITL_RIDE_FDM_MAGIC    0x52464D31  // "RFM1": master -> slaves fdm JSON
+#define AP_SITL_RIDE_SERVO_MAGIC  0x52535631  // "RSV1": slave -> master servo packet
+
 /*
   Robust process-shared mutexes make a peer that is killed mid-write hand
   the next locker EOWNERDEAD instead of deadlocking every other instance
@@ -153,18 +186,20 @@ class AP_SITL_SharedMem {
       write up to AP_SITL_SHMEM_PAYLOAD_SIZE bytes into this instance's
       payload block, for peers to read via read_payload(). Takes the
       slot's robust process-shared mutex for the duration of the copy so
-      readers never observe a torn write.
+      readers never observe a torn write. offset positions the write
+      within the block, for tenants above the first (see the payload
+      partition map by AP_SITL_SHMEM_RIDE_OFFSET).
     */
-    void write_payload(const void *data, uint32_t len);
+    void write_payload(const void *data, uint32_t len, uint32_t offset = 0);
 
     /*
       read the given instance's payload block into data (up to len bytes,
-      capped at AP_SITL_SHMEM_PAYLOAD_SIZE). Takes the slot's robust
-      process-shared mutex for the duration of the copy. Returns true on
-      success, false if shm is not initialised or instance_id is out of
-      range.
+      capped at AP_SITL_SHMEM_PAYLOAD_SIZE, starting offset bytes into
+      the block). Takes the slot's robust process-shared mutex for the
+      duration of the copy. Returns true on success, false if shm is not
+      initialised or instance_id is out of range.
     */
-    bool read_payload(uint8_t instance_id, void *data, uint32_t len) const;
+    bool read_payload(uint8_t instance_id, void *data, uint32_t len, uint32_t offset = 0) const;
 
     /*
       barrier sync: spin-wait until every peer that has joined the cluster

libraries/AP_HAL_SITL/SITL_cmdline.cpp

@@ -119,7 +119,9 @@ void SITL_State::_usage(void)
            "\t--sysid ID               set MAV_SYSID\n"
            "\t--cluster ID             join SITL cluster ID (0-255) for shared-memory\n"
            "\t                         clock sync with other instances; off unless given\n"
-           "\t--slave number           set the number of JSON slaves\n"
+           "\t--slave number           set the number of JSON ride-along slaves; with\n"
+           "\t                         --cluster the fdm/servo exchange runs over the\n"
+           "\t                         cluster's shared memory instead of UDP\n"
            "\t--use_sim_time <true|false>  use ROS2 simulation clock for DDS topics. Defaults to false\n"
         );
 }

libraries/SITL/SIM_JSON.cpp

@@ -31,6 +31,14 @@
 #include <AP_HAL/utility/replace.h>
 #include <SRV_Channel/SRV_Channel.h>
 #include <AP_Filesystem/AP_Filesystem.h>
+#include <stddef.h>
+#include <time.h>
+#include <unistd.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include <AP_HAL_SITL/HAL_SITL_Class.h>
+extern const HAL_SITL& hal_sitl;
+#endif
 
 #define UDP_TIMEOUT_MS 100
 
@@ -99,6 +107,98 @@ void JSON::set_interface_ports(const char* address, const int port_in, const int
     printf("JSON control interface set to %s:%u\n", target_ip, control_port);
 }
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+static uint64_t ride_wall_micros(void)
+{
+    struct timespec ts {};
+    clock_gettime(CLOCK_MONOTONIC, &ts);
+    return ts.tv_sec * 1000000ULL + ts.tv_nsec / 1000ULL;
+}
+
+/*
+  the ride-along exchange runs over the cluster's shared memory segment
+  instead of the UDP sockets when this instance was started with
+  --cluster (the master must be too)
+*/
+bool JSON::ride_shmem_active(void) const
+{
+    return hal_sitl.get_sitl_state()->_shared_mem.is_initialised();
+}
+
+// publish our servo packet in our own slot's ride-along channel
+void JSON::ride_shmem_send(const void *pkt, uint32_t len)
+{
+    auto &shm = hal_sitl.get_sitl_state()->_shared_mem;
+    static AP_SITL_RideAlongChannel ch;
+    ch.magic = AP_SITL_RIDE_SERVO_MAGIC;
+    ch.len = MIN(len, (uint32_t)sizeof(ch.data));
+    memcpy(ch.data, pkt, ch.len);
+    ch.seq = ++ride_out_seq;
+    shm.write_payload(&ch, offsetof(AP_SITL_RideAlongChannel, data) + ch.len,
+                      AP_SITL_SHMEM_RIDE_OFFSET);
+}
+
+/*
+  poll the master's slot for a fresh fdm JSON line; mirrors
+  sock.recv(buf, size, timeout_ms) semantics, returning 0 on timeout
+*/
+ssize_t JSON::ride_shmem_recv(uint8_t *buf, uint32_t size, uint32_t timeout_ms)
+{
+    auto &shm = hal_sitl.get_sitl_state()->_shared_mem;
+    const uint64_t deadline_us = ride_wall_micros() + timeout_ms * 1000ULL;
+    uint32_t spins = 0;
+    while (true) {
+        if (ride_master_slot < 0) {
+            // discover which slot carries the fdm channel (usually the
+            // master's instance 0, but any slot with the magic works;
+            // give each ride-along group its own --cluster id)
+            for (uint8_t i = 0; i < AP_SITL_SHMEM_MAX_INSTANCES; i++) {
+                AP_SITL_RideAlongChannel hdr;
+                if (shm.instance_active(i) &&
+                    shm.read_payload(i, &hdr, offsetof(AP_SITL_RideAlongChannel, data),
+                                     AP_SITL_SHMEM_RIDE_OFFSET) &&
+                    hdr.magic == AP_SITL_RIDE_FDM_MAGIC) {
+                    ride_master_slot = i;
+                    printf("JSON: ride-along fdm found in shared-memory slot %u\n", i);
+                    break;
+                }
+            }
+        }
+        if (ride_master_slot >= 0) {
+            static AP_SITL_RideAlongChannel ch;
+            if (shm.read_payload(ride_master_slot, &ch, sizeof(ch),
+                                 AP_SITL_SHMEM_RIDE_OFFSET) &&
+                ch.magic == AP_SITL_RIDE_FDM_MAGIC &&
+                ch.seq != ride_in_seq) {
+                ride_in_seq = ch.seq;
+                const uint32_t n = MIN(ch.len, MIN((uint32_t)sizeof(ch.data), size));
+                memcpy(buf, ch.data, n);
+                return n;
+            }
+        }
+        if (ride_wall_micros() >= deadline_us) {
+            return 0;
+        }
+        if (++spins > 5000) {
+            usleep(100);
+            spins = 0;
+        }
+    }
+}
+#endif  // CONFIG_HAL_BOARD == HAL_BOARD_SITL
+
+ssize_t JSON::transport_recv(uint32_t timeout_ms)
+{
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    if (ride_shmem_active()) {
+        return ride_shmem_recv(&sensor_buffer[sensor_buffer_len],
+                               sizeof(sensor_buffer)-sensor_buffer_len, timeout_ms);
+    }
+#endif
+    return sock.recv(&sensor_buffer[sensor_buffer_len],
+                     sizeof(sensor_buffer)-sensor_buffer_len, timeout_ms);
+}
+
 /*
     Decode and send servos
 */
@@ -114,6 +214,12 @@ void JSON::output_servos(const struct sitl_input &input)
           pkt.pwm[i] = input.servos[i];
       }
       pkt_size = sizeof(pkt);
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+      if (ride_shmem_active()) {
+          ride_shmem_send(&pkt, pkt_size);
+          send_ret = pkt_size;
+      } else
+#endif
       send_ret = sock.sendto(&pkt, pkt_size, target_ip, control_port);
     } else {
       servo_packet_16 pkt;
@@ -123,6 +229,12 @@ void JSON::output_servos(const struct sitl_input &input)
           pkt.pwm[i] = input.servos[i];
       }
       pkt_size = sizeof(pkt);
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+      if (ride_shmem_active()) {
+          ride_shmem_send(&pkt, pkt_size);
+          send_ret = pkt_size;
+      } else
+#endif
       send_ret = sock.sendto(&pkt, pkt_size, target_ip, control_port);
     }
 
@@ -303,7 +415,7 @@ uint64_t JSON::parse_sensors(const char *json)
 void JSON::recv_fdm(const struct sitl_input &input)
 {
     // Receive sensor packet
-    ssize_t ret = sock.recv(&sensor_buffer[sensor_buffer_len], sizeof(sensor_buffer)-sensor_buffer_len, UDP_TIMEOUT_MS);
+    ssize_t ret = transport_recv(UDP_TIMEOUT_MS);
     uint32_t wait_ms = UDP_TIMEOUT_MS;
 
     if (state.no_lockstep && ret <= 0) {
@@ -320,7 +432,7 @@ void JSON::recv_fdm(const struct sitl_input &input)
 
     while (ret <= 0) {
         //printf("No JSON sensor message received - %s\n", strerror(errno));
-        ret = sock.recv(&sensor_buffer[sensor_buffer_len], sizeof(sensor_buffer)-sensor_buffer_len, UDP_TIMEOUT_MS);
+        ret = transport_recv(UDP_TIMEOUT_MS);
         wait_ms += UDP_TIMEOUT_MS;
         // if no sensor message is received after 10 second resend servos, this help cope with SITL and the physics getting out of sync
         if (wait_ms > 1000) {

libraries/SITL/SIM_JSON.h

@@ -75,6 +75,24 @@ class JSON : public Aircraft {
     void output_servos(const struct sitl_input &input);
     void recv_fdm(const struct sitl_input &input);
 
+    // receive one chunk from whichever transport is active into
+    // sensor_buffer (shared-memory ride-along channel when this
+    // instance was started with --cluster, the UDP socket otherwise);
+    // mirrors sock.recv() semantics, returning 0 on timeout
+    ssize_t transport_recv(uint32_t timeout_ms);
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    // ride-along over the cluster's shared memory segment, as an
+    // alternative to the UDP sockets: see AP_SITL_RideAlongChannel in
+    // SITL_SharedMem.h
+    bool ride_shmem_active(void) const;
+    void ride_shmem_send(const void *pkt, uint32_t len);
+    ssize_t ride_shmem_recv(uint8_t *buf, uint32_t size, uint32_t timeout_ms);
+    uint32_t ride_out_seq;
+    uint32_t ride_in_seq;
+    int8_t ride_master_slot = -1;   // shmem slot the fdm channel was found in
+#endif
+
     uint64_t parse_sensors(const char *json);
 
     // buffer for parsing pose data in JSON format