From 2f2b3c8a4dc94b39e838341e2eda7bdc2a0862b1 Mon Sep 17 00:00:00 2001
From: PonomarevDA <ponomarevda96@gmail.com>
Date: Wed, 1 Nov 2023 00:17:42 +0300
Subject: [PATCH] add optimized compact RawImu

---
 Udral/imu.cpp | 22 ++++++++++++++++++++++
 Udral/imu.hpp |  6 ++++++
 2 files changed, 28 insertions(+)

diff --git a/Udral/imu.cpp b/Udral/imu.cpp
index 73be0f9..536a24f 100644
--- a/Udral/imu.cpp
+++ b/Udral/imu.cpp
@@ -27,3 +27,25 @@ void ImuGyroPublisher::publish(const uavcan_si_unit_angular_velocity_Vector3_1_0
         push(buffer_size, buffer);
     }
 }
+
+void RawImuPublisher::publish(const std::array<double, 3>& accel, const std::array<double, 3>& gyro) {
+    if (!isEnabled()) {
+        return;
+    }
+
+    uavcan_primitive_array_Real16_1_0 msg;
+    msg.value.count = 6;
+    msg.value.elements[0] = accel[0];
+    msg.value.elements[1] = accel[1];
+    msg.value.elements[2] = accel[2];
+    msg.value.elements[3] = gyro[0];
+    msg.value.elements[4] = gyro[1];
+    msg.value.elements[5] = gyro[2];
+
+    static uint8_t buffer[uavcan_primitive_array_Real16_1_0_EXTENT_BYTES_];
+    size_t buffer_size = uavcan_primitive_array_Real16_1_0_EXTENT_BYTES_;
+    int32_t result = uavcan_primitive_array_Real16_1_0_serialize_(&msg, buffer, &buffer_size);
+    if (NUNAVUT_SUCCESS == result) {
+        push(buffer_size, buffer);
+    }
+}
diff --git a/Udral/imu.hpp b/Udral/imu.hpp
index 4ebec4d..cf167f3 100644
--- a/Udral/imu.hpp
+++ b/Udral/imu.hpp
@@ -8,6 +8,7 @@
 #include "cyphal.hpp"
 #include "uavcan/si/unit/angular_velocity/Vector3_1_0.h"
 #include "uavcan/si/unit/acceleration/Vector3_1_0.h"
+#include "uavcan/primitive/array/Real16_1_0.h"
 
 struct ImuAccelPublisher: public CyphalPublisher {
     ImuAccelPublisher(Cyphal* driver_, CanardPortID port_id) : CyphalPublisher(driver_, port_id) {};
@@ -19,4 +20,9 @@ struct ImuGyroPublisher: public CyphalPublisher {
     void publish(const uavcan_si_unit_angular_velocity_Vector3_1_0& msg);
 };
 
+struct RawImuPublisher: public CyphalPublisher {
+    RawImuPublisher(Cyphal* driver_, CanardPortID port_id) : CyphalPublisher(driver_, port_id) {};
+    void publish(const std::array<double, 3>& accel, const std::array<double, 3>& gyro);
+};
+
 #endif  // UDRAL_IMU_HPP_