AP_ExternalAHRS: Populate Microstrain7 orientation from filter

libraries/AP_ExternalAHRS/AP_ExternalAHRS.h

@@ -90,7 +90,7 @@ class AP_ExternalAHRS {
 
         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 @@ class AP_MicroStrain
         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 {

libraries/SITL/SIM_MicroStrain.cpp

@@ -13,7 +13,15 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 /*
-    simulate MicroStrain GNSS-INS devices
+    Simulate MicroStrain CX5 GNSS-INS devices
+    
+    Usage:
+    PARAMS:
+        param set AHRS_EKF_TYPE 11
+        param set EAHRS_TYPE 2
+        param set SERIAL3_PROTOCOL 36
+        param set SERIAL3_BAUD 115
+    sim_vehicle.py -v Plane -A "--serial3=sim:MicroStrain5" --console --map -DG
 */
 #include "SIM_MicroStrain.h"
 #include <stdio.h>

libraries/SITL/SIM_MicroStrain.h

@@ -1,12 +1,5 @@
 // Created by Asa Davis and Davis Schenkenberger on 23rd September 21.
 
-//usage:
-//PARAMS:
-// param set AHRS_EKF_TYPE 11
-// param set EAHRS_TYPE 2
-// param set SERIAL3_PROTOCOL 36
-// param set SERIAL3_BAUD 115
-// sim_vehicle.py -v Plane -A "--serial3=sim:MicroStrain7" --console --map -DG
 #pragma once
 
 #include "SIM_Aircraft.h"

libraries/SITL/SIM_MicroStrain7.cpp

@@ -13,7 +13,15 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 /*
-    simulate MicroStrain 7-series GNSS-INS devices
+    Simulate MicroStrain 7-series GNSS-INS devices
+
+    Usage:
+    PARAMS:
+        param set AHRS_EKF_TYPE 11
+        param set EAHRS_TYPE 7
+        param set SERIAL3_PROTOCOL 36
+        param set SERIAL3_BAUD 115
+    sim_vehicle.py -v Plane -A "--serial3=sim:MicroStrain7" --console --map -DG
 */
 
 #include "SIM_MicroStrain.h"
@@ -140,8 +148,17 @@ void MicroStrain7::send_filter_packet(void)
     put_int(packet, 0x03); // Dynamics mode (Airborne)
     put_int(packet, 0); // Filter flags (None, no warnings)
 
-    packet.header[3] = packet.payload_size;
+    // Add Attitude Quaternion
+    // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/data/mip_field_filter_attitude_quaternion.htm
+    packet.payload[packet.payload_size++] = 0x14; // Attitude Quaternion Field Size
+    packet.payload[packet.payload_size++] = 0x03; // Descriptor
+    put_float(packet, fdm.quaternion.q1);
+    put_float(packet, fdm.quaternion.q2);
+    put_float(packet, fdm.quaternion.q3);
+    put_float(packet, fdm.quaternion.q4);
+    put_int(packet, 0x0001); // Valid flags
 
+    packet.header[3] = packet.payload_size;
 
     send_packet(packet);
 }