From 6a2966c31b587b01db91603a1fea9fc34223d996 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 30 Jun 2020 19:06:50 +0100 Subject: [PATCH] SITL: JSON: support quaternion and euler attitude --- libraries/SITL/SIM_JSON.cpp | 85 +++++++++++++++++++++++++------------ libraries/SITL/SIM_JSON.h | 31 ++++++++++---- 2 files changed, 82 insertions(+), 34 deletions(-) diff --git a/libraries/SITL/SIM_JSON.cpp b/libraries/SITL/SIM_JSON.cpp index 1b454d2bfc..fb787e373f 100644 --- a/libraries/SITL/SIM_JSON.cpp +++ b/libraries/SITL/SIM_JSON.cpp @@ -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; ix, &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(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); diff --git a/libraries/SITL/SIM_JSON.h b/libraries/SITL/SIM_JSON.h index 126859cd30..5fa28b0374 100644 --- a/libraries/SITL/SIM_JSON.h +++ b/libraries/SITL/SIM_JSON.h @@ -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, }; };