battery: add status and parameters publishers

Udral/battery.cpp

@@ -6,14 +6,37 @@
 #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);
+#define WH_TO_JOULE     3600
+#define AH_TO_COULOMB   3600
+
+void UdralBatteryPublisher::set_nominal_data(float design_capacity_ah, uint64_t unique_id, float nominal_voltage) {
+    _parameters_pub.set_nominal_data(design_capacity_ah, unique_id, nominal_voltage);
+}
+
+void UdralBatteryPublisher::publish(float voltage, float current, float temperature_kelvin, float full_capacity_ah, float remaining_capacity_ah) {
+
+    uint32_t crnt_time_ms = HAL_GetTick();
+
+    if (crnt_time_ms > _next_source_pub_time_ms) {
+        _next_source_pub_time_ms = crnt_time_ms + 10;
+
+        // here we expect 4s lipo
+        float full_energy_joule = WH_TO_JOULE * full_capacity_ah * _parameters_pub.get_nominal_voltage();
+        float energy_joule = WH_TO_JOULE * remaining_capacity_ah * voltage;
+        _source_pub.publish(voltage, current, full_energy_joule, energy_joule);
+    }
+
+    if (crnt_time_ms > _next_status_pub_time_ms) {
+        _next_status_pub_time_ms = crnt_time_ms + 1000;
+
+        _status_pub.publish(voltage, temperature_kelvin);
+    }
+
+    if (crnt_time_ms > _next_parameters_pub_time_ms) {
+        _next_parameters_pub_time_ms = crnt_time_ms + 5000;
+
+        _parameters_pub.publish();
+    }
 }
 
 void ElectricitySourcePublisher::publish(float voltage, float current, float full_energy_joule, float energy_joule) {
@@ -34,3 +57,46 @@ void ElectricitySourcePublisher::publish(float voltage, float current, float ful
         push(buffer_size, buffer);
     }
 }
+
+void BatteryStatusPublisher::publish(float voltage, float temperature_kelvin) {
+    if (!isEnabled()) {
+        return;
+    }
+
+    reg_udral_service_battery_Status_0_2 msg;
+    msg.temperature_min_max[1].kelvin = temperature_kelvin;
+    msg.cell_voltages.count = 1;
+    msg.cell_voltages.elements[0] = voltage;
+
+    static uint8_t buffer[reg_udral_service_battery_Status_0_2_SERIALIZATION_BUFFER_SIZE_BYTES_];
+    size_t buffer_size = reg_udral_service_battery_Status_0_2_SERIALIZATION_BUFFER_SIZE_BYTES_;
+    int32_t result = reg_udral_service_battery_Status_0_2_serialize_(&msg, buffer, &buffer_size);
+    if (NUNAVUT_SUCCESS == result) {
+        push(buffer_size, buffer);
+    }
+}
+
+void BatteryParametersPublisher::set_nominal_data(float design_capacity_ah, uint64_t unique_id, float nominal_voltage) {
+    _msg.design_capacity.coulomb = design_capacity_ah * AH_TO_COULOMB;
+    _msg.unique_id = unique_id;
+    _msg.nominal_voltage.volt = nominal_voltage;
+    _msg.cycle_count = 0;
+    _msg.state_of_health_pct = 100;  // ranges from 0 (unusable) to 100 (new)
+}
+
+void BatteryParametersPublisher::publish() {
+    if (!isEnabled()) {
+        return;
+    }
+
+    static uint8_t buffer[reg_udral_service_battery_Parameters_0_3_SERIALIZATION_BUFFER_SIZE_BYTES_];
+    size_t buffer_size = reg_udral_service_battery_Parameters_0_3_SERIALIZATION_BUFFER_SIZE_BYTES_;
+    int32_t result = reg_udral_service_battery_Parameters_0_3_serialize_(&_msg, buffer, &buffer_size);
+    if (NUNAVUT_SUCCESS == result) {
+        push(buffer_size, buffer);
+    }
+}
+
+float BatteryParametersPublisher::get_nominal_voltage() {
+    return _msg.nominal_voltage.volt;
+}

Udral/battery.hpp

@@ -7,18 +7,58 @@
 
 #include "cyphal.hpp"
 #include "reg/udral/physics/electricity/SourceTs_0_1.h"
+#include "reg/udral/service/battery/Status_0_2.h"
+#include "reg/udral/service/battery/Parameters_0_3.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 BatteryStatusPublisher: public CyphalPublisher {
+    BatteryStatusPublisher(Cyphal* driver_, CanardPortID port_id) : CyphalPublisher(driver_, port_id) {};
+    void publish(float voltage, float temperature_kelvin);
+};
+
+struct BatteryParametersPublisher: public CyphalPublisher {
+    BatteryParametersPublisher(Cyphal* driver_, CanardPortID port_id) : CyphalPublisher(driver_, port_id) {}
+    void publish();
+    void set_nominal_data(float design_capacity_ah, uint64_t unique_id, float nominal_voltage);
+    float get_nominal_voltage();
+private:
+    reg_udral_service_battery_Parameters_0_3 _msg;
+};
+
 
 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);
+    UdralBatteryPublisher(Cyphal* driver_, CanardPortID source_port, CanardPortID status_port, CanardPortID params_port) :
+        _source_pub(driver_, source_port),
+        _status_pub(driver_, status_port),
+        _parameters_pub(driver_, params_port) {
+    };
+
+    /**
+     * @brief Initialize the battery parameters in the beginning of the application
+     */
+    void set_nominal_data(float design_capacity_ah, uint64_t unique_id, float nominal_voltage);
+
+    /**
+     * @brief Call this function at least once per second. It publishes the following topics:
+     * SUBJECT                  TYP. RATE [Hz]
+     * energy_source            1...100 (calling rate)
+     * battery_status           1
+     * battery_parameters       0.2
+     */
+    void publish(float voltage, float current, float temperature_kelvin, float full_capacity_ah, float remaining_capacity_ah);
+
 private:
     ElectricitySourcePublisher _source_pub;
+    BatteryStatusPublisher _status_pub;
+    BatteryParametersPublisher _parameters_pub;
+
+    uint32_t _next_source_pub_time_ms{1000};
+    uint32_t _next_status_pub_time_ms{1000};
+    uint32_t _next_parameters_pub_time_ms{1000};
 };