mirror of https://github.com/ArduPilot/ardupilot
SITL: JSON add rangefinder support
This commit is contained in:
parent
537eec9091
commit
bdec15f708
|
@ -124,9 +124,9 @@ void JSON::output_servos(const struct sitl_input &input)
|
||||||
This parser does not do any syntax checking, and is not at all
|
This parser does not do any syntax checking, and is not at all
|
||||||
general purpose
|
general purpose
|
||||||
*/
|
*/
|
||||||
uint8_t JSON::parse_sensors(const char *json)
|
uint16_t JSON::parse_sensors(const char *json)
|
||||||
{
|
{
|
||||||
uint8_t received_bitmask = 0;
|
uint16_t received_bitmask = 0;
|
||||||
|
|
||||||
//printf("%s\n", json);
|
//printf("%s\n", json);
|
||||||
for (uint16_t i=0; i<ARRAY_SIZE(keytable); i++) {
|
for (uint16_t i=0; i<ARRAY_SIZE(keytable); i++) {
|
||||||
|
@ -234,9 +234,10 @@ void JSON::recv_fdm(const struct sitl_input &input)
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
const uint8_t received_bitmask = parse_sensors((const char *)(p1+1));
|
const uint16_t received_bitmask = parse_sensors((const char *)(p1+1));
|
||||||
if (received_bitmask == 0) {
|
if (received_bitmask == 0) {
|
||||||
// did not receve one of the required fields
|
// did not receve one of the mandatory fields
|
||||||
|
printf("Did not contain all mandatory fields\n");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -246,6 +247,20 @@ void JSON::recv_fdm(const struct sitl_input &input)
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (received_bitmask != last_received_bitmask) {
|
||||||
|
// some change in the message we have received, print what we got
|
||||||
|
printf("\nJSON received:\n");
|
||||||
|
for (uint16_t i=0; i<ARRAY_SIZE(keytable); i++) {
|
||||||
|
struct keytable &key = keytable[i];
|
||||||
|
if ((received_bitmask & 1U << i) == 0) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
printf("\t%s\n",key.key);
|
||||||
|
}
|
||||||
|
printf("\n");
|
||||||
|
}
|
||||||
|
last_received_bitmask = received_bitmask;
|
||||||
|
|
||||||
memmove(sensor_buffer, p2, sensor_buffer_len - (p2 - sensor_buffer));
|
memmove(sensor_buffer, p2, sensor_buffer_len - (p2 - sensor_buffer));
|
||||||
sensor_buffer_len = sensor_buffer_len - (p2 - sensor_buffer);
|
sensor_buffer_len = sensor_buffer_len - (p2 - sensor_buffer);
|
||||||
|
|
||||||
|
@ -274,11 +289,20 @@ void JSON::recv_fdm(const struct sitl_input &input)
|
||||||
// Convert from a meters from origin physics to a lat long alt
|
// Convert from a meters from origin physics to a lat long alt
|
||||||
update_position();
|
update_position();
|
||||||
|
|
||||||
|
// update range finder distances
|
||||||
|
for (uint8_t i=7; i<13; i++) {
|
||||||
|
if ((received_bitmask & 1U << i) == 0) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
rangefinder_m[i-7] = state.rng[i-7];
|
||||||
|
}
|
||||||
|
|
||||||
double deltat;
|
double deltat;
|
||||||
if (state.timestamp_s < last_timestamp_s) {
|
if (state.timestamp_s < last_timestamp_s) {
|
||||||
// Physics time has gone backwards, don't reset AP, assume an average size timestep
|
// Physics time has gone backwards, don't reset AP
|
||||||
printf("Detected physics reset\n");
|
printf("Detected physics reset\n");
|
||||||
deltat = 0;
|
deltat = 0;
|
||||||
|
last_received_bitmask = 0;
|
||||||
} else {
|
} else {
|
||||||
deltat = state.timestamp_s - last_timestamp_s;
|
deltat = state.timestamp_s - last_timestamp_s;
|
||||||
}
|
}
|
||||||
|
|
|
@ -57,7 +57,7 @@ private:
|
||||||
void output_servos(const struct sitl_input &input);
|
void output_servos(const struct sitl_input &input);
|
||||||
void recv_fdm(const struct sitl_input &input);
|
void recv_fdm(const struct sitl_input &input);
|
||||||
|
|
||||||
uint8_t parse_sensors(const char *json);
|
uint16_t parse_sensors(const char *json);
|
||||||
|
|
||||||
// buffer for parsing pose data in JSON format
|
// buffer for parsing pose data in JSON format
|
||||||
uint8_t sensor_buffer[65000];
|
uint8_t sensor_buffer[65000];
|
||||||
|
@ -81,6 +81,7 @@ private:
|
||||||
Vector3f attitude;
|
Vector3f attitude;
|
||||||
Quaternion quaternion;
|
Quaternion quaternion;
|
||||||
Vector3f velocity;
|
Vector3f velocity;
|
||||||
|
float rng[6];
|
||||||
} state;
|
} state;
|
||||||
|
|
||||||
// table to aid parsing of JSON sensor data
|
// table to aid parsing of JSON sensor data
|
||||||
|
@ -90,7 +91,7 @@ private:
|
||||||
void *ptr;
|
void *ptr;
|
||||||
enum data_type type;
|
enum data_type type;
|
||||||
bool required;
|
bool required;
|
||||||
} keytable[7] = {
|
} keytable[13] = {
|
||||||
{ "", "timestamp", &state.timestamp_s, DATA_DOUBLE, true },
|
{ "", "timestamp", &state.timestamp_s, DATA_DOUBLE, true },
|
||||||
{ "imu", "gyro", &state.imu.gyro, DATA_VECTOR3F, true },
|
{ "imu", "gyro", &state.imu.gyro, DATA_VECTOR3F, true },
|
||||||
{ "imu", "accel_body", &state.imu.accel_body, DATA_VECTOR3F, true },
|
{ "imu", "accel_body", &state.imu.accel_body, DATA_VECTOR3F, true },
|
||||||
|
@ -98,6 +99,12 @@ private:
|
||||||
{ "", "attitude", &state.attitude, DATA_VECTOR3F, false },
|
{ "", "attitude", &state.attitude, DATA_VECTOR3F, false },
|
||||||
{ "", "quaternion", &state.quaternion, QUATERNION, false },
|
{ "", "quaternion", &state.quaternion, QUATERNION, false },
|
||||||
{ "", "velocity", &state.velocity, DATA_VECTOR3F, true },
|
{ "", "velocity", &state.velocity, DATA_VECTOR3F, true },
|
||||||
|
{ "", "rng_1", &state.rng[0], DATA_FLOAT, false },
|
||||||
|
{ "", "rng_2", &state.rng[1], DATA_FLOAT, false },
|
||||||
|
{ "", "rng_3", &state.rng[2], DATA_FLOAT, false },
|
||||||
|
{ "", "rng_4", &state.rng[3], DATA_FLOAT, false },
|
||||||
|
{ "", "rng_5", &state.rng[4], DATA_FLOAT, false },
|
||||||
|
{ "", "rng_6", &state.rng[5], DATA_FLOAT, false },
|
||||||
};
|
};
|
||||||
|
|
||||||
// Enum coresponding to the ordering of keys in the keytable.
|
// Enum coresponding to the ordering of keys in the keytable.
|
||||||
|
@ -109,7 +116,14 @@ private:
|
||||||
EULER_ATT = 1U << 4,
|
EULER_ATT = 1U << 4,
|
||||||
QUAT_ATT = 1U << 5,
|
QUAT_ATT = 1U << 5,
|
||||||
VELOCITY = 1U << 6,
|
VELOCITY = 1U << 6,
|
||||||
|
RNG_1 = 1U << 7,
|
||||||
|
RNG_2 = 1U << 8,
|
||||||
|
RNG_3 = 1U << 9,
|
||||||
|
RNG_4 = 1U << 10,
|
||||||
|
RNG_5 = 1U << 11,
|
||||||
|
RNG_6 = 1U << 12,
|
||||||
};
|
};
|
||||||
|
uint16_t last_received_bitmask;
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue