SITL: added parsing of variable length vectors for Morse

used for laser scanner

libraries/SITL/SIM_Morse.cpp

@@ -137,17 +137,93 @@ bool Morse::parse_sensors(const char *json)
             return false;
         }
 
-        p += strlen(key.key)+2;
+        p += strlen(key.key)+3;
-        if (key.is_vector3) {
+        switch (key.type) {
-            p += 2;
+        case DATA_FLOAT:
-            if (sscanf(p, "%lf, %lf, %lf", &key.ptr[0], &key.ptr[1], &key.ptr[2]) != 3) {
+            *((float *)key.ptr) = atof(p);
-                printf("Failed to parse vector3 for %s/%s\n", key.section, key.key);
+            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]);
+            break;
-        } else {
+        }
-            key.ptr[0] = atof(p);
+
-            //printf("%s.%s %f\n", key.section, key.key, *key.ptr);
+        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++;

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];
+            Vector3f angular_velocity;
-            double linear_acceleration[3];
+            Vector3f linear_acceleration;
-            double magnetic_field[3];
+            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;
+        void *ptr;
-        bool is_vector3;
+        enum data_type type;
-    } keytable[11] = {
+    } keytable[13] = {
-        { "", "timestamp", &state.timestamp },
+        { "", "timestamp", &state.timestamp, DATA_DOUBLE },
-        { "vehicle.imu", "angular_velocity",    &state.imu.angular_velocity[0], true },
+        { "vehicle.imu", "angular_velocity",    &state.imu.angular_velocity, DATA_VECTOR3F },
-        { "vehicle.imu", "linear_acceleration", &state.imu.linear_acceleration[0], true },
+        { "vehicle.imu", "linear_acceleration", &state.imu.linear_acceleration, DATA_VECTOR3F },
-        { "vehicle.imu", "magnetic_field",      &state.imu.magnetic_field[0], true },
+        { "vehicle.imu", "magnetic_field",      &state.imu.magnetic_field, DATA_VECTOR3F },
-        { "vehicle.gps", "x", &state.gps.x },
+        { "vehicle.gps", "x", &state.gps.x, DATA_FLOAT },
-        { "vehicle.gps", "y", &state.gps.y },
+        { "vehicle.gps", "y", &state.gps.y, DATA_FLOAT },
-        { "vehicle.gps", "z", &state.gps.z },
+        { "vehicle.gps", "z", &state.gps.z, DATA_FLOAT },
-        { "vehicle.pose", "roll",  &state.pose.roll },
+        { "vehicle.pose", "roll",  &state.pose.roll, DATA_FLOAT },
-        { "vehicle.pose", "pitch", &state.pose.pitch },
+        { "vehicle.pose", "pitch", &state.pose.pitch, DATA_FLOAT },
-        { "vehicle.pose", "yaw",   &state.pose.yaw },
+        { "vehicle.pose", "yaw",   &state.pose.yaw, DATA_FLOAT },
-        { "vehicle.velocity", "world_linear_velocity", &state.velocity.world_linear_velocity[0], true },
+        { "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 },
     };
 };
 

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