From 0c6efa5f261e02cfe47916ebb5ca9f9cefdab776 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 20 May 2020 02:01:39 +0100 Subject: [PATCH] SITL: add JSON backend --- libraries/SITL/SIM_JSON.cpp | 343 ++++++++++++++++++++++++++++++++++++ libraries/SITL/SIM_JSON.h | 103 +++++++++++ 2 files changed, 446 insertions(+) create mode 100644 libraries/SITL/SIM_JSON.cpp create mode 100644 libraries/SITL/SIM_JSON.h diff --git a/libraries/SITL/SIM_JSON.cpp b/libraries/SITL/SIM_JSON.cpp new file mode 100644 index 0000000000..d6ec0a1a1d --- /dev/null +++ b/libraries/SITL/SIM_JSON.cpp @@ -0,0 +1,343 @@ +/* + 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 . +*/ +/* + Simulator Connector for JSON based interfaces +*/ + +#include "SIM_JSON.h" + +#include +#include +#include + +#include +#include +#include + +#define UDP_TIMEOUT_MS 100 + +extern const AP_HAL::HAL& hal; + +using namespace SITL; + +static const struct { + const char *name; + float value; + bool save; +} sim_defaults[] = { + { "BRD_OPTIONS", 0}, + { "INS_GYR_CAL", 0 }, + { "INS_ACC2OFFS_X", 0.001 }, + { "INS_ACC2OFFS_Y", 0.001 }, + { "INS_ACC2OFFS_Z", 0.001 }, + { "INS_ACC2SCAL_X", 1.001 }, + { "INS_ACC2SCAL_Y", 1.001 }, + { "INS_ACC2SCAL_Z", 1.001 }, + { "INS_ACCOFFS_X", 0.001 }, + { "INS_ACCOFFS_Y", 0.001 }, + { "INS_ACCOFFS_Z", 0.001 }, + { "INS_ACCSCAL_X", 1.001 }, + { "INS_ACCSCAL_Y", 1.001 }, + { "INS_ACCSCAL_Z", 1.001 }, +}; + + +JSON::JSON(const char *frame_str) : + Aircraft(frame_str), + sock(true) +{ + printf("Starting SITL: JSON\n"); + + const char *colon = strchr(frame_str, ':'); + if (colon) { + target_ip = colon+1; + } + + for (uint8_t i=0; iconfigured()) { + p->save(); + } + } + } +} + +/* + Create & set in/out socket +*/ +void JSON::set_interface_ports(const char* address, const int port_in, const int port_out) +{ + if (!sock.bind("0.0.0.0", port_in)) { + printf("Unable to bind JSON sensor_in socket at port %u - Error: %s\n", + port_in, strerror(errno)); + return; + } + printf("Bind SITL sensor input at %s:%u\n", "127.0.0.1", port_in); + sock.set_blocking(false); + sock.reuseaddress(); + + if (strcmp("127.0.0.1",address) != 0) { + target_ip = address; + } + control_port = port_out; + sensor_port = port_in; + + printf("JSON control interface set to %s:%u\n", target_ip, control_port); +} + +/* + Decode and send servos +*/ +void JSON::output_servos(const struct sitl_input &input) +{ + servo_packet pkt; + pkt.frame_count = frame_counter; + pkt.speedup = get_speedup(); + for (uint8_t i=0; i<16; i++) { + pkt.pwm[i] = input.servos[i]; + } + + size_t send_ret = sock.sendto(&pkt, sizeof(pkt), target_ip, control_port); + 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)); + } + } +} + + +/* + very simple JSON parser for sensor data + called with pointer to one row of sensor data, nul terminated + + This parser does not do any syntax checking, and is not at all + general purpose +*/ +bool JSON::parse_sensors(const char *json) +{ + //printf("%s\n", json); + for (uint16_t i=0; ix, &v->y, &v->z) != 3) { + printf("Failed to parse Vector3f for %s/%s\n", key.section, key.key); + return false; + } + //printf("%s/%s = %f, %f, %f\n", key.section, key.key, v->x, v->y, v->z); + break; + } + + } + } + return true; +} + +/* + Receive new sensor data from simulator + This is a blocking function +*/ +void JSON::recv_fdm(const struct sitl_input &input) +{ + // Receive sensor packet + ssize_t ret = sock.recv(&sensor_buffer[sensor_buffer_len], sizeof(sensor_buffer)-sensor_buffer_len, UDP_TIMEOUT_MS); + uint32_t wait_ms = UDP_TIMEOUT_MS; + while (ret <= 0) { + //printf("No JSON sensor message received - %s\n", strerror(errno)); + ret = sock.recv(&sensor_buffer[sensor_buffer_len], sizeof(sensor_buffer)-sensor_buffer_len, UDP_TIMEOUT_MS); + wait_ms += UDP_TIMEOUT_MS; + // if no sensor message is received after 10 second resend servos, this help cope with SITL and the physics getting out of sync + if (wait_ms > 1000) { + wait_ms = 0; + printf("No JSON sensor message received, resending servos\n"); + output_servos(input); + } + } + + // convert '\n' into nul + while (uint8_t *p = (uint8_t *)memchr(&sensor_buffer[sensor_buffer_len], '\n', ret)) { + *p = 0; + } + sensor_buffer_len += ret; + + const uint8_t *p2 = (const uint8_t *)memrchr(sensor_buffer, 0, sensor_buffer_len); + if (p2 == nullptr || p2 == sensor_buffer) { + return; + } + + const uint8_t *p1 = (const uint8_t *)memrchr(sensor_buffer, 0, p2 - sensor_buffer); + if (p1 == nullptr) { + return; + } + + parse_sensors((const char *)(p1+1)); + + memmove(sensor_buffer, p2, sensor_buffer_len - (p2 - sensor_buffer)); + sensor_buffer_len = sensor_buffer_len - (p2 - sensor_buffer); + + accel_body = Vector3f(state.imu.accel_body[0], + state.imu.accel_body[1], + state.imu.accel_body[2]); + + gyro = Vector3f(state.imu.gyro[0], + state.imu.gyro[1], + state.imu.gyro[2]); + + velocity_ef = Vector3f(state.velocity[0], + state.velocity[1], + state.velocity[2]); + + position = Vector3f(state.position[0], + state.position[1], + state.position[2]); + + dcm.from_euler(state.attitude[0], state.attitude[1], state.attitude[2]); + + // Convert from a meters from origin physics to a lat long alt + update_position(); + + if (last_timestamp) { + int deltat; + if (state.timestamp < last_timestamp) { + // Physics time has gone backwards, don't reset AP, assume an average size timestep + printf("Detected physics reset\n"); + deltat = average_frame_time; + } else { + deltat = state.timestamp - last_timestamp; + } + time_now_us += deltat; + + if (deltat > 0 && deltat < 100000) { + if (average_frame_time < 1) { + average_frame_time = deltat; + } + average_frame_time = average_frame_time * 0.98 + deltat * 0.02; + } + } + +#if 0 +// @LoggerMessage: JSN1 +// @Description: Log data received from JSON simulator +// @Field: TimeUS: Time since system startup +// @Field: TUS: Simulation's timestamp +// @Field: R: Simulation's roll +// @Field: P: Simulation's pitch +// @Field: Y: Simulation's yaw +// @Field: GX: Simulated gyroscope, X-axis +// @Field: GY: Simulated gyroscope, Y-axis +// @Field: GZ: Simulated gyroscope, Z-axis + AP::logger().Write("JSN1", "TimeUS,TUS,R,P,Y,GX,GY,GZ", + "QQffffff", + AP_HAL::micros64(), + state.timestamp, + degrees(state.pose.roll), + degrees(state.pose.pitch), + degrees(state.pose.yaw), + degrees(gyro.x), + degrees(gyro.y), + degrees(gyro.z)); + + Vector3f velocity_bf = dcm.transposed() * velocity_ef; + position = home.get_distance_NED(location); + +// @LoggerMessage: JSN2 +// @Description: Log data received from JSON simulator +// @Field: TimeUS: Time since system startup +// @Field: AX: simulation's acceleration, X-axis +// @Field: AY: simulation's acceleration, Y-axis +// @Field: AZ: simulation's acceleration, Z-axis +// @Field: VX: simulation's velocity, X-axis +// @Field: VY: simulation's velocity, Y-axis +// @Field: VZ: simulation's velocity, Z-axis +// @Field: PX: simulation's position, X-axis +// @Field: PY: simulation's position, Y-axis +// @Field: PZ: simulation's position, Z-axis +// @Field: Alt: simulation's gps altitude +// @Field: SD: simulation's earth-frame speed-down + AP::logger().Write("JSN2", "TimeUS,AX,AY,AZ,VX,VY,VZ,PX,PY,PZ,Alt,SD", + "Qfffffffffff", + AP_HAL::micros64(), + accel_body.x, + accel_body.y, + accel_body.z, + velocity_bf.x, + velocity_bf.y, + velocity_bf.z, + position.x, + position.y, + position.z, + state.gps.alt, + velocity_ef.z); +#endif + + last_timestamp = state.timestamp; + frame_counter++; +} + +/* + update the JSON simulation by one time step +*/ +void JSON::update(const struct sitl_input &input) +{ + // send to JSON model + output_servos(input); + + // receive from JSON model + recv_fdm(input); + + // update magnetic field + // as the model does not provide mag feild we calculate it from position and attitude + update_mag_field_bf(); +} diff --git a/libraries/SITL/SIM_JSON.h b/libraries/SITL/SIM_JSON.h new file mode 100644 index 0000000000..4ecd6ccfdb --- /dev/null +++ b/libraries/SITL/SIM_JSON.h @@ -0,0 +1,103 @@ +/* + 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 . +*/ +#pragma once + +#include +#include "SIM_Aircraft.h" + +namespace SITL { + +class JSON : public Aircraft { +public: + JSON(const char *frame_str); + + /* update model by one time step */ + void update(const struct sitl_input &input) override; + + /* static object creator */ + static Aircraft *create(const char *frame_str) { + return new JSON(frame_str); + } + + /* Create and set in/out socket for JSON generic simulator */ + void set_interface_ports(const char* address, const int port_in, const int port_out) override; + +private: + + struct servo_packet { + uint32_t frame_count; + float speedup; + uint16_t pwm[16]; + }; + + // default connection_info_.ip_address + const char *target_ip = "127.0.0.1"; + + // default connection_info_.ip_port + uint16_t sensor_port = 9003; + + // default connection_info_.sitl_ip_port + uint16_t control_port = 9002; + + SocketAPM sock; + + double average_frame_time; + uint32_t frame_counter; + uint64_t last_timestamp; + + void output_servos(const struct sitl_input &input); + void recv_fdm(const struct sitl_input &input); + + bool parse_sensors(const char *json); + + // buffer for parsing pose data in JSON format + uint8_t sensor_buffer[65000]; + uint32_t sensor_buffer_len; + + enum data_type { + DATA_UINT64, + DATA_FLOAT, + DATA_DOUBLE, + DATA_VECTOR3F, + }; + + struct { + uint64_t timestamp; + struct { + Vector3f gyro; + Vector3f accel_body; + } imu; + Vector3f position; + Vector3f attitude; + Vector3f velocity; + } state; + + // table to aid parsing of JSON sensor data + struct keytable { + const char *section; + const char *key; + void *ptr; + enum data_type type; + } keytable[6] = { + { "", "timestamp", &state.timestamp, DATA_UINT64 }, + { "imu", "gyro", &state.imu.gyro, DATA_VECTOR3F }, + { "imu", "accel_body", &state.imu.accel_body, DATA_VECTOR3F }, + { "", "position", &state.position, DATA_VECTOR3F }, + { "", "attitude", &state.attitude, DATA_VECTOR3F }, + { "", "velocity", &state.velocity, DATA_VECTOR3F }, + }; +}; + +}