From 3cb4d2ec3b4b4ae8145e25a7327c459924cf3d37 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Fri, 2 Feb 2024 15:18:04 -0700 Subject: [PATCH] AP_ExternalAHRS: Populate orientation from filter * Populating AHRS orientation from IMU was not correct Signed-off-by: Ryan Friedman --- libraries/AP_ExternalAHRS/AP_ExternalAHRS.h | 2 +- .../AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp | 6 +++--- libraries/AP_ExternalAHRS/MicroStrain_common.cpp | 10 ++++++++++ libraries/AP_ExternalAHRS/MicroStrain_common.h | 3 +++ 4 files changed, 17 insertions(+), 4 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index 72e3161f79..2d01f28c28 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -90,7 +90,7 @@ public: Vector3f accel; Vector3f gyro; - Quaternion quat; + Quaternion quat; // NED Location location; Vector3f velocity; Location origin; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp index 09762f67d6..d4ee38811c 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp @@ -134,9 +134,6 @@ void AP_ExternalAHRS_MicroStrain7::post_imu() const WITH_SEMAPHORE(state.sem); state.accel = imu_data.accel; state.gyro = imu_data.gyro; - - state.quat = imu_data.quat; - state.have_quaternion = true; } { @@ -188,6 +185,9 @@ void AP_ExternalAHRS_MicroStrain7::post_filter() const // Use GNSS 0 even though it may be bad. state.location = Location{filter_data.lat, filter_data.lon, gnss_data[0].msl_altitude, Location::AltFrame::ABSOLUTE}; state.have_location = true; + + state.quat = filter_data.attitude_quat; + state.have_quaternion = true; } for (int instance = 0; instance < NUM_GNSS_INSTANCES; instance++) { diff --git a/libraries/AP_ExternalAHRS/MicroStrain_common.cpp b/libraries/AP_ExternalAHRS/MicroStrain_common.cpp index 19a6e22509..7ced1123aa 100644 --- a/libraries/AP_ExternalAHRS/MicroStrain_common.cpp +++ b/libraries/AP_ExternalAHRS/MicroStrain_common.cpp @@ -58,6 +58,8 @@ enum class FilterPacketField { FILTER_STATUS = 0x10, LLH_POSITION = 0x01, NED_VELOCITY = 0x02, + // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/data/mip_field_filter_attitude_quaternion.htm + ATTITUDE_QUAT = 0x03, // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/shared_data/data/mip_field_shared_gps_timestamp.htm GPS_TIMESTAMP = 0xD3, }; @@ -288,6 +290,11 @@ void AP_MicroStrain::handle_filter(const MicroStrain_Packet &packet) filter_data.ned_velocity_down = be32tofloat_ptr(packet.payload, i+10); break; } + case FilterPacketField::ATTITUDE_QUAT: { + filter_data.attitude_quat = populate_quaternion(packet.payload, i+2); + filter_data.attitude_quat.normalize(); + break; + } case FilterPacketField::FILTER_STATUS: { filter_status.state = be16toh_ptr(&packet.payload[i+2]); filter_status.mode = be16toh_ptr(&packet.payload[i+4]); @@ -309,6 +316,9 @@ Vector3f AP_MicroStrain::populate_vector3f(const uint8_t *data, uint8_t offset) Quaternion AP_MicroStrain::populate_quaternion(const uint8_t *data, uint8_t offset) { + // https://github.com/clemense/quaternion-conventions + // AP follows W + Xi + Yj + Zk format. + // Microstrain follows the same return Quaternion { be32tofloat_ptr(data, offset), be32tofloat_ptr(data, offset+4), diff --git a/libraries/AP_ExternalAHRS/MicroStrain_common.h b/libraries/AP_ExternalAHRS/MicroStrain_common.h index b71027720d..9792a4f2ca 100644 --- a/libraries/AP_ExternalAHRS/MicroStrain_common.h +++ b/libraries/AP_ExternalAHRS/MicroStrain_common.h @@ -86,6 +86,9 @@ protected: float ned_velocity_east; float ned_velocity_down; float speed_accuracy; + // 4x1 vector representation of the quaternion describing the orientation of the device with respect to the NED local-level frame. + // NED [Qw, Qx, Qy, Qz] + Quaternion attitude_quat; } filter_data; enum class DescriptorSet {