GCS_MAVLink: support HIGHRES_IMU

HIGHRES_IMU MAVLink message. Enable, when BOARD_FLASH_SIZE is > 1024
ArduPilot · May 13, 2024 · 12f1ee2 · 12f1ee2
1 parent 34f5579
commit 12f1ee2
Show file tree

Hide file tree

Showing 4 changed files with 97 additions and 0 deletions.
diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
@@ -354,6 +354,9 @@ class GCS_MAVLINK
     void send_rc_channels() const;
     void send_rc_channels_raw() const;
     void send_raw_imu();
+#if AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED
+    void send_highres_imu();
+#endif
 
     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();
@@ -1400,3 +1403,20 @@ enum MAV_SEVERITY
 
 #endif // HAL_GCS_ENABLED
 
+#if AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED
+#define HIGHRES_IMU_UPDATED_NONE 0x00
+#define HIGHRES_IMU_UPDATED_XACC 0x01
+#define HIGHRES_IMU_UPDATED_YACC 0x02
+#define HIGHRES_IMU_UPDATED_ZACC 0x04
+#define HIGHRES_IMU_UPDATED_XGYRO 0x08
+#define HIGHRES_IMU_UPDATED_YGYRO 0x10
+#define HIGHRES_IMU_UPDATED_ZGYRO 0x20
+#define HIGHRES_IMU_UPDATED_XMAG 0x40
+#define HIGHRES_IMU_UPDATED_YMAG 0x80
+#define HIGHRES_IMU_UPDATED_ZMAG 0x100
+#define HIGHRES_IMU_UPDATED_ABS_PRESSURE 0x200
+#define HIGHRES_IMU_UPDATED_DIFF_PRESSURE 0x400
+#define HIGHRES_IMU_UPDATED_PRESSURE_ALT 0x800
+#define HIGHRES_IMU_UPDATED_TEMPERATURE 0x1000
+#define HIGHRES_IMU_UPDATED_ALL 0xFFFF
+#endif
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -1005,6 +1005,9 @@ 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},
+#if AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED
+        { MAVLINK_MSG_ID_HIGHRES_IMU,           MSG_HIGHRES_IMU},
+#endif
         { 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 +2130,67 @@ void GCS_MAVLINK::send_raw_imu()
 #endif
 }
 
+#if AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED
+void GCS_MAVLINK::send_highres_imu()
+{
+#if AP_INERTIALSENSOR_ENABLED
+    const AP_InertialSensor &ins = AP::ins();
+    const Vector3f& accel = ins.get_accel();
+    const Vector3f& gyro = ins.get_gyro();
+    uint16_t fields_updated = (HIGHRES_IMU_UPDATED_XACC | HIGHRES_IMU_UPDATED_YACC | HIGHRES_IMU_UPDATED_ZACC |
+        HIGHRES_IMU_UPDATED_XGYRO | HIGHRES_IMU_UPDATED_YGYRO | HIGHRES_IMU_UPDATED_ZGYRO);
+    uint8_t accel_id = ins.get_primary_accel();
+
+    Vector3f mag = Vector3f();
+#if AP_COMPASS_ENABLED
+    const Compass &compass = AP::compass();
+    if (compass.get_count() >= 1) {
+        mag = compass.get_field() * 1000.0f; // convert to gauss
+    }
+    fields_updated |= (HIGHRES_IMU_UPDATED_XMAG | HIGHRES_IMU_UPDATED_YMAG | HIGHRES_IMU_UPDATED_ZMAG);
+#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_AMSL();
+    diff_pressure = press_abs - barometer.get_ground_pressure() * 0.01f;
+    fields_updated |= (HIGHRES_IMU_UPDATED_ABS_PRESSURE | HIGHRES_IMU_UPDATED_DIFF_PRESSURE |
+        HIGHRES_IMU_UPDATED_PRESSURE_ALT | HIGHRES_IMU_UPDATED_TEMPERATURE);
+#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,
+        (fields_updated == (HIGHRES_IMU_UPDATED_XACC | HIGHRES_IMU_UPDATED_YACC | HIGHRES_IMU_UPDATED_ZACC |
+        HIGHRES_IMU_UPDATED_XGYRO | HIGHRES_IMU_UPDATED_YGYRO | HIGHRES_IMU_UPDATED_ZGYRO | 
+        HIGHRES_IMU_UPDATED_XMAG | HIGHRES_IMU_UPDATED_YMAG | HIGHRES_IMU_UPDATED_ZMAG |
+        HIGHRES_IMU_UPDATED_ABS_PRESSURE | HIGHRES_IMU_UPDATED_DIFF_PRESSURE |
+        HIGHRES_IMU_UPDATED_PRESSURE_ALT | HIGHRES_IMU_UPDATED_TEMPERATURE)) ? HIGHRES_IMU_UPDATED_ALL : fields_updated,
+        accel_id
+    );
+#endif
+}
+#endif
+
 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 +6244,12 @@ 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;
+#if AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED
+    case MSG_HIGHRES_IMU:
+        CHECK_PAYLOAD_SIZE(HIGHRES_IMU);
+        send_highres_imu();
+        break;
+#endif
 
     case MSG_SCALED_PRESSURE:
         CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);

diff --git a/libraries/GCS_MAVLink/GCS_config.h b/libraries/GCS_MAVLink/GCS_config.h
@@ -102,3 +102,7 @@
 #ifndef AP_MAVLINK_COMMAND_LONG_ENABLED
 #define AP_MAVLINK_COMMAND_LONG_ENABLED 1
 #endif
+
+#ifndef AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED
+#define AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED (BOARD_FLASH_SIZE > 1024)
+#endif
diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h
@@ -94,5 +94,8 @@ enum ap_message : uint8_t {
     MSG_HYGROMETER,
     MSG_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE,
     MSG_RELAY_STATUS,
+#if AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED
+    MSG_HIGHRES_IMU,
+#endif
     MSG_LAST // MSG_LAST must be the last entry in this enum
 };