add optimized compact RawImu

RaccoonlabDev · Oct 31, 2023 · 2f2b3c8 · 2f2b3c8
1 parent 94fd6e4
commit 2f2b3c8
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 0 deletions.
diff --git 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
@@ -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_