Better battery estimation (#452)

* better battery estimation

* update README

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).

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_;

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_;

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;

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)

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));
 }
 

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);

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);

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_);