From d9360cee960b9744e0490e5975169af63bc75992 Mon Sep 17 00:00:00 2001
From: PonomarevDA <ponomarevda96@gmail.com>
Date: Sun, 15 Oct 2023 18:28:29 +0300
Subject: [PATCH] battery: add electricity source publisher

---
 Udral/battery.cpp | 36 ++++++++++++++++++++++++++++++++++++
 Udral/battery.hpp | 25 +++++++++++++++++++++++++
 2 files changed, 61 insertions(+)
 create mode 100644 Udral/battery.cpp
 create mode 100644 Udral/battery.hpp

diff --git a/Udral/battery.cpp b/Udral/battery.cpp
new file mode 100644
index 0000000..d87556a
--- /dev/null
+++ b/Udral/battery.cpp
@@ -0,0 +1,36 @@
+/// This software is distributed under the terms of the MIT License.
+/// Copyright (c) 2023 Dmitry Ponomarev.
+/// Author: Dmitry Ponomarev <ponomarevda96@gmail.com>
+
+#include "battery.hpp"
+#include "main.h"
+#include "params.hpp"
+
+#define WH_TO_JOULE   3600
+
+void UdralBatteryPublisher::publish(float voltage, float current, float soc, float full_capacity, float remaining_capacity) {
+    (void)soc;
+    // here we expect 4s lipo
+    float full_energy_joule = 3600 * full_capacity * 14.8;
+    float energy_joule = 3600 * remaining_capacity * voltage;
+    _source_pub.publish(voltage, current, full_energy_joule, energy_joule);
+}
+
+void ElectricitySourcePublisher::publish(float voltage, float current, float full_energy_joule, float energy_joule) {
+    if (!isEnabled()) {
+        return;
+    }
+
+    reg_udral_physics_electricity_SourceTs_0_1 msg;
+    msg.value.power.voltage.volt = voltage;
+    msg.value.power.current.ampere = current;
+    msg.value.energy.joule = energy_joule;
+    msg.value.full_energy.joule = full_energy_joule;
+
+    static uint8_t buffer[reg_udral_physics_electricity_SourceTs_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
+    size_t buffer_size = reg_udral_physics_electricity_SourceTs_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
+    int32_t result = reg_udral_physics_electricity_SourceTs_0_1_serialize_(&msg, buffer, &buffer_size);
+    if (NUNAVUT_SUCCESS == result) {
+        push(buffer_size, buffer);
+    }
+}
diff --git a/Udral/battery.hpp b/Udral/battery.hpp
new file mode 100644
index 0000000..d26ab93
--- /dev/null
+++ b/Udral/battery.hpp
@@ -0,0 +1,25 @@
+/// This software is distributed under the terms of the MIT License.
+/// Copyright (c) 2023 Dmitry Ponomarev.
+/// Author: Dmitry Ponomarev <ponomarevda96@gmail.com>
+
+#ifndef UDRAL_BATTERY_HPP_
+#define UDRAL_BATTERY_HPP_
+
+#include "cyphal.hpp"
+#include "reg/udral/physics/electricity/SourceTs_0_1.h"
+
+struct ElectricitySourcePublisher: public CyphalPublisher {
+    ElectricitySourcePublisher(Cyphal* driver_, CanardPortID port_id) : CyphalPublisher(driver_, port_id) {};
+    void publish(float voltage, float current, float full_energy_joule, float energy_joule);
+};
+
+
+struct UdralBatteryPublisher {
+    UdralBatteryPublisher(Cyphal* driver_, CanardPortID port_id) : _source_pub(driver_, port_id) {};
+    void publish(float voltage, float current, float soc, float full_capacity, float remaining_capacity);
+private:
+    ElectricitySourcePublisher _source_pub;
+};
+
+
+#endif  // UDRAL_BATTERY_HPP_