SITL: enable 32 servos in SITL_JSON

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

SITL: update JSON readme

- Update servo data packet section for 32 channel output.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

libraries/SITL/SIM_JSON.cpp

@@ -27,6 +27,7 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Logger/AP_Logger.h>
 #include <AP_HAL/utility/replace.h>
+#include <SRV_Channel/SRV_Channel.h>
 
 #define UDP_TIMEOUT_MS 100
 
@@ -100,20 +101,34 @@ void JSON::set_interface_ports(const char* address, const int port_in, const int
 */
 void JSON::output_servos(const struct sitl_input &input)
 {
-    servo_packet pkt;
+    size_t pkt_size = 0;
+    ssize_t send_ret = -1;
+    if (SRV_Channels::have_32_channels()) {
+      servo_packet_32 pkt;
+      pkt.frame_rate = rate_hz;
+      pkt.frame_count = frame_counter;
+      for (uint8_t i=0; i<32; i++) {
+          pkt.pwm[i] = input.servos[i];
+      }
+      pkt_size = sizeof(pkt);
+      send_ret = sock.sendto(&pkt, pkt_size, target_ip, control_port);
+    } else {
+      servo_packet_16 pkt;
       pkt.frame_rate = rate_hz;
       pkt.frame_count = frame_counter;
       for (uint8_t i=0; i<16; i++) {
           pkt.pwm[i] = input.servos[i];
       }
+      pkt_size = sizeof(pkt);
+      send_ret = sock.sendto(&pkt, pkt_size, target_ip, control_port);
+    }
 
-    size_t send_ret = sock.sendto(&pkt, sizeof(pkt), target_ip, control_port);
+    if ((size_t)send_ret != pkt_size) {
-    if (send_ret != sizeof(pkt)) {
         if (send_ret <= 0) {
             printf("Unable to send servo output to %s:%u - Error: %s, Return value: %ld\n",
                    target_ip, control_port, strerror(errno), (long)send_ret);
         } else {
-            printf("Sent %ld bytes instead of %lu bytes\n", (long)send_ret, (unsigned long)sizeof(pkt));
+            printf("Sent %ld bytes instead of %lu bytes\n", (long)send_ret, (unsigned long)pkt_size);
         }
     }
 }

libraries/SITL/SIM_JSON.h

@@ -44,13 +44,20 @@ public:
 
 private:
 
-    struct servo_packet {
+    struct servo_packet_16 {
         uint16_t magic = 18458; // constant magic value
         uint16_t frame_rate;
         uint32_t frame_count;
         uint16_t pwm[16];
     };
 
+    struct servo_packet_32 {
+        uint16_t magic = 29569; // constant magic value
+        uint16_t frame_rate;
+        uint32_t frame_count;
+        uint16_t pwm[32];
+    };
+
     // default connection_info_.ip_address
     const char *target_ip = "127.0.0.1";
 

libraries/SITL/examples/JSON/readme.md

@@ -14,13 +14,26 @@ Data is output from SITL in a binary format:
     uint16 pwm[16]
 ```
 
-The magic value is a constant of 18458, this is used to confirm the packet is from ArduPilot, in the future this may also be used for protocol versioning.
+The magic value is a constant of 18458, this is used to confirm the packet is from ArduPilot and is used for protocol versioning.
+
+The number of output channels may be increased to 32 by setting the parameter
+SERVO_32_ENABLE = 1. The SITL output packet is then
+
+```
+    uint16 magic = 29569
+    uint16 frame_rate
+    uint32 frame_count
+    uint16 pwm[32]
+```
+
+and uses a magic value 29569.  
+
 
 The frame rate represents the time step the simulation should take, this can be changed with the SIM_RATE_HZ ArduPilot parameter. The physics backend is free to ignore this value, a maximum time step size would typically be set. The SIM_RATE_HZ should value be kept above the vehicle loop rate, by default this 400hz on copter and quadplanes and 50 hz on plane and rover.
 
 The frame_count will increment for each output frame sent by ArduPilot, this count can be used to detect lost or duplicate frames. This count will be reset when SITL is re-started allowing the physics backend to reset the vehicle. If not input data is received after 10 seconds ArduPilot will re-send the output frame without incrementing the counter. This allows the physics model to be restarted and re-connect. Note that this may fill up the input buffer of the physics backend after some time. 
 
-PWM is a array of 16 servo values in micro seconds, typically in the 1000 to 2000 range as set by the servo output functions.
+PWM is a array of 16 (or 32) servo values in micro seconds, typically in the 1000 to 2000 range as set by the servo output functions.
 
 SITL input
 Data is received from the physics backend in a plain text JSON format. The data must contain the following fields: