Browse Source

SITL: Add RC support for Airsim

Adds variable length float array parsing
master
Rajat Singhal 6 years ago committed by Andrew Tridgell
parent
commit
8e7ab23b4f
  1. 33
      libraries/SITL/SIM_AirSim.cpp
  2. 7
      libraries/SITL/SIM_AirSim.h

33
libraries/SITL/SIM_AirSim.cpp

@ -164,6 +164,34 @@ bool AirSim::parse_sensors(const char *json) @@ -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() @@ -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",

7
libraries/SITL/SIM_AirSim.h

@ -69,6 +69,7 @@ private: @@ -69,6 +69,7 @@ private:
DATA_DOUBLE,
DATA_VECTOR3F,
DATA_VECTOR3F_ARRAY,
DATA_FLOAT_ARRAY,
};
struct {
@ -89,6 +90,9 @@ private: @@ -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: @@ -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: @@ -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 },
};
};

Loading…
Cancel
Save