From 9e0c2237dac7ab0a4c50f32ed57a6e8542a9f8ec Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 25 Apr 2021 01:31:35 +0100 Subject: [PATCH 1/5] 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 347f495c96a80..7e347c6103e9a 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 b6e2e9165a0a6..2d450032d3ccc 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/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}; diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index c32e810e592e8..afacad45d71f8 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); From 3d7bb011dbe8c8866516c2fd57dd7924bf6cd351 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 25 Apr 2021 01:32:27 +0100 Subject: [PATCH 2/5] SITL: SIM_JSON: add no time sync to JSON format --- libraries/SITL/SIM_JSON.cpp | 19 +++++++++++++------ libraries/SITL/SIM_JSON.h | 10 +++++++--- 2 files changed, 20 insertions(+), 9 deletions(-) diff --git a/libraries/SITL/SIM_JSON.cpp b/libraries/SITL/SIM_JSON.cpp index 8ec04a73783a1..229ac1dda0d77 100644 --- a/libraries/SITL/SIM_JSON.cpp +++ b/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 Date: Sun, 25 Apr 2021 01:33:30 +0100 Subject: [PATCH 3/5] SITL: add SIM_JSON_MASTER param --- libraries/SITL/SITL.cpp | 4 ++++ libraries/SITL/SITL.h | 3 +++ 2 files changed, 7 insertions(+) diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 8d8c14e4fe8ab..ab3051c917134 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -437,6 +437,10 @@ const AP_Param::GroupInfo SITL::var_ins[] = { // @Description: 0: mainsail with sheet, 1: directly actuated wing AP_GROUPINFO("SAIL_TYPE", 26, SITL, sail_type, 0), + // @Param: JSON_MASTER + // @DisplayName: JSON master instance + // @Description: the instance number to take servos from + AP_GROUPINFO("JSON_MASTER", 27, SITL, ride_along_master, 0), // the IMUT parameters must be last due to the enable parameters AP_SUBGROUPINFO(imu_tcal[0], "IMUT1_", 61, SITL, AP_InertialSensor::TCal), diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 63ab85e173a6c..cde99b97250d0 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -475,6 +475,9 @@ class SITL { // Sailboat sim only AP_Int8 sail_type; + // Master instance to use servos from with slave instances + AP_Int8 ride_along_master; + }; } // namespace SITL From 5f056e350d68c4f38b407177a246413ffe736512 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 25 Apr 2021 01:33:58 +0100 Subject: [PATCH 4/5] SITL: add JSON Master --- libraries/SITL/SIM_Aircraft.h | 1 + libraries/SITL/SIM_JSON_Master.cpp | 185 +++++++++++++++++++++++++++++ libraries/SITL/SIM_JSON_Master.h | 56 +++++++++ libraries/SITL/SITL.h | 2 +- 4 files changed, 243 insertions(+), 1 deletion(-) create mode 100644 libraries/SITL/SIM_JSON_Master.cpp create mode 100644 libraries/SITL/SIM_JSON_Master.h diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 57137903772de..a0e7f7af31cb2 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -32,6 +32,7 @@ #include "SIM_Buzzer.h" #include "SIM_Battery.h" #include +#include "SIM_JSON_Master.h" namespace SITL { diff --git a/libraries/SITL/SIM_JSON_Master.cpp b/libraries/SITL/SIM_JSON_Master.cpp new file mode 100644 index 0000000000000..3c95195319a65 --- /dev/null +++ b/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 . + */ +/* + Send and receve JSON backend data to alow a second AP instance to ride along +*/ + +#include "SIM_JSON_Master.h" +#include +#include + +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); + } +} diff --git a/libraries/SITL/SIM_JSON_Master.h b/libraries/SITL/SIM_JSON_Master.h new file mode 100644 index 0000000000000..b1f0b13933afa --- /dev/null +++ b/libraries/SITL/SIM_JSON_Master.h @@ -0,0 +1,56 @@ +/* + 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 . + */ +/* + Send and receve JSON backend data to alow a second AP instance to ride along +*/ + +#pragma once + +#include "SITL_Input.h" +#include +#include + +namespace SITL { + +class JSON_Master { +public: + JSON_Master() {}; + + // setup sockets + void init(const int32_t num_slaves); + + // Receive PWM outs from ride along controlers + void receive(struct sitl_input &input); + + // send vehicle state to ride along controlers + void send(const struct sitl_fdm &output, const Vector3d &position); + +private: + + struct socket_list { + SocketAPM sock_in{true}; + SocketAPM sock_out{true}; + uint8_t instance; + bool connected; + socket_list *next; + } _list; + + char *json_out; + + bool initialized; + +}; + +} diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index cde99b97250d0..76129877e1e43 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -51,7 +51,7 @@ struct sitl_fdm { double heading; // degrees double speedN, speedE, speedD; // m/s double xAccel, yAccel, zAccel; // m/s/s in body frame - double rollRate, pitchRate, yawRate; // degrees/s/s in body frame + double rollRate, pitchRate, yawRate; // degrees/s in body frame double rollDeg, pitchDeg, yawDeg; // euler angles, degrees Quaternion quaternion; double airspeed; // m/s From ebf0bb906beaa9d168973bf15b07fef6aaae5e28 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 25 Apr 2021 01:34:27 +0100 Subject: [PATCH 5/5] Tools: sim_vehicle: pass on slave comandline CMD --- Tools/autotest/sim_vehicle.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/Tools/autotest/sim_vehicle.py b/Tools/autotest/sim_vehicle.py index c67dc0d618b24..39301b0319a4a 100755 --- a/Tools/autotest/sim_vehicle.py +++ b/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)