add rangefinder

Udral/rangefinder.cpp

@@ -0,0 +1,22 @@
+/// This software is distributed under the terms of the MIT License.
+/// Copyright (c) 2023 Dmitry Ponomarev.
+/// Author: Dmitry Ponomarev <[email protected]>
+
+#include "rangefinder.hpp"
+#include "main.h"
+
+void RangefinderRangePublisher::publish(float range) {
+    if (!isEnabled()) {
+        return;
+    }
+
+    uavcan_si_sample_length_Scalar_1_0 msg;
+    msg.meter = range;
+
+    static uint8_t buffer[uavcan_si_sample_length_Scalar_1_0_EXTENT_BYTES_];
+    size_t buffer_size = uavcan_si_sample_length_Scalar_1_0_EXTENT_BYTES_;
+    int32_t result = uavcan_si_sample_length_Scalar_1_0_serialize_(&msg, buffer, &buffer_size);
+    if (NUNAVUT_SUCCESS == result) {
+        push(buffer_size, buffer);
+    }
+}

Udral/rangefinder.hpp

@@ -0,0 +1,18 @@
+/// This software is distributed under the terms of the MIT License.
+/// Copyright (c) 2023 Dmitry Ponomarev.
+/// Author: Dmitry Ponomarev <[email protected]>
+
+#ifndef UDRAL_RANGEFINDER_HPP_
+#define UDRAL_RANGEFINDER_HPP_
+
+#include "cyphal.hpp"
+#include "uavcan/si/sample/length/Scalar_1_0.h"
+
+
+struct RangefinderRangePublisher: public CyphalPublisher {
+    RangefinderRangePublisher(Cyphal* driver_, CanardPortID port_id) : CyphalPublisher(driver_, port_id) {};
+    void publish(float range);
+};
+
+
+#endif  // UDRAL_RANGEFINDER_HPP_