diff --git a/libraries/SITL/SIM_AirSim.cpp b/libraries/SITL/SIM_AirSim.cpp index 0a3d0d8c0f..5875031a17 100644 --- a/libraries/SITL/SIM_AirSim.cpp +++ b/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", diff --git a/libraries/SITL/SIM_AirSim.h b/libraries/SITL/SIM_AirSim.h index f93d40bc84..e03e3387b3 100644 --- a/libraries/SITL/SIM_AirSim.h +++ b/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 }, }; };