diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 5713790377..a0e7f7af31 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 0000000000..3c95195319 --- /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 0000000000..b1f0b13933 --- /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 cde99b9725..76129877e1 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