diff --git a/husarion_ugv_battery/README.md b/husarion_ugv_battery/README.md index e952f05dc..263ea2526 100644 --- a/husarion_ugv_battery/README.md +++ b/husarion_ugv_battery/README.md @@ -33,11 +33,11 @@ Publishes battery state read from ADC unit. #### Parameters -- `~/adc/device0` [*string*, default: **/sys/bus/iio/devices/iio:device0**]: ADC number 0 IIO device. -- `~/adc/device1` [*string*, default: **/sys/bus/iio/devices/iio:device1**]: ADC number 1 IIO device. -- `~/adc/ma_window_len/charge` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery charge readings. -- `~/adc/ma_window_len/temp` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery temperature readings. +- `~/adc.device0` [*string*, default: **/sys/bus/iio/devices/iio:device0**]: ADC number 0 IIO device. +- `~/adc.device1` [*string*, default: **/sys/bus/iio/devices/iio:device1**]: ADC number 1 IIO device. +- `~/adc.ma_window_len.charge` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery charge readings. +- `~/adc.ma_window_len.temp` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery temperature readings. - `~/battery_timeout` [*float*, default: **1.0**]: Specifies the timeout in seconds. If the node fails to read battery data exceeding this duration, the node will publish an unknown battery state. -- `~/ma_window_len/voltage` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery voltage readings. -- `~/ma_window_len/current` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery current readings. -- `~/roboteq/driver_state_timeout` [*float*, default: **0.2**]: Specifies timeout in seconds after which driver state messages will be considered old (deprecated). +- `~/ma_window_len.voltage` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery voltage readings. +- `~/ma_window_len.current` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery current readings. +- `~/roboteq.driver_state_timeout` [*float*, default: **0.2**]: Specifies timeout in seconds after which driver state messages will be considered old (deprecated). diff --git a/husarion_ugv_battery/include/husarion_ugv_battery/battery/adc_battery.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery/adc_battery.hpp index 94dd428d4..20b39591a 100644 --- a/husarion_ugv_battery/include/husarion_ugv_battery/battery/adc_battery.hpp +++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery/adc_battery.hpp @@ -77,7 +77,12 @@ class ADCBattery : public Battery static constexpr float kR1 = 10000.0; static constexpr float kR0 = 10000.0; static constexpr float kUSupply = 3.28; - static constexpr float kKelvinToCelciusOffset = 273.15; + static constexpr float kKelvinToCelsiusOffset = 273.15; + + // Threshold used for diagnostics. At this voltage level, and below, the battery shall draw a + // significant current from the charger if the charger is connected. If not, the charging circuit + // may be broken. + static constexpr float kBatteryCCCheckTresh = 41.2; float voltage_raw_; float current_raw_; diff --git a/husarion_ugv_battery/include/husarion_ugv_battery/battery/battery.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery/battery.hpp index 7e461cfa4..94392164b 100644 --- a/husarion_ugv_battery/include/husarion_ugv_battery/battery/battery.hpp +++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery/battery.hpp @@ -57,14 +57,14 @@ class Battery float GetBatteryPercent(const float voltage) const { - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 5; i++) { if (voltage > battery_approx_ranges[i]) { return std::clamp( (battery_approx_a_values[i] * voltage + battery_approx_b_values[i]) / 100, 0.0f, 1.0f); } } return std::clamp( - (battery_approx_a_values[4] * voltage + battery_approx_b_values[4]) / 100, 0.0f, 1.0f); + (battery_approx_a_values[5] * voltage + battery_approx_b_values[5]) / 100, 0.0f, 1.0f); } void ResetBatteryMsgs(const rclcpp::Time & header_stamp) @@ -117,9 +117,10 @@ class Battery static constexpr float kDesignedCapacity = 20.0; static constexpr std::string_view kLocation = "user_compartment"; - static constexpr float battery_approx_ranges[4] = {41.25, 37, 35, 33.7}; - static constexpr float battery_approx_a_values[5] = {1.733, 9.153, 19.8, 10.538, 0.989}; - static constexpr float battery_approx_b_values[5] = {27.214, -278.861, -672.6, -351.47, -29.67}; + static constexpr float battery_approx_ranges[5] = {41.25, 37.0, 36.0, 35.0, 33.7}; + static constexpr float battery_approx_a_values[6] = {8.665, 9.153, 19.8, 22.84, 10.538, 0.989}; + static constexpr float battery_approx_b_values[6] = {-258.73, -278.861, -672.6, + -782.04, -351.47, -29.669}; std::string error_msg_; BatteryStateMsg battery_state_; diff --git a/husarion_ugv_battery/src/battery/adc_battery.cpp b/husarion_ugv_battery/src/battery/adc_battery.cpp index cd13f25a0..68db4d73f 100644 --- a/husarion_ugv_battery/src/battery/adc_battery.cpp +++ b/husarion_ugv_battery/src/battery/adc_battery.cpp @@ -107,7 +107,7 @@ float ADCBattery::ADCToBatteryTemp(const float adc_data) const const float R_therm = (adc_data * kR1) / (kUSupply - adc_data); return (kTempCoeffA * kTempCoeffB / (kTempCoeffA * logf(R_therm / kR0) + kTempCoeffB)) - - kKelvinToCelciusOffset; + kKelvinToCelsiusOffset; } void ADCBattery::UpdateBatteryMsgs(const rclcpp::Time & header_stamp, const bool charger_connected) @@ -167,7 +167,7 @@ std::uint8_t ADCBattery::GetBatteryStatus(const float charge, const bool charger if (charger_connected) { if (fabs(battery_state_.percentage - 1.0f) < std::numeric_limits::epsilon()) { return BatteryStateMsg::POWER_SUPPLY_STATUS_FULL; - } else if (charge > kChargingCurrentTresh) { + } else if (charge > kChargingCurrentTresh || battery_state_.voltage > kBatteryCCCheckTresh) { return BatteryStateMsg::POWER_SUPPLY_STATUS_CHARGING; } else { return BatteryStateMsg::POWER_SUPPLY_STATUS_NOT_CHARGING; diff --git a/husarion_ugv_battery/test/battery/test_adc_battery.cpp b/husarion_ugv_battery/test/battery/test_adc_battery.cpp index 9486c85ef..13a5680ff 100644 --- a/husarion_ugv_battery/test/battery/test_adc_battery.cpp +++ b/husarion_ugv_battery/test/battery/test_adc_battery.cpp @@ -253,6 +253,11 @@ TEST_F(TestADCBattery, BatteryMsgStatusCharging) UpdateBattery(1.5, 0.01, 0.98, 0.5, true); EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_STATUS_CHARGING, battery_state_.power_supply_status); + + // Small charge, but voltage over 41.2 V + UpdateBattery(1.65, 0.01, 0.98, 0.04, true); + + EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_STATUS_CHARGING, battery_state_.power_supply_status); } TEST_F(TestADCBattery, BatteryMsgStatusNotCharging) diff --git a/husarion_ugv_battery/test/battery/test_battery.cpp b/husarion_ugv_battery/test/battery/test_battery.cpp index 9b2d12c0d..62195ca3b 100644 --- a/husarion_ugv_battery/test/battery/test_battery.cpp +++ b/husarion_ugv_battery/test/battery/test_battery.cpp @@ -109,10 +109,13 @@ TEST_F(TestBattery, GetErrorMsg) TEST_F(TestBattery, GetBatteryPercent) { - EXPECT_FLOAT_EQ(0.0, battery_->GetBatteryPercent(30.0f)); - EXPECT_FLOAT_EQ(0.019780006, battery_->GetBatteryPercent(32.0f)); - EXPECT_FLOAT_EQ(0.68953001, battery_->GetBatteryPercent(38.0f)); - EXPECT_FLOAT_EQ(1.0, battery_->GetBatteryPercent(42.0f)); + EXPECT_NEAR(0.0, battery_->GetBatteryPercent(30.0f), 1e-5); + EXPECT_NEAR(0.01979, battery_->GetBatteryPercent(32.0f), 1e-6); + EXPECT_NEAR(0.06822, battery_->GetBatteryPercent(34.0f), 1e-6); + EXPECT_NEAR(0.2878, battery_->GetBatteryPercent(35.5f), 1e-6); + EXPECT_NEAR(0.501, battery_->GetBatteryPercent(36.5f), 1e-6); + EXPECT_NEAR(0.68953, battery_->GetBatteryPercent(38.0f), 1e-6); + EXPECT_FLOAT_EQ(1.0, battery_->GetBatteryPercent(41.4f)); EXPECT_FLOAT_EQ(1.0, battery_->GetBatteryPercent(45.0f)); } diff --git a/husarion_ugv_battery/test/test_battery_driver_node_adc_dual.cpp b/husarion_ugv_battery/test/test_battery_driver_node_adc_dual.cpp index 2718407a7..167035cec 100644 --- a/husarion_ugv_battery/test/test_battery_driver_node_adc_dual.cpp +++ b/husarion_ugv_battery/test/test_battery_driver_node_adc_dual.cpp @@ -38,8 +38,8 @@ TEST_F(TestBatteryNodeADCDual, BatteryValues) EXPECT_FLOAT_EQ(35.05957, battery_state_->voltage); EXPECT_FLOAT_EQ(2.02, battery_state_->current); EXPECT_FLOAT_EQ(26.094543, battery_state_->temperature); - EXPECT_FLOAT_EQ(0.21579468, battery_state_->percentage); - EXPECT_FLOAT_EQ(8.6317873, battery_state_->charge); + EXPECT_FLOAT_EQ(0.18720642, battery_state_->percentage); + EXPECT_FLOAT_EQ(7.4882565, battery_state_->charge); // If both batteries have the same reading values they should have equal values EXPECT_FLOAT_EQ(battery_1_state_->voltage, battery_2_state_->voltage); diff --git a/husarion_ugv_battery/test/test_battery_driver_node_adc_single.cpp b/husarion_ugv_battery/test/test_battery_driver_node_adc_single.cpp index 8c0ec4987..8ad638e26 100644 --- a/husarion_ugv_battery/test/test_battery_driver_node_adc_single.cpp +++ b/husarion_ugv_battery/test/test_battery_driver_node_adc_single.cpp @@ -43,8 +43,8 @@ TEST_F(TestBatteryNodeADCSingle, BatteryValues) EXPECT_FLOAT_EQ(35.05957, battery_state_->voltage); EXPECT_FLOAT_EQ(2.01, battery_state_->current); EXPECT_FLOAT_EQ(26.094543, battery_state_->temperature); - EXPECT_FLOAT_EQ(0.21579468, battery_state_->percentage); - EXPECT_FLOAT_EQ(4.3158937, battery_state_->charge); + EXPECT_FLOAT_EQ(0.18720642, battery_state_->percentage); + EXPECT_FLOAT_EQ(3.7441282, battery_state_->charge); // For single battery if readings stay the same values of battery_1 and battery should be the same EXPECT_FLOAT_EQ(battery_1_state_->voltage, battery_state_->voltage); diff --git a/husarion_ugv_battery/test/utils/test_battery_driver_node.hpp b/husarion_ugv_battery/test/utils/test_battery_driver_node.hpp index 319dfd6db..7bb73c3a5 100644 --- a/husarion_ugv_battery/test/utils/test_battery_driver_node.hpp +++ b/husarion_ugv_battery/test/utils/test_battery_driver_node.hpp @@ -71,9 +71,8 @@ TestBatteryNode::TestBatteryNode(const bool use_adc_battery, const bool dual_bat device0_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice0; device1_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice1; - params.push_back(rclcpp::Parameter("adc/device0", device0_path_.string())); - params.push_back(rclcpp::Parameter("adc/device1", device1_path_.string())); - params.push_back(rclcpp::Parameter("adc/path", testing::TempDir())); + params.push_back(rclcpp::Parameter("adc.device0", device0_path_.string())); + params.push_back(rclcpp::Parameter("adc.device1", device1_path_.string())); // Create the device0 and device1 directories if they do not exist std::filesystem::create_directory(device0_path_);