From 5ff0f423725d5d1f77ac0bb7032834648950ab7c Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sun, 25 Apr 2021 01:31:35 +0100
Subject: [PATCH] AP_HAL_SITL: add slave JSON instances

---
 libraries/AP_HAL_SITL/SITL_State.cpp   |  6 ++++++
 libraries/AP_HAL_SITL/SITL_State.h     |  5 ++++-
 libraries/AP_HAL_SITL/SITL_cmdline.cpp | 10 ++++++++++
 3 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp
index 347f495c96..7e347c6103 100644
--- a/libraries/AP_HAL_SITL/SITL_State.cpp
+++ b/libraries/AP_HAL_SITL/SITL_State.cpp
@@ -613,6 +613,9 @@ void SITL_State::_fdm_input_local(void)
     // construct servos structure for FDM
     _simulator_servos(input);
 
+    // read servo inputs from ride along flight controllers
+    ride_along.receive(input);
+
     // update the model
     sitl_model->update_model(input);
 
@@ -628,6 +631,9 @@ void SITL_State::_fdm_input_local(void)
         }
     }
 
+    // output JSON state to ride along flight controllers
+    ride_along.send(_sitl->state,sitl_model->get_position_relhome());
+
     if (gimbal != nullptr) {
         gimbal->update();
     }
diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h
index b6e2e9165a..2d450032d3 100644
--- a/libraries/AP_HAL_SITL/SITL_State.h
+++ b/libraries/AP_HAL_SITL/SITL_State.h
@@ -308,7 +308,10 @@ private:
 
     // simulated VectorNav system:
     SITL::VectorNav *vectornav;
-    
+
+    // Ride along instances via JSON SITL backend
+    SITL::JSON_Master ride_along;
+
     // output socket for flightgear viewing
     SocketAPM fg_socket{true};
     
diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp
index c32e810e59..afacad45d7 100644
--- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp
+++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp
@@ -113,6 +113,7 @@ void SITL_State::_usage(void)
            "\t--irlock-port PORT       set port num for irlock\n"
            "\t--start-time TIMESTR     set simulation start time in UNIX timestamp\n"
            "\t--sysid ID               set SYSID_THISMAV\n"
+           "\t--slave number           set the number of JSON slaves\n"
         );
 }
 
@@ -260,6 +261,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
         CMDLINE_IRLOCK_PORT,
         CMDLINE_START_TIME,
         CMDLINE_SYSID,
+        CMDLINE_SLAVE,
     };
 
     const struct GetOptLong::option options[] = {
@@ -307,6 +309,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
         {"irlock-port",     true,   0, CMDLINE_IRLOCK_PORT},
         {"start-time",      true,   0, CMDLINE_START_TIME},
         {"sysid",           true,   0, CMDLINE_SYSID},
+        {"slave",           true,   0, CMDLINE_SLAVE},
         {0, false, 0, 0}
     };
 
@@ -462,6 +465,13 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
         case 'h':
             _usage();
             exit(0);
+        case CMDLINE_SLAVE: {
+            const int32_t slaves = atoi(gopt.optarg);
+            if (slaves > 0) {
+                ride_along.init(slaves);
+            }
+            break;
+        }
         default:
             _usage();
             exit(1);