From a28c13d9908d686443a0deae3f6add1d5b61c554 Mon Sep 17 00:00:00 2001 From: PonomarevDA Date: Mon, 16 Oct 2023 19:12:52 +0300 Subject: [PATCH] battery: add status and parameters publishers --- Udral/battery.cpp | 82 ++++++++++++++++++++++++++++++++++++++++++----- Udral/battery.hpp | 44 +++++++++++++++++++++++-- 2 files changed, 116 insertions(+), 10 deletions(-) diff --git a/Udral/battery.cpp b/Udral/battery.cpp index d87556a..e2f6b37 100644 --- a/Udral/battery.cpp +++ b/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; +} diff --git a/Udral/battery.hpp b/Udral/battery.hpp index d26ab93..b8a749b 100644 --- a/Udral/battery.hpp +++ b/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}; };