AP_ExternalAHRS: Populate orientation from filter

* Populating AHRS orientation from IMU was not correct

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

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;

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++) {

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),

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 {