From 5f796f35392d3274c9f449db2b0bb378c7f5d40b Mon Sep 17 00:00:00 2001
From: koehlert <tim.koehler.net@gmail.com>
Date: Tue, 7 May 2024 09:08:54 +0200
Subject: [PATCH] GCS_MAVLink: support HIGHRES_IMU

---
 libraries/GCS_MAVLink/GCS.h          |  1 +
 libraries/GCS_MAVLink/GCS_Common.cpp | 56 ++++++++++++++++++++++++++++
 libraries/GCS_MAVLink/ap_message.h   |  1 +
 3 files changed, 58 insertions(+)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index 148757e0b49f0a..ba84b4245c3166 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -354,6 +354,7 @@ class GCS_MAVLINK
     void send_rc_channels() const;
     void send_rc_channels_raw() const;
     void send_raw_imu();
+    void send_highres_imu();
 
     void send_scaled_pressure_instance(uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff));
     void send_scaled_pressure();
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index fadf622bdc21e3..917aa940ec6d3f 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -1005,6 +1005,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
         { MAVLINK_MSG_ID_SCALED_IMU,            MSG_SCALED_IMU},
         { MAVLINK_MSG_ID_SCALED_IMU2,           MSG_SCALED_IMU2},
         { MAVLINK_MSG_ID_SCALED_IMU3,           MSG_SCALED_IMU3},
+        { MAVLINK_MSG_ID_HIGHRES_IMU,           MSG_HIGHRES_IMU},
         { MAVLINK_MSG_ID_SCALED_PRESSURE,       MSG_SCALED_PRESSURE},
         { MAVLINK_MSG_ID_SCALED_PRESSURE2,      MSG_SCALED_PRESSURE2},
         { MAVLINK_MSG_ID_SCALED_PRESSURE3,      MSG_SCALED_PRESSURE3},
@@ -2127,6 +2128,57 @@ void GCS_MAVLINK::send_raw_imu()
 #endif
 }
 
+void GCS_MAVLINK::send_highres_imu()
+{
+    Vector3f accel;
+    Vector3f gyro;
+#if AP_INERTIALSENSOR_ENABLED
+    const AP_InertialSensor &ins = AP::ins();
+    accel = ins.get_accel(0);
+    gyro = ins.get_gyro(0);
+#endif
+    
+    Vector3f mag;
+#if AP_COMPASS_ENABLED
+    const Compass &compass = AP::compass();
+    if (compass.get_count() >= 1) {
+        mag = compass.get_field(0) * 1000.0f; // convert to gauss
+    }
+#endif
+
+    float press_abs = 0.0f;
+    float temperature = 0.0f;
+    float altitude = 0.0f;
+    float diff_pressure = 0.0f;
+#if AP_BARO_ENABLED
+    const AP_Baro &barometer = AP::baro();
+    press_abs = barometer.get_pressure() * 0.01f;
+    temperature = barometer.get_temperature();
+    altitude = barometer.get_altitude();
+    diff_pressure = press_abs - barometer.get_ground_pressure() * 0.01f;
+#endif
+
+    mavlink_msg_highres_imu_send(
+        chan,
+        AP_HAL::micros64(),
+        accel.x,
+        accel.y,
+        accel.z,
+        gyro.x,
+        gyro.y,
+        gyro.z,
+        mag.x,
+        mag.y,
+        mag.z,
+        press_abs,
+        diff_pressure,
+        altitude,
+        temperature,
+        0x3f,
+        0
+    );
+}
+
 void GCS_MAVLINK::send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature))
 {
 #if AP_INERTIALSENSOR_ENABLED
@@ -6180,6 +6232,10 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
         CHECK_PAYLOAD_SIZE(SCALED_IMU3);
         send_scaled_imu(2, mavlink_msg_scaled_imu3_send);
         break;
+    case MSG_HIGHRES_IMU:
+        CHECK_PAYLOAD_SIZE(HIGHRES_IMU);
+        send_highres_imu();
+        break;
 
     case MSG_SCALED_PRESSURE:
         CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h
index 42724b12a2548c..58ce734beb8af3 100644
--- a/libraries/GCS_MAVLink/ap_message.h
+++ b/libraries/GCS_MAVLink/ap_message.h
@@ -94,5 +94,6 @@ enum ap_message : uint8_t {
     MSG_HYGROMETER,
     MSG_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE,
     MSG_RELAY_STATUS,
+    MSG_HIGHRES_IMU,
     MSG_LAST // MSG_LAST must be the last entry in this enum
 };