diff --git a/libraries/SITL/SIM_AirSim.cpp b/libraries/SITL/SIM_AirSim.cpp index 5875031a17..3ee19fc834 100644 --- a/libraries/SITL/SIM_AirSim.cpp +++ b/libraries/SITL/SIM_AirSim.cpp @@ -164,6 +164,34 @@ bool AirSim::parse_sensors(const char *json) 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; + } } } return true; @@ -234,6 +262,11 @@ void AirSim::recv_fdm() scanner.points = state.lidar.points; + rcin_chan_count = state.rc.rc_channels.length < 8 ? state.rc.rc_channels.length : 8; + for (uint8_t i=0; i < rcin_chan_count; i++) { + rcin[i] = state.rc.rc_channels.data[i]; + } + #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 e03e3387b3..d87858840f 100644 --- a/libraries/SITL/SIM_AirSim.h +++ b/libraries/SITL/SIM_AirSim.h @@ -69,6 +69,7 @@ private: DATA_DOUBLE, DATA_VECTOR3F, DATA_VECTOR3F_ARRAY, + DATA_FLOAT_ARRAY, }; struct { @@ -89,6 +90,9 @@ private: struct { struct vector3f_array points; } lidar; + struct { + struct float_array rc_channels; + } rc; } state, last_state; // table to aid parsing of JSON sensor data @@ -97,7 +101,7 @@ private: const char *key; void *ptr; enum data_type type; - } keytable[11] = { + } keytable[12] = { { "", "timestamp", &state.timestamp, DATA_UINT64 }, { "imu", "angular_velocity", &state.imu.angular_velocity, DATA_VECTOR3F }, { "imu", "linear_acceleration", &state.imu.linear_acceleration, DATA_VECTOR3F }, @@ -109,6 +113,7 @@ private: { "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 }, + { "rc", "channels", &state.rc.rc_channels, DATA_FLOAT_ARRAY }, }; };