From 713351893c382cac7527b5cf699814622c548a03 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Sat, 3 Feb 2024 20:43:51 -0700 Subject: [PATCH] AP_ExternalAHRS: SIM_MicroStrain support quaternion attitude * This replaces IMU orientation * Fixed some usage docs Signed-off-by: Ryan Friedman --- libraries/SITL/SIM_MicroStrain.cpp | 10 +++++++++- libraries/SITL/SIM_MicroStrain.h | 7 ------- libraries/SITL/SIM_MicroStrain7.cpp | 21 +++++++++++++++++++-- 3 files changed, 28 insertions(+), 10 deletions(-) diff --git a/libraries/SITL/SIM_MicroStrain.cpp b/libraries/SITL/SIM_MicroStrain.cpp index f4ec6c78f6..af4b2c26d6 100644 --- a/libraries/SITL/SIM_MicroStrain.cpp +++ b/libraries/SITL/SIM_MicroStrain.cpp @@ -13,7 +13,15 @@ along with this program. If not, see . */ /* - 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 diff --git a/libraries/SITL/SIM_MicroStrain.h b/libraries/SITL/SIM_MicroStrain.h index bb2d3ebe73..80684f1ef4 100644 --- a/libraries/SITL/SIM_MicroStrain.h +++ b/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" diff --git a/libraries/SITL/SIM_MicroStrain7.cpp b/libraries/SITL/SIM_MicroStrain7.cpp index 4664942049..2bee3bc00d 100644 --- a/libraries/SITL/SIM_MicroStrain7.cpp +++ b/libraries/SITL/SIM_MicroStrain7.cpp @@ -13,7 +13,15 @@ along with this program. If not, see . */ /* - 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); }