From f52af1fc4035661c37c0c70ef6d9b2d58b883048 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 4 Dec 2018 15:06:53 +1100 Subject: [PATCH] SITL: added parsing of variable length vectors for Morse used for laser scanner --- libraries/SITL/SIM_Morse.cpp | 94 ++++++++++++++++++++++++++++++++---- libraries/SITL/SIM_Morse.h | 56 +++++++++++++-------- libraries/SITL/SITL.h | 17 +++++++ 3 files changed, 137 insertions(+), 30 deletions(-) diff --git a/libraries/SITL/SIM_Morse.cpp b/libraries/SITL/SIM_Morse.cpp index 24fc598fee..1c6ddcbc63 100644 --- a/libraries/SITL/SIM_Morse.cpp +++ b/libraries/SITL/SIM_Morse.cpp @@ -137,17 +137,93 @@ bool Morse::parse_sensors(const char *json) return false; } - p += strlen(key.key)+2; - if (key.is_vector3) { - p += 2; - if (sscanf(p, "%lf, %lf, %lf", &key.ptr[0], &key.ptr[1], &key.ptr[2]) != 3) { - printf("Failed to parse vector3 for %s/%s\n", key.section, key.key); + p += strlen(key.key)+3; + switch (key.type) { + case DATA_FLOAT: + *((float *)key.ptr) = atof(p); + break; + + case DATA_DOUBLE: + *((double *)key.ptr) = atof(p); + 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, key.ptr[0], key.ptr[1], key.ptr[2]); - } else { - key.ptr[0] = atof(p); - //printf("%s.%s %f\n", key.section, key.key, *key.ptr); + break; + } + + case DATA_VECTOR3F_ARRAY: { + // example: [[0.0, 0.0, 0.0], [-8.97607135772705, -8.976069450378418, -8.642673492431641e-07]] + if (*p++ != '[') { + return false; + } + uint16_t n = 0; + struct vector3f_array *v = (struct vector3f_array *)key.ptr; + while (true) { + if (n >= v->length) { + Vector3f *d = (Vector3f *)realloc(v->data, sizeof(Vector3f)*(n+1)); + if (d == nullptr) { + return false; + } + v->data = d; + v->length = n+1; + } + if (sscanf(p, "[%f, %f, %f]", &v->data[n].x, &v->data[n].y, &v->data[n].z) != 3) { + printf("Failed to parse Vector3f for %s/%s[%u]\n", key.section, key.key, n); + return false; + } + n++; + p = strchr(p,']'); + if (!p) { + return false; + } + p++; + if (p[0] != ',') { + break; + } + if (p[1] != ' ') { + return false; + } + p += 2; + } + if (p[0] != ']') { + return false; + } + v->length = n; + break; + } + + case DATA_FLOAT_ARRAY: { + // example: [18.0, 12.694079399108887] + if (*p++ != '[') { + return false; + } + uint16_t n = 0; + struct float_array *v = (struct float_array *)key.ptr; + while (true) { + if (n >= v->length) { + float *d = (float *)realloc(v->data, sizeof(float)*(n+1)); + if (d == nullptr) { + return false; + } + v->data = d; + v->length = n+1; + } + v->data[n] = atof(p); + n++; + p = strchr(p,','); + if (!p) { + break; + } + p++; + } + v->length = n; + break; + } } } socket_frame_counter++; diff --git a/libraries/SITL/SIM_Morse.h b/libraries/SITL/SIM_Morse.h index ca347b5390..df8e594bb6 100644 --- a/libraries/SITL/SIM_Morse.h +++ b/libraries/SITL/SIM_Morse.h @@ -60,7 +60,7 @@ private: void report_FPS(); // buffer for parsing pose data in JSON format - uint8_t sensor_buffer[2048]; + uint8_t sensor_buffer[4096]; uint32_t sensor_buffer_len; SocketAPM *sensors_sock; @@ -81,42 +81,56 @@ private: uint64_t frame_counter; double last_frame_count_s; + enum data_type { + DATA_FLOAT, + DATA_DOUBLE, + DATA_VECTOR3F, + DATA_VECTOR3F_ARRAY, + DATA_FLOAT_ARRAY, + }; + struct { double timestamp; struct { - double angular_velocity[3]; - double linear_acceleration[3]; - double magnetic_field[3]; + Vector3f angular_velocity; + Vector3f linear_acceleration; + Vector3f magnetic_field; } imu; struct { - double x, y, z; + float x, y, z; } gps; struct { - double roll, pitch, yaw; + float roll, pitch, yaw; } pose; struct { - double world_linear_velocity[3]; + Vector3f world_linear_velocity; } velocity; + struct { + struct vector3f_array points; + struct float_array ranges; + } scanner; } state, last_state; // table to aid parsing of JSON sensor data struct keytable { const char *section; const char *key; - double *ptr; - bool is_vector3; - } keytable[11] = { - { "", "timestamp", &state.timestamp }, - { "vehicle.imu", "angular_velocity", &state.imu.angular_velocity[0], true }, - { "vehicle.imu", "linear_acceleration", &state.imu.linear_acceleration[0], true }, - { "vehicle.imu", "magnetic_field", &state.imu.magnetic_field[0], true }, - { "vehicle.gps", "x", &state.gps.x }, - { "vehicle.gps", "y", &state.gps.y }, - { "vehicle.gps", "z", &state.gps.z }, - { "vehicle.pose", "roll", &state.pose.roll }, - { "vehicle.pose", "pitch", &state.pose.pitch }, - { "vehicle.pose", "yaw", &state.pose.yaw }, - { "vehicle.velocity", "world_linear_velocity", &state.velocity.world_linear_velocity[0], true }, + void *ptr; + enum data_type type; + } keytable[13] = { + { "", "timestamp", &state.timestamp, DATA_DOUBLE }, + { "vehicle.imu", "angular_velocity", &state.imu.angular_velocity, DATA_VECTOR3F }, + { "vehicle.imu", "linear_acceleration", &state.imu.linear_acceleration, DATA_VECTOR3F }, + { "vehicle.imu", "magnetic_field", &state.imu.magnetic_field, DATA_VECTOR3F }, + { "vehicle.gps", "x", &state.gps.x, DATA_FLOAT }, + { "vehicle.gps", "y", &state.gps.y, DATA_FLOAT }, + { "vehicle.gps", "z", &state.gps.z, DATA_FLOAT }, + { "vehicle.pose", "roll", &state.pose.roll, DATA_FLOAT }, + { "vehicle.pose", "pitch", &state.pose.pitch, DATA_FLOAT }, + { "vehicle.pose", "yaw", &state.pose.yaw, DATA_FLOAT }, + { "vehicle.velocity", "world_linear_velocity", &state.velocity.world_linear_velocity, DATA_VECTOR3F }, + { "vehicle.scan", "point_list", &state.scanner.points, DATA_VECTOR3F_ARRAY }, + { "vehicle.scan", "range_list", &state.scanner.ranges, DATA_FLOAT_ARRAY }, }; }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 1f0a678057..2dda00152d 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -11,6 +11,17 @@ class DataFlash_Class; namespace SITL { +struct vector3f_array { + uint16_t length; + Vector3f *data; +}; + +struct float_array { + uint16_t length; + float *data; +}; + + struct sitl_fdm { // this is the structure passed between FDM models and the main SITL code uint64_t timestamp_us; @@ -33,6 +44,12 @@ struct sitl_fdm { double range; // rangefinder value Vector3f bodyMagField; // Truth XYZ magnetic field vector in body-frame. Includes motor interference. Units are milli-Gauss. Vector3f angAccel; // Angular acceleration in degrees/s/s about the XYZ body axes + + struct { + // data from simulated laser scanner, if available + struct vector3f_array points; + struct float_array ranges; + } scanner; }; // number of rc output channels