Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 17:53:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: Add RC support for Airsim

Browse Source 
Adds variable length float array parsing
This commit is contained in:
Rajat Singhal 2019-07-25 13:03:16 +05:30 committed by Andrew Tridgell
parent 2954f2b4d0
commit 8e7ab23b4f
2 changed files with 39 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/SITL/SIM_AirSim.cpp
View File

@ -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",

7
libraries/SITL/SIM_AirSim.h
View File

@ -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 },
};
};