/* 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" #if HAL_SIM_JSON_ENABLED #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) { sock.set_blocking(false); sock.reuseaddress(); if (strcmp("127.0.0.1",address) != 0) { target_ip = address; } control_port = port_out; 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_rate = rate_hz; pkt.frame_count = frame_counter; 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 */ uint32_t JSON::parse_sensors(const char *json) { uint32_t received_bitmask = 0; //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 received_bitmask; } //printf("%s/%s = %f, %f, %f\n", key.section, key.key, v->x, v->y, v->z); break; } case DATA_VECTOR3D: { Vector3d *v = (Vector3d *)key.ptr; if (sscanf(p, "[%lf, %lf, %lf]", &v->x, &v->y, &v->z) != 3) { printf("Failed to parse Vector3f for %s/%s\n", key.section, key.key); return received_bitmask; } //printf("%s/%s = %f, %f, %f\n", key.section, key.key, v->x, v->y, v->z); break; } case QUATERNION: { Quaternion *v = static_cast(key.ptr); if (sscanf(p, "[%f, %f, %f, %f]", &(v->q1), &(v->q2), &(v->q3), &(v->q4)) != 4) { printf("Failed to parse Vector4f for %s/%s\n", key.section, key.key); return received_bitmask; } 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; } } return received_bitmask; } /* 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; } const uint32_t received_bitmask = parse_sensors((const char *)(p1+1)); if (received_bitmask == 0) { // did not receive one of the mandatory fields printf("Did not contain all mandatory fields\n"); return; } // Must get either attitude or quaternion fields if ((received_bitmask & (EULER_ATT | QUAT_ATT)) == 0) { printf("Did not receive attitude or quaternion\n"); return; } if (received_bitmask != last_received_bitmask) { // some change in the message we have received, print what we got printf("\nJSON received:\n"); for (uint16_t i=0; iloop_rate_hz, rate_hz-1, rate_hz+1)); #if 0 // report frame rate if (frame_counter % 1000 == 0) { printf("FPS %.2f\n", rate_hz); // this is instantaneous rather than any clever average } #endif } #endif // HAL_SIM_JSON_ENABLED