Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: add JSON backend

This commit is contained in:
Iampete1 2020-05-20 02:01:39 +01:00 committed by Andrew Tridgell
parent c31a4f4b14
commit 0c6efa5f26
2 changed files with 446 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

343
libraries/SITL/SIM_JSON.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
Simulator Connector for JSON based interfaces
*/
#include "SIM_JSON.h"
#include <stdio.h>
#include <arpa/inet.h>
#include <errno.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_HAL/utility/replace.h>
#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; i<ARRAY_SIZE(sim_defaults); i++) {
AP_Param::set_default_by_name(sim_defaults[i].name, sim_defaults[i].value);
if (sim_defaults[i].save) {
enum ap_var_type ptype;
AP_Param *p = AP_Param::find(sim_defaults[i].name, &ptype);
if (!p->configured()) {
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; i<ARRAY_SIZE(keytable); i++) {
struct keytable &key = keytable[i];
/* look for section header */
const char *p = strstr(json, key.section);
if (!p) {
// we don't have this sensor
printf("Failed to find %s\n", key.section);
continue;
}
p += strlen(key.section)+1;
// find key inside section
p = strstr(p, key.key);
if (!p) {
printf("Failed to find key %s/%s\n", key.section, key.key);
return false;
}
p += strlen(key.key)+2;
switch (key.type) {
case DATA_UINT64:
*((uint64_t *)key.ptr) = atof(p); // using atof rather than strtoul means we support scientific notation
//printf("%s/%s = %lu\n", key.section, key.key, *((uint64_t *)key.ptr));
break;
case DATA_FLOAT:
*((float *)key.ptr) = atof(p);
//printf("%s/%s = %f\n", key.section, key.key, *((float *)key.ptr));
break;
case DATA_DOUBLE:
*((double *)key.ptr) = atof(p);
//printf("%s/%s = %f\n", key.section, key.key, *((double *)key.ptr));
break;
case DATA_VECTOR3F: {
Vector3f *v = (Vector3f *)key.ptr;
if (sscanf(p, "[%f, %f, %f]", &v->x, &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();
}

103
libraries/SITL/SIM_JSON.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <AP_HAL/utility/Socket.h>
#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 },
};
};
}