Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 15:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: JSON: support quaternion and euler attitude

Browse Source
This commit is contained in:
Iampete1 2020-06-30 19:06:50 +01:00 committed by Peter Barker
parent 564244ce9a
commit 6a2966c31b
2 changed files with 82 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
libraries/SITL/SIM_JSON.cpp
View File

@ -124,8 +124,10 @@ void JSON::output_servos(const struct sitl_input &input)
This parser does not do any syntax checking, and is not at all
general purpose
*/
bool JSON::parse_sensors(const char *json)
uint8_t JSON::parse_sensors(const char *json)
{
uint8_t received_bitmask = 0;
//printf("%s\n", json);
for (uint16_t i=0; i<ARRAY_SIZE(keytable); i++) {
struct keytable &key = keytable[i];
@ -142,10 +144,17 @@ bool JSON::parse_sensors(const char *json)
// find key inside section
p = strstr(p, key.key);
if (!p) {
printf("Failed to find key %s/%s\n", key.section, key.key);
return false;
if (key.required) {
printf("Failed to find key %s/%s\n", key.section, key.key);
return 0;
} else {
continue;
}
}
// record the keys that are found
received_bitmask |= 1U << i;
p += strlen(key.key)+2;
switch (key.type) {
case DATA_UINT64:
@ -167,15 +176,25 @@ bool JSON::parse_sensors(const char *json)
Vector3f *v = (Vector3f *)key.ptr;
if (sscanf(p, "[%f, %f, %f]", &v->x, &v->y, &v->z) != 3) {
printf("Failed to parse Vector3f for %s/%s\n", key.section, key.key);
return false;
return received_bitmask;
}
//printf("%s/%s = %f, %f, %f\n", key.section, key.key, v->x, v->y, v->z);
break;
}
case QUATERNION: {
Quaternion *v = static_cast<Quaternion*>(key.ptr);
if (sscanf(p, "[%f, %f, %f, %f]", &(v->q1), &(v->q2), &(v->q3), &(v->q4)) != 4) {
printf("Failed to parse Vector4f for %s/%s\n", key.section, key.key);
return received_bitmask;
}
break;
}
}
}
return true;
return received_bitmask;
}
/*
@ -215,23 +234,33 @@ void JSON::recv_fdm(const struct sitl_input &input)
return;
}
parse_sensors((const char *)(p1+1));
const uint8_t received_bitmask = parse_sensors((const char *)(p1+1));
if (received_bitmask == 0) {
// did not receve one of the required fields
return;
}
// Must get either attitude or quaternion fields
if ((received_bitmask & (EULER_ATT | QUAT_ATT)) == 0) {
printf("Did not receive attitude or quaternion\n");
return;
}
memmove(sensor_buffer, p2, sensor_buffer_len - (p2 - sensor_buffer));
sensor_buffer_len = sensor_buffer_len - (p2 - sensor_buffer);
accel_body = Vector3f(state.imu.accel_body[0],
state.imu.accel_body[1],
state.imu.accel_body[2]);
gyro = Vector3f(state.imu.gyro[0],
state.imu.gyro[1],
state.imu.gyro[2]);
velocity_ef = Vector3f(state.velocity[0],
state.velocity[1],
state.velocity[2]);
accel_body = state.imu.accel_body;
gyro = state.imu.gyro;
velocity_ef = state.velocity;
position = state.position;
// deal with euler or quaternion attitude
if ((received_bitmask & QUAT_ATT) != 0) {
// if we have a quaternion attitude use it rather than euler
state.quaternion.rotation_matrix(dcm);
} else {
dcm.from_euler(state.attitude[0], state.attitude[1], state.attitude[2]);
}
// velocity relative to airmass in body frame
velocity_air_bf = dcm.transposed() * velocity_ef;
@ -242,12 +271,6 @@ void JSON::recv_fdm(const struct sitl_input &input)
// airspeed as seen by a fwd pitot tube (limited to 120m/s)
airspeed_pitot = constrain_float(velocity_air_bf * Vector3f(1.0f, 0.0f, 0.0f), 0.0f, 120.0f);
position = Vector3f(state.position[0],
state.position[1],
state.position[2]);
dcm.from_euler(state.attitude[0], state.attitude[1], state.attitude[2]);
// Convert from a meters from origin physics to a lat long alt
update_position();
@ -272,6 +295,16 @@ void JSON::recv_fdm(const struct sitl_input &input)
frame_counter++;
#if 0
float roll, pitch, yaw;
if ((received_bitmask & QUAT_ATT) != 0) {
dcm.to_euler(&roll, &pitch, &yaw);
} else {
roll = state.attitude[0];
pitch = state.attitude[1];
yaw = state.attitude[2];
}
// @LoggerMessage: JSN1
// @Description: Log data received from JSON simulator
// @Field: TimeUS: Time since system startup (us)
@ -288,9 +321,9 @@ void JSON::recv_fdm(const struct sitl_input &input)
"Qfffffff",
AP_HAL::micros64(),
state.timestamp_s,
state.attitude[0],
state.attitude[1],
state.attitude[2],
roll,
pitch,
yaw,
gyro.x,
gyro.y,
gyro.z);

31
libraries/SITL/SIM_JSON.h
View File

@ -57,7 +57,7 @@ private:
void output_servos(const struct sitl_input &input);
void recv_fdm(const struct sitl_input &input);
bool parse_sensors(const char *json);
uint8_t parse_sensors(const char *json);
// buffer for parsing pose data in JSON format
uint8_t sensor_buffer[65000];
@ -68,6 +68,7 @@ private:
DATA_FLOAT,
DATA_DOUBLE,
DATA_VECTOR3F,
QUATERNION,
};
struct {
@ -78,6 +79,7 @@ private:
} imu;
Vector3f position;
Vector3f attitude;
Quaternion quaternion;
Vector3f velocity;
} state;
@ -87,13 +89,26 @@ private:
const char *key;
void *ptr;
enum data_type type;
} keytable[6] = {
{ "", "timestamp", &state.timestamp_s, DATA_DOUBLE },
{ "imu", "gyro", &state.imu.gyro, DATA_VECTOR3F },
{ "imu", "accel_body", &state.imu.accel_body, DATA_VECTOR3F },
{ "", "position", &state.position, DATA_VECTOR3F },
{ "", "attitude", &state.attitude, DATA_VECTOR3F },
{ "", "velocity", &state.velocity, DATA_VECTOR3F },
bool required;
} keytable[7] = {
{ "", "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 },
{ "", "position", &state.position, DATA_VECTOR3F, true },
{ "", "attitude", &state.attitude, DATA_VECTOR3F, false },
{ "", "quaternion", &state.quaternion, QUATERNION, false },
{ "", "velocity", &state.velocity, DATA_VECTOR3F, true },
};
// Enum coresponding to the ordering of keys in the keytable.
enum DataKey {
TIMESTAMP = 1U << 0,
GYRO = 1U << 1,
ACCEL_BODY = 1U << 2,
POSITION = 1U << 3,
EULER_ATT = 1U << 4,
QUAT_ATT = 1U << 5,
VELOCITY = 1U << 6,
};
};