From b05a6c00b691f1b1fcbe14f675791c292a797b3a Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Sun, 13 Oct 2024 14:00:04 +0800
Subject: [PATCH] AP_Periph: add support for publishing raw imu data

---
 Tools/AP_Periph/AP_Periph.cpp  |  9 ++++++
 Tools/AP_Periph/AP_Periph.h    |  9 ++++++
 Tools/AP_Periph/Parameters.cpp | 14 ++++++++++
 Tools/AP_Periph/Parameters.h   |  6 ++++
 Tools/AP_Periph/imu.cpp        | 50 ++++++++++++++++++++++++++++++++++
 5 files changed, 88 insertions(+)
 create mode 100644 Tools/AP_Periph/imu.cpp

diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp
index b952d7ae24..2ab0e4437a 100644
--- a/Tools/AP_Periph/AP_Periph.cpp
+++ b/Tools/AP_Periph/AP_Periph.cpp
@@ -168,6 +168,15 @@ void AP_Periph_FW::init()
     baro.init();
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_IMU
+    if (g.imu_sample_rate) {
+        imu.init(g.imu_sample_rate);
+        if (imu.get_accel_count() > 0 || imu.get_gyro_count() > 0) {
+            hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Periph_FW::can_imu_update, void), "IMU_UPDATE", 16384, AP_HAL::Scheduler::PRIORITY_CAN, 0);
+        }
+    }
+#endif
+
 #ifdef HAL_PERIPH_ENABLE_BATTERY
     battery_lib.init();
 #endif
diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
index dc3f6754e4..0cbf5f419e 100644
--- a/Tools/AP_Periph/AP_Periph.h
+++ b/Tools/AP_Periph/AP_Periph.h
@@ -6,6 +6,7 @@
 #include <AP_GPS/AP_GPS.h>
 #include <AP_Compass/AP_Compass.h>
 #include <AP_Baro/AP_Baro.h>
+#include <AP_InertialSensor/AP_InertialSensor.h>
 #include "SRV_Channel/SRV_Channel.h"
 #include <AP_Notify/AP_Notify.h>
 #include <AP_Logger/AP_Logger.h>
@@ -173,6 +174,10 @@ public:
     void send_relposheading_msg();
     void can_baro_update();
     void can_airspeed_update();
+#ifdef HAL_PERIPH_ENABLE_IMU
+    void can_imu_update();
+#endif
+
 #ifdef HAL_PERIPH_ENABLE_RANGEFINDER
     void can_rangefinder_update();
 #endif
@@ -230,6 +235,10 @@ public:
     AP_Baro baro;
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_IMU
+    AP_InertialSensor imu;
+#endif
+
 #ifdef HAL_PERIPH_ENABLE_RPM
     AP_RPM rpm_sensor;
     uint32_t rpm_last_update_ms;
diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
index e0196045a9..21826edd67 100644
--- a/Tools/AP_Periph/Parameters.cpp
+++ b/Tools/AP_Periph/Parameters.cpp
@@ -723,6 +723,20 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GSCALAR(esc_extended_telem_rate, "ESC_EXT_TLM_RATE", AP_PERIPH_ESC_TELEM_RATE_DEFAULT / 10),
 #endif
 
+
+#ifdef HAL_PERIPH_ENABLE_IMU
+    // @Param: IMU_SAMPLE_RATE
+    // @DisplayName: IMU Sample Rate
+    // @Description: IMU Sample Rate
+    // @Range: 0 1000
+    // @User: Standard
+    GSCALAR(imu_sample_rate, "INS_SAMPLE_RATE", 0),
+
+    // @Group: INS_
+    // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
+    GOBJECT(imu, "INS", AP_InertialSensor),
+#endif
+
     AP_VAREND
 };
 
diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h
index 6479b51168..c2df5d27f0 100644
--- a/Tools/AP_Periph/Parameters.h
+++ b/Tools/AP_Periph/Parameters.h
@@ -97,6 +97,8 @@ public:
         k_param_rpm_msg_rate,
         k_param_esc_rate,
         k_param_esc_extended_telem_rate,
+        k_param_imu_sample_rate,
+        k_param_imu,
     };
 
     AP_Int16 format_version;
@@ -211,6 +213,10 @@ public:
     AP_Int8 efi_port;
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_IMU
+    AP_Int16 imu_sample_rate;
+#endif
+
 #if HAL_PERIPH_CAN_MIRROR
     AP_Int8 can_mirror_ports;
 #endif // HAL_PERIPH_CAN_MIRROR
diff --git a/Tools/AP_Periph/imu.cpp b/Tools/AP_Periph/imu.cpp
new file mode 100644
index 0000000000..c0c5aaf401
--- /dev/null
+++ b/Tools/AP_Periph/imu.cpp
@@ -0,0 +1,50 @@
+#include "AP_Periph.h"
+
+#ifdef HAL_PERIPH_ENABLE_IMU
+#include <dronecan_msgs.h>
+
+extern const AP_HAL::HAL &hal;
+
+/*
+  update CAN magnetometer
+ */
+void AP_Periph_FW::can_imu_update(void)
+{
+    while (true) {
+        // sleep for a bit to avoid flooding the CPU
+        hal.scheduler->delay_microseconds(100);
+
+        imu.update();
+
+        if (!imu.healthy()) {
+            continue;
+        }
+
+        uavcan_equipment_ahrs_RawIMU pkt {};
+
+        Vector3f tmp;
+        imu.get_delta_velocity(tmp, pkt.integration_interval);
+        pkt.accelerometer_integral[0] = tmp.x;
+        pkt.accelerometer_integral[1] = tmp.y;
+        pkt.accelerometer_integral[2] = tmp.z;
+
+        imu.get_delta_angle(tmp, pkt.integration_interval);
+        pkt.rate_gyro_integral[0] = tmp.x;
+        pkt.rate_gyro_integral[1] = tmp.y;
+        pkt.rate_gyro_integral[2] = tmp.z;
+
+        tmp = imu.get_accel();
+        pkt.accelerometer_latest[0] = tmp.x;
+        pkt.accelerometer_latest[1] = tmp.y;
+        pkt.accelerometer_latest[2] = tmp.z;
+
+        uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_RAWIMU_MAX_SIZE];
+        uint16_t total_size = uavcan_equipment_ahrs_RawIMU_encode(&pkt, buffer, !canfdout());
+        canard_broadcast(UAVCAN_EQUIPMENT_AHRS_RAWIMU_SIGNATURE,
+                         UAVCAN_EQUIPMENT_AHRS_RAWIMU_ID,
+                         CANARD_TRANSFER_PRIORITY_HIGH,
+                         &buffer[0],
+                         total_size);
+    }
+}
+#endif // HAL_PERIPH_ENABLE_IMU