SITL: support ride along instances via JSON

Tools/autotest/sim_vehicle.py

@@ -644,6 +644,8 @@ def start_vehicle(binary, opts, stuff, spawns=None):
     cmd.extend(["--speedup", str(opts.speedup)])
     if opts.sysid is not None:
         cmd.extend(["--sysid", str(opts.sysid)])
+    if opts.slave is not None:
+        cmd.extend(["--slave", str(opts.slave)])
     if opts.sitl_instance_args:
         # this could be a lot better:
         cmd.extend(opts.sitl_instance_args.split(" "))
@@ -1114,6 +1116,10 @@ def generate_frame_help():
 group_sim.add_option("--ekf-single",
                      action='store_true',
                      help="use single precision in EKF")
+group_sim.add_option("", "--slave",
+                     type='int',
+                     default=0,
+                     help="Set the number of JSON slave")
 parser.add_option_group(group_sim)
 
 

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();
     }

libraries/AP_HAL_SITL/SITL_State.h

@@ -308,7 +308,10 @@ class HALSITL::SITL_State {
 
     // 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};
 

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);

libraries/SITL/SIM_Aircraft.h

@@ -32,6 +32,7 @@
 #include "SIM_Buzzer.h"
 #include "SIM_Battery.h"
 #include <Filter/Filter.h>
+#include "SIM_JSON_Master.h"
 
 namespace SITL {
 

libraries/SITL/SIM_JSON.cpp

@@ -124,9 +124,9 @@ void JSON::output_servos(const struct sitl_input &input)
     This parser does not do any syntax checking, and is not at all
     general purpose
 */
-uint16_t JSON::parse_sensors(const char *json)
+uint32_t JSON::parse_sensors(const char *json)
 {
-    uint16_t received_bitmask = 0;
+    uint32_t received_bitmask = 0;
 
     //printf("%s\n", json);
     for (uint16_t i=0; i<ARRAY_SIZE(keytable); i++) {
@@ -203,6 +203,11 @@ uint16_t JSON::parse_sensors(const char *json)
                 break;
             }
 
+            case BOOLEAN:
+                *((bool *)key.ptr) = strtoull(p, nullptr, 10) != 0;
+                //printf("%s/%s = %i\n", key.section, key.key, *((unit8_t *)key.ptr));
+                break;
+
         }
     }
 
@@ -246,7 +251,7 @@ void JSON::recv_fdm(const struct sitl_input &input)
         return;
     }
 
-    const uint16_t received_bitmask = parse_sensors((const char *)(p1+1));
+    const uint32_t received_bitmask = parse_sensors((const char *)(p1+1));
     if (received_bitmask == 0) {
         // did not receve one of the mandatory fields
         printf("Did not contain all mandatory fields\n");
@@ -285,6 +290,7 @@ void JSON::recv_fdm(const struct sitl_input &input)
     velocity_ef = state.velocity;
     position = state.position;
     position.xy() += origin.get_distance_NE_double(home);
+    use_time_sync = !state.no_time_sync;
 
     // deal with euler or quaternion attitude
     if ((received_bitmask & QUAT_ATT) != 0) {
@@ -342,8 +348,9 @@ void JSON::recv_fdm(const struct sitl_input &input)
 
     if (is_positive(deltat) && deltat < 0.1) {
         // time in us to hz
-        adjust_frame_time(1.0 / deltat);
-
+        if (use_time_sync) {
+            adjust_frame_time(1.0 / deltat);
+        }
         // match actual frame rate with desired speedup
         time_advance();
     }
@@ -437,7 +444,7 @@ void JSON::update(const struct sitl_input &input)
 #if 0
     // report frame rate
     if (frame_counter % 1000 == 0) {
-        printf("FPS %.2f\n", achieved_rate_hz); // this is instantaneous rather than any clever average
+        printf("FPS %.2f\n", rate_hz); // this is instantaneous rather than any clever average
     }
 #endif
 }

libraries/SITL/SIM_JSON.h

@@ -57,7 +57,7 @@ class JSON : public Aircraft {
     void output_servos(const struct sitl_input &input);
     void recv_fdm(const struct sitl_input &input);
 
-    uint16_t parse_sensors(const char *json);
+    uint32_t parse_sensors(const char *json);
 
     // buffer for parsing pose data in JSON format
     uint8_t sensor_buffer[65000];
@@ -70,6 +70,7 @@ class JSON : public Aircraft {
         DATA_VECTOR3F,
         DATA_VECTOR3D,
         QUATERNION,
+        BOOLEAN,
     };
 
     struct {
@@ -88,6 +89,7 @@ class JSON : public Aircraft {
             float speed;
         } wind_vane_apparent;
         float airspeed;
+        bool no_time_sync;
     } state;
 
     // table to aid parsing of JSON sensor data
@@ -97,7 +99,7 @@ class JSON : public Aircraft {
         void *ptr;
         enum data_type type;
         bool required;
-    } keytable[16] = {
+    } keytable[17] = {
         { "", "timestamp", &state.timestamp_s, DATA_DOUBLE, true },
         { "imu", "gyro",    &state.imu.gyro, DATA_VECTOR3F, true },
         { "imu", "accel_body", &state.imu.accel_body, DATA_VECTOR3F, true },
@@ -114,6 +116,7 @@ class JSON : public Aircraft {
         {"windvane","direction", &state.wind_vane_apparent.direction, DATA_FLOAT, false},
         {"windvane","speed", &state.wind_vane_apparent.speed, DATA_FLOAT, false},
         {"", "airspeed", &state.airspeed, DATA_FLOAT, false},
+        {"", "no_time_sync", &state.no_time_sync, BOOLEAN, false},
     };
 
     // Enum coresponding to the ordering of keys in the keytable.
@@ -134,8 +137,9 @@ class JSON : public Aircraft {
         WIND_DIR    = 1U << 13,
         WIND_SPD    = 1U << 14,
         AIRSPEED    = 1U << 15,
+        TIME_SYNC   = 1U << 16,
     };
-    uint16_t last_received_bitmask;
+    uint32_t last_received_bitmask;
 };
 
 }

libraries/SITL/SIM_JSON_Master.cpp

@@ -0,0 +1,185 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  Send and receve JSON backend data to alow a second AP instance to ride along
+*/
+
+#include "SIM_JSON_Master.h"
+#include <AP_Logger/AP_Logger.h>
+#include <errno.h>
+
+using namespace SITL;
+
+void JSON_Master::init(const int32_t num_slaves)
+{
+    socket_list *list = &_list;
+    uint8_t i = 1;
+    for (i = 1 ; i <= num_slaves; i++) {
+        // init each socket and instance
+        list->instance = i;
+        uint16_t port = 9002 + 10 * i;
+
+        if (!list->sock_in.reuseaddress()) {
+            AP_HAL::panic("JSON master: socket reuseaddress failed on port: %d - %s\n", port, strerror(errno));
+        }
+
+        if (!list->sock_in.bind("127.0.0.1", port)) {
+            AP_HAL::panic("JSON master: socket reuseaddress failed on port: %d - %s\n", port, strerror(errno));
+        }
+
+        if (!list->sock_in.set_blocking(false)) {
+            AP_HAL::panic( "JSON master: socket set_blocking(false) failed on port: %d - %s\n", port, strerror(errno));
+        }
+
+        printf("Slave %u: listening on %u\n", list->instance, port);
+        list->next = new socket_list;
+        list = list->next;
+
+        initialized = true;
+    }
+}
+
+// Receive PWM outs from ride along controlers
+void JSON_Master::receive(struct sitl_input &input)
+{
+    if (!initialized) {
+        return;
+    }
+
+    uint8_t master_instance = AP::sitl()->ride_along_master.get();
+
+    for (socket_list *list = &_list; list->next; list=list->next) {
+        // cycle through all ride along instances
+        struct servo_packet {
+            uint16_t magic;
+            uint16_t frame_rate;
+            uint32_t frame_count;
+            uint16_t pwm[16];
+        } buffer;
+
+        while (true) {
+            ssize_t ret = list->sock_in.recv(&buffer, sizeof(buffer), 100);
+            if (ret == 0) {
+                // wait some more
+                continue;
+            }
+            if (buffer.magic != 18458) {
+                // magic value does not match
+                continue;
+            }
+            if (!list->connected) {
+                // connect back to the last address for send
+                uint16_t port;
+                const char *_ip = nullptr;
+                list->sock_in.last_recv_address(_ip, port);
+                list->connected = list->sock_out.connect(_ip, port) && (port != 0);
+                if (list->connected) {
+                    printf("Slave %u connected to %s:%u\n", list->instance, _ip, port);
+                }
+            }
+            if (list->connected) {
+                break;
+            }
+        }
+
+        const bool use_servos = list->instance == master_instance;
+
+// @LoggerMessage: SLV1
+// @Description: Log data received from JSON simulator 1
+// @Field: TimeUS: Time since system startup (us)
+// @Field: Instance: Slave instance
+// @Field: magic: magic JSON protocol key
+// @Field: frame_rate: Slave instance's desired frame rate
+// @Field: frame_count: Slave instance's current frame count
+// @Field: active: 1 if the servo outputs are being used from this instance
+        AP::logger().Write("SLV1", "TimeUS,Instance,magic,frame_rate,frame_count,active",
+                       "s#----",
+                       "F?????",
+                       "QBHHIB",
+                       AP_HAL::micros64(),
+                       list->instance,
+                       buffer.magic,
+                       buffer.frame_rate,
+                       buffer.frame_count,
+                       use_servos);
+
+// @LoggerMessage: SLV2
+// @Description: Log data received from JSON simulator 2
+// @Field: TimeUS: Time since system startup
+// @Field: Instance: Slave instance
+// @Field: C1: channel 1 output
+// @Field: C2: channel 2 output
+// @Field: C3: channel 3 output
+// @Field: C4: channel 4 output
+// @Field: C5: channel 5 output
+// @Field: C6: channel 6 output
+// @Field: C7: channel 7 output
+// @Field: C8: channel 8 output
+// @Field: C9: channel 9 output
+// @Field: C10: channel 10 output
+// @Field: C11: channel 11 output
+// @Field: C12: channel 12 output
+// @Field: C13: channel 13 output
+// @Field: C14: channel 14 output
+// @Field: C15: channel 15 output
+        AP::logger().Write("SLV2", "TimeUS,Instance,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,C12,C13,C14,C15",
+                       "s#YYYYYYYYYYYYYY",
+                       "F?--------------",
+                       "QBHHHHHHHHHHHHHH",
+                       AP_HAL::micros64(),
+                       list->instance,
+                       buffer.pwm[0],
+                       buffer.pwm[1],
+                       buffer.pwm[2],
+                       buffer.pwm[3],
+                       buffer.pwm[4],
+                       buffer.pwm[5],
+                       buffer.pwm[6],
+                       buffer.pwm[7],
+                       buffer.pwm[8],
+                       buffer.pwm[9],
+                       buffer.pwm[10],
+                       buffer.pwm[11],
+                       buffer.pwm[12],
+                       buffer.pwm[13],
+                       buffer.pwm[14]);
+
+        if (list->instance == master_instance) {
+            // Use the servo outs from this instance
+            memcpy(input.servos,buffer.pwm,sizeof(input.servos));
+        }
+    }
+}
+
+// send vehicle state to ride along controlers
+void JSON_Master::send(const struct sitl_fdm &output, const Vector3d &position)
+{
+    if (!initialized) {
+        return;
+    }
+
+    // message is the same to all slaves
+    int length = asprintf(&json_out,"\n{\"timestamp\":%f,\"imu\":{\"gyro\":[%f,%f,%f],\"accel_body\":[%f,%f,%f]},\"position\":[%f,%f,%f],\"quaternion\":[%f,%f,%f,%f],\"velocity\":[%f,%f,%f],\"no_time_sync\":1}\n",
+                            output.timestamp_us * 1e-6,
+                            radians(output.rollRate), radians(output.pitchRate), radians(output.yawRate),
+                            output.xAccel, output.yAccel, output.zAccel,
+                            position.x, position.y, position.z,
+                            output.quaternion.q1, output.quaternion.q2, output.quaternion.q3, output.quaternion.q4,
+                            output.speedN, output.speedE, output.speedD);
+
+    for (socket_list *list = &_list; list->next; list=list->next) {
+        list->sock_out.send(json_out,length);
+    }
+}