SITL: improve SITL::Battery in multiple ways

Author: hunt0r

libraries/SITL/SIM_Battery.cpp

@@ -17,6 +17,7 @@
 */
 
 #include "SIM_Battery.h"
+#include <float.h>
 #include <AP_Math/AP_Math.h>
 
 using namespace SITL;
@@ -72,9 +73,14 @@ static const struct {
 /*
   use table to get resting voltage from remaining capacity
  */
-float Battery::get_resting_voltage(float charge_pct) const
+float Battery::get_resting_voltage(void) const
 {
+    if (capacity_is_unlimited()) {
+        return max_voltage;
+    }
+    float charge_pct = 100 * remaining_Ah / capacity_Ah;
     const float max_cell_voltage = soc_table[0].volt_per_cell;
+    const float min_cell_voltage = soc_table[ARRAY_SIZE(soc_table) - 1].volt_per_cell;
     for (uint8_t i=1; i<ARRAY_SIZE(soc_table); i++) {
         if (charge_pct >= soc_table[i].soc_pct) {
             // linear interpolation between table rows
@@ -86,15 +92,21 @@ float Battery::get_resting_voltage(float charge_pct) const
             return (cell_volt / max_cell_voltage) * max_voltage;
         }
     }
-    // off the bottom of the table, return a small non-zero to prevent math errors
-    return 0.001;
+    // off the bottom of the table
+    return min_cell_voltage;
 }
 
 /*
-  use table to set initial state of charge from voltage
+  return remaining Amp-hours (aka "charge", "state of charge") corresponding to a voltage
+
+  this is const for readability: it has no "side effects"
  */
-void Battery::set_initial_SoC(float voltage)
+float Battery::compute_remaining_ah(float voltage) const
 {
+    if (capacity_is_unlimited()) {
+        return FLT_MAX;
+    }
+
     const float max_cell_voltage = soc_table[0].volt_per_cell;
     float cell_volt = (voltage / max_voltage) * max_cell_voltage;
 
@@ -106,24 +118,28 @@ void Battery::set_initial_SoC(float voltage)
             float soc1 = soc_table[i].soc_pct;
             float soc2 = soc_table[i-1].soc_pct;
             float soc = soc1 + (dv1 / dv2) * (soc2 - soc1);
-            remaining_Ah = capacity_Ah * soc * 0.01;
-            return;
+            return capacity_Ah * soc * 0.01;
         }
     }
-
     // off the bottom of the table
-    remaining_Ah = 0;
+    return 0.0f;
 }
 
-void Battery::setup(float _capacity_Ah, float _resistance, float _max_voltage)
+void Battery::set_remaining_ah(void)
+{
+    remaining_Ah = compute_remaining_ah(voltage_set);
+}
+
+// Reminder: capacity <= 0 means **unlimited**
+void Battery::setup(float _capacity_Ah, float _resistance_ohm, float _max_voltage)
 {
     capacity_Ah = _capacity_Ah;
-    resistance = _resistance;
+    resistance_ohm = _resistance_ohm;
     max_voltage = _max_voltage;
 
     voltage_set = max_voltage;
     voltage_filter.reset(voltage_set);
-    set_initial_SoC(voltage_set);
+    set_remaining_ah();
 }
 
 void Battery::maybe_reset(float desired_voltage, float desired_capacity_Ah)
@@ -138,23 +154,10 @@ void Battery::maybe_reset(float desired_voltage, float desired_capacity_Ah)
     // a negative desired voltage is unexpected, but not problematic
     voltage_set = MIN(desired_voltage, max_voltage);
     voltage_filter.reset(voltage_set);
-    set_initial_SoC(voltage_set);
+    set_remaining_ah();
 }
 
-void Battery::init_voltage(float voltage)
-{
-    voltage_filter.reset(voltage);
-    voltage_set = voltage;
-    set_initial_SoC(voltage);
-}
-
-void Battery::init_capacity(float capacity)
-{
-    capacity_Ah = capacity;
-    set_initial_SoC(voltage_set);
-}
-
-void Battery::set_current(float current)
+void Battery::consume_energy(float current_amps)
 {
     uint64_t now = AP_HAL::micros64();
     float dt = (now - last_us) * 1.0e-6;
@@ -163,31 +166,29 @@ void Battery::set_current(float current)
         dt = 0;
     }
     last_us = now;
-    float delta_Ah = current * dt / 3600;
+    float delta_Ah = current_amps * dt / 3600;
     remaining_Ah -= delta_Ah;
     remaining_Ah = MAX(0, remaining_Ah);
 
-    float voltage_delta = current * resistance;
-    float voltage;
-    if (!is_positive(capacity_Ah)) {
-        voltage = voltage_set;
-    } else {
-        voltage = get_resting_voltage(100 * remaining_Ah / capacity_Ah) - voltage_delta;
-    }
-
-    voltage_filter.apply(voltage, dt);
+    float voltage_delta = current_amps * resistance_ohm;
+    float sagged_voltage = get_resting_voltage() - voltage_delta;
+    voltage_filter.apply(sagged_voltage, dt);
 
-    {
-        const uint64_t temperature_dt = now - temperature.last_update_micros;
-        temperature.last_update_micros = now;
-        // 1 amp*1 second == 0.1 degrees of energy.  Did those units hurt?
-        temperature.kelvin += 0.1 * current * temperature_dt * 0.000001;
-        // decay temperature at some %second towards ambient
-        temperature.kelvin -= (temperature.kelvin - 273) * 0.10 * temperature_dt * 0.000001;
-    }
+    update_temperature(current_amps, now);
 }
 
-float Battery::get_voltage(void) const
+void Battery::update_temperature(float current_amps, uint64_t now)
 {
-    return voltage_filter.get();
+    const float dt = (now - temperature.last_update_micros) * 1.0e-6;
+    temperature.last_update_micros = now;
+
+    // temperature growth model
+
+    // thermal_capacity value chosen to match previous steady-state behavior at 28amps
+    // (reminder: thermal_capacity = mass * specific_heat)
+    const float inverse_of_thermal_capacity = 0.36f;  // use inverse so we can multiply, not divide
+    temperature.kelvin += (current_amps * current_amps) * resistance_ohm * dt * inverse_of_thermal_capacity;
+
+    // temperature decay model: first-order
+    temperature.kelvin -= (temperature.kelvin - (ambient_temperature + 273.15f)) * 0.10f * dt;
 }

libraries/SITL/SIM_Battery.h

@@ -22,40 +22,45 @@
 
 namespace SITL {
 
+constexpr float ambient_temperature = 0.0f;  // degC
+
 class Battery {
 public:
-    void setup(float _capacity_Ah, float _resistance, float _max_voltage);
+    // Note: capacity<=0 means **unlimited** capacity.
+    void setup(float _capacity_Ah, float _resistance_ohm, float _max_voltage);
 
     // Implements the (re)setting behavior described by SIM_BATT_* parameters
     // (reminder: if the desired values are different than previous choices, reset)
     void maybe_reset(float desired_voltage, float desired_capacity_Ah);
 
-    void init_voltage(float voltage);
-    void init_capacity(float capacity);
+    // Call this periodically to "step" the battery forward in time
+    void consume_energy(float current_amps);
 
-    void set_current(float current_amps);
-    float get_voltage(void) const;
+    float get_voltage(void) const { return voltage_filter.get(); }
 
     // return battery temperature in Kelvin:
     float get_temperature(void) const { return temperature.kelvin; }
 
 private:
-    float capacity_Ah;
-    float resistance;
+    float capacity_Ah; // set <= 0 for unlmited capacity
+    float resistance_ohm;
     float max_voltage;
     float voltage_set;
-    float remaining_Ah;
+    float remaining_Ah; // if capacity is unlimited, set this to FLT_MAX
     uint64_t last_us;
 
     struct {
-        float kelvin = 273;
+        float kelvin = ambient_temperature + 273.15f;
         uint64_t last_update_micros;
     } temperature;
+    void update_temperature(float current_amps, uint64_t now);
 
     // 10Hz filter for battery voltage
     LowPassFilterFloat voltage_filter{10};
 
-    float get_resting_voltage(float charge_pct) const;
-    void set_initial_SoC(float voltage);
+    float get_resting_voltage(void) const;
+    float compute_remaining_ah(float voltage) const; // const shows this has no side effects
+    void set_remaining_ah(void);
+    bool capacity_is_unlimited(void) const { return !(is_positive(capacity_Ah)); } // for readability
 };
 }

libraries/SITL/SIM_Multicopter.cpp

@@ -77,7 +77,7 @@ void MultiCopter::update(const struct sitl_input &input)
     battery_voltage = battery.get_voltage();
     battery_current = frame->get_current_amp();
 
-    battery.set_current(battery_current);
+    battery.consume_energy(battery_current);
     battery_temperature = battery.get_temperature();
 
     update_dynamics(rot_accel);

libraries/SITL/SIM_QuadPlane.cpp

@@ -142,7 +142,7 @@ void QuadPlane::update(const struct sitl_input &input)
     battery_voltage = battery.get_voltage();
     battery_current = frame->get_current_amp();
 
-    battery.set_current(battery_current);
+    battery.consume_energy(battery_current);
     battery_temperature = battery.get_temperature();
 
     float throttle;

libraries/SITL/SITL.cpp

@@ -117,13 +117,13 @@ const AP_Param::GroupInfo SIM::var_info[] = {
     AP_GROUPINFO("SONAR_ROT",     17, SIM,  sonar_rot, Rotation::ROTATION_PITCH_270),
     // @Param: BATT_VOLTAGE
     // @DisplayName: Simulated battery voltage
-    // @Description: Simulated battery voltage. Constant voltage when SIM_BATT_CAP_AH is 0, otherwise changing this parameter will re-initialize the state of charge of the battery based on this voltage versus the battery's maximum voltage (default is max voltage).
+    // @Description: Simulated battery voltage. This parameter's default value is the battery's maximum voltage. A value higher than the max will be treated identically as exactly that max value. When capacity is unlimited (see SIM_BATT_CAP_AH), this is the (constant) resting voltage. Otherwise, changing this value correspondingly (re)sets the battery's charge percentage (by comparing the new value against the max voltage).
     // @Units: V
     // @User: Advanced
     AP_GROUPINFO("BATT_VOLTAGE",  19, SIM,  batt_voltage,  12.6f),
     // @Param: BATT_CAP_AH
     // @DisplayName: Simulated battery capacity
-    // @Description: Simulated battery capacity. Set to 0 for unlimited capacity. Changing this parameter will re-initialize the state of charge of the battery.
+    // @Description: Simulated battery capacity. Set to 0 for unlimited capacity. Changing this value (re)sets the battery's charge percentage (based on SIM_BATT_VOLTAGE).
     // @Units: Ah
     // @User: Advanced
     AP_GROUPINFO("BATT_CAP_AH",   20, SIM,  batt_capacity_ah,  0),