better battery estimation

husarion · Dec 3, 2024 · 16b9931 · 16b9931
1 parent bfaa42c
commit 16b9931
Show file tree

Hide file tree

Showing 8 changed files with 32 additions and 19 deletions.
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
@@ -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
@@ -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
@@ -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<float>::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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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_);