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
|
||||
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);
|
||||
for (uint16_t i=0; i<ARRAY_SIZE(keytable); i++) {
|
||||
|
@ -234,9 +234,10 @@ void JSON::recv_fdm(const struct sitl_input &input)
|
|||
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) {
|
||||
// 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;
|
||||
}
|
||||
|
||||
|
@ -246,6 +247,20 @@ void JSON::recv_fdm(const struct sitl_input &input)
|
|||
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));
|
||||
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
|
||||
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;
|
||||
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");
|
||||
deltat = 0;
|
||||
last_received_bitmask = 0;
|
||||
} else {
|
||||
deltat = state.timestamp_s - last_timestamp_s;
|
||||
}
|
||||
|
|
|
@ -57,7 +57,7 @@ private:
|
|||
void output_servos(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
|
||||
uint8_t sensor_buffer[65000];
|
||||
|
@ -81,6 +81,7 @@ private:
|
|||
Vector3f attitude;
|
||||
Quaternion quaternion;
|
||||
Vector3f velocity;
|
||||
float rng[6];
|
||||
} state;
|
||||
|
||||
// table to aid parsing of JSON sensor data
|
||||
|
@ -90,7 +91,7 @@ private:
|
|||
void *ptr;
|
||||
enum data_type type;
|
||||
bool required;
|
||||
} keytable[7] = {
|
||||
} keytable[13] = {
|
||||
{ "", "timestamp", &state.timestamp_s, DATA_DOUBLE, true },
|
||||
{ "imu", "gyro", &state.imu.gyro, DATA_VECTOR3F, true },
|
||||
{ "imu", "accel_body", &state.imu.accel_body, DATA_VECTOR3F, true },
|
||||
|
@ -98,6 +99,12 @@ private:
|
|||
{ "", "attitude", &state.attitude, DATA_VECTOR3F, false },
|
||||
{ "", "quaternion", &state.quaternion, QUATERNION, false },
|
||||
{ "", "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.
|
||||
|
@ -109,7 +116,14 @@ private:
|
|||
EULER_ATT = 1U << 4,
|
||||
QUAT_ATT = 1U << 5,
|
||||
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