AP_Periph: add support for publishing raw imu data

bugobliterator · Oct 13, 2024 · 268d56a · 268d56a
1 parent ab25df0
commit 268d56a
Show file tree

Hide file tree

Showing 5 changed files with 77 additions and 0 deletions.
diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp
@@ -168,6 +168,13 @@ void AP_Periph_FW::init()
     baro.init();
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_IMU
+    if (g.imu_sample_rate) {
+        imu.init(g.imu_sample_rate);
+        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
@@ -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,7 @@ class AP_Periph_FW {
     void send_relposheading_msg();
     void can_baro_update();
     void can_airspeed_update();
+    void can_imu_update();
 #ifdef HAL_PERIPH_ENABLE_RANGEFINDER
     void can_rangefinder_update();
 #endif
@@ -230,6 +232,10 @@ class AP_Periph_FW {
     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
@@ -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: ardupilot/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
@@ -97,6 +97,8 @@ class Parameters {
         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 @@ class Parameters {
     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
@@ -0,0 +1,44 @@
+#include "AP_Periph.h"
+#include <dronecan_msgs.h>
+
+/*
+  update CAN magnetometer
+ */
+void AP_Periph_FW::can_imu_update(void)
+{
+#ifdef HAL_PERIPH_ENABLE_IMU
+    while (true) {
+        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
+}