SITL: Add Lidar Sensor for Airsim

Adds variable length array of vector3f parsing

libraries/SITL/SIM_AirSim.cpp

@@ -116,6 +116,54 @@ bool AirSim::parse_sensors(const char *json)
                 }
                 break;
             }
+
+            case DATA_VECTOR3F_ARRAY: {
+                // - array of floats that represent [x,y,z] coordinate for each point hit within the range
+                //       x0, y0, z0, x1, y1, z1, ..., xn, yn, zn
+                // example: [23.1,0.677024,1.4784,-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++;
+                    // Goto 3rd occurence of ,
+                    p = strchr(p,',');
+                    if (!p) {
+                        return false;
+                    }
+                    p++;
+                    p = strchr(p,',');
+                    if (!p) {
+                        return false;
+                    }
+                    p++;
+                    p = strchr(p,',');
+                    if (!p) {
+                        return false;
+                    }
+                    p++;
+                    // Reached end of point cloud
+                    if (p[0] == ']') {
+                        break;
+                    }
+                }
+                v->length = n;
+                break;
+            }
         }
     }
     return true;
@@ -149,15 +197,11 @@ void AirSim::recv_fdm()
         return;
     }
 
-    bool parse_ok = parse_sensors((const char *)(p1+1));
+    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);
 
-    if (!parse_ok) {
-        return;
-    }
-
     accel_body = Vector3f(state.imu.linear_acceleration[0],
                           state.imu.linear_acceleration[1],
                           state.imu.linear_acceleration[2]);
@@ -188,6 +232,8 @@ void AirSim::recv_fdm()
         }
     }
 
+    scanner.points = state.lidar.points;
+
 #if 0
     AP::logger().Write("ASM1", "TimeUS,TUS,R,P,Y,GX,GY,GZ",
                        "QQffffff",

libraries/SITL/SIM_AirSim.h

@@ -60,7 +60,7 @@ private:
 	bool parse_sensors(const char *json);
 
 	// buffer for parsing pose data in JSON format
-    uint8_t sensor_buffer[2048];
+    uint8_t sensor_buffer[65000];
     uint32_t sensor_buffer_len;
 
 	enum data_type {
@@ -68,6 +68,7 @@ private:
         DATA_FLOAT,
         DATA_DOUBLE,
         DATA_VECTOR3F,
+        DATA_VECTOR3F_ARRAY,
     };
 
     struct {
@@ -85,6 +86,9 @@ private:
         struct {
             Vector3f world_linear_velocity;
         } velocity;
+        struct {
+            struct vector3f_array points;
+        } lidar;
     } state, last_state;
 
     // table to aid parsing of JSON sensor data
@@ -93,7 +97,7 @@ private:
         const char *key;
         void *ptr;
         enum data_type type;
-    } keytable[10] = {
+    } keytable[11] = {
         { "", "timestamp", &state.timestamp, DATA_UINT64 },
         { "imu", "angular_velocity",    &state.imu.angular_velocity, DATA_VECTOR3F },
         { "imu", "linear_acceleration", &state.imu.linear_acceleration, DATA_VECTOR3F },
@@ -104,6 +108,7 @@ private:
         { "pose", "pitch", &state.pose.pitch, DATA_FLOAT },
         { "pose", "yaw",   &state.pose.yaw, DATA_FLOAT },
         { "velocity", "world_linear_velocity", &state.velocity.world_linear_velocity, DATA_VECTOR3F },
+        { "lidar", "point_cloud", &state.lidar.points, DATA_VECTOR3F_ARRAY },
     };
 };