Cleanup and improve sim battery

libraries/SITL/SIM_Aircraft.cpp

@@ -50,8 +50,7 @@ Aircraft *Aircraft::instances[MAX_SIM_INSTANCES];
   parent class for all simulator types
  */
 
-Aircraft::Aircraft(const char *frame_str) :
-    frame(frame_str)
+Aircraft::Aircraft(const char *unused_frame_str)
 {
     // make the SIM_* variables available to simulator backends
     sitl = AP::sitl();

libraries/SITL/SIM_Aircraft.h

@@ -53,7 +53,7 @@ namespace SITL {
  */
 class Aircraft {
 public:
-    Aircraft(const char *frame_str);
+    Aircraft(const char *unused_frame_str);
 
     // called directly after constructor:
     virtual void set_start_location(const Location &start_loc, const float start_yaw);
@@ -174,7 +174,8 @@ class Aircraft {
     void set_dronecan_device(DroneCANDevice *_dronecan) { dronecan = _dronecan; }
 #endif
     float get_battery_voltage() const { return battery_voltage; }
-    float get_battery_temperature() const { return battery.get_temperature(); }
+    float get_battery_temperature() const { return battery_temperature; }
+    float get_battery_current() const { return battery_current; }
 
     float ambient_temperature_degC() const;
 
@@ -222,12 +223,14 @@ class Aircraft {
     float airspeed_pitot;                // m/s, EAS airspeed, as seen by fwd pitot tube
     float battery_voltage;
     float battery_current;
+    float battery_temperature;
     float local_ground_level;            // ground level at local position
     bool lock_step_scheduled;
     bool flightaxis_sync_imus_to_frames; // causes the frame counter to be incremented on each timestep, IMUs will then update at the same rate
     uint32_t last_one_hz_ms;
 
-    // battery model
+    // OPTIONAL battery model
+    // ("OPTIONAL" because a child can ignore it and directly set/get battery_* members.)
     Battery battery;
 
     uint32_t motor_mask;
@@ -274,7 +277,6 @@ class Aircraft {
     uint32_t last_frame_count;
     uint8_t instance;
     const char *autotest_dir;
-    const char *frame;
     bool use_time_sync = true;
     float last_speedup = -1.0f;
     const char *config_ = "";

libraries/SITL/SIM_Battery.cpp

@@ -17,6 +17,8 @@
 */
 
 #include "SIM_Battery.h"
+#include <float.h>
+#include <AP_Math/AP_Math.h>
 
 using namespace SITL;
 
@@ -71,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 voltage_set;
+    }
+    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
@@ -85,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: the remaining_ah computation 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;
 
@@ -105,36 +118,46 @@ 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)
 {
-    capacity_Ah = _capacity_Ah;
-    resistance = _resistance;
-    max_voltage = _max_voltage;
+    remaining_Ah = compute_remaining_ah(voltage_set);
 }
 
-void Battery::init_voltage(float voltage)
+// Reminder: capacity <= 0 means **unlimited**
+void Battery::setup(float _capacity_Ah, float _resistance_ohm, float _max_voltage)
 {
-    voltage_filter.reset(voltage);
-    voltage_set = voltage;
-    set_initial_SoC(voltage);
+    capacity_Ah = _capacity_Ah;
+    resistance_ohm = _resistance_ohm;
+    max_voltage = _max_voltage;
+
+    voltage_set = max_voltage;
+    voltage_filter.reset(voltage_set);
+    set_remaining_ah();
 }
 
-void Battery::init_capacity(float capacity)
+void Battery::maybe_reset(float desired_voltage, float desired_capacity_Ah)
 {
-    capacity_Ah = capacity;
-    set_initial_SoC(voltage_set);
+    const bool reset_not_needed = (is_equal(voltage_set, desired_voltage)
+                                   && is_equal(capacity_Ah, desired_capacity_Ah));
+    if (reset_not_needed) {
+        return;
+    }
+
+    capacity_Ah = 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_remaining_ah();
 }
 
-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;
@@ -143,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,37 +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_capacity(void) const { return capacity_Ah; }
+    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_FlightAxis.cpp

@@ -656,6 +656,8 @@ void FlightAxis::update(const struct sitl_input &input)
 
     battery_voltage = MAX(state.m_batteryVoltage_VOLTS, 0);
     battery_current = MAX(state.m_batteryCurrentDraw_AMPS, 0);
+    // this aircraft has a constant battery temperature
+    battery_temperature = 273.0f; // kelvin
     rpm[0] = state.m_heliMainRotorRPM;
     rpm[1] = state.m_propRPM;
     motor_mask = 3;

libraries/SITL/SIM_Frame.cpp

@@ -577,10 +577,9 @@ void Frame::parse_vector3(AP_JSON::value val, const char* label, Vector3f &param
 /*
   initialise the frame
  */
-void Frame::init(const char *frame_str, Battery *_battery)
+void Frame::init(const char *frame_str)
 {
     model = default_model;
-    battery = _battery;
 
     const char *colon = strchr(frame_str, ':');
     size_t slen = strlen(frame_str);
@@ -620,8 +619,6 @@ void Frame::init(const char *frame_str, Battery *_battery)
     // power_factor is ratio of power consumed per newton of thrust
     float power_factor = hover_power / hover_thrust;
 
-    battery->setup(model.battCapacityAh, model.refBatRes, model.maxVoltage);
-
     if (uint8_t(model.num_motors) != num_motors) {
         ::printf("Warning model expected %u motors and got %u\n", uint8_t(model.num_motors), num_motors);
     }
@@ -682,7 +679,8 @@ void Frame::calculate_forces(const Aircraft &aircraft,
     const auto *_sitl = AP::sitl();
     for (uint8_t i=0; i<num_motors; i++) {
         Vector3f mtorque, mthrust;
-        motors[i].calculate_forces(input, motor_offset, mtorque, mthrust, vel_air_bf, gyro, air_density, battery->get_voltage(), use_drag);
+        motors[i].calculate_forces(input, motor_offset, mtorque, mthrust, vel_air_bf,
+                                   gyro, air_density, aircraft.get_battery_voltage(), use_drag);
         torque += mtorque;
         thrust += mthrust;
         // simulate motor rpm
@@ -727,23 +725,13 @@ void Frame::calculate_forces(const Aircraft &aircraft,
     body_accel = thrust/aircraft.gross_mass();
 }
 
-
-// calculate current and voltage
-void Frame::current_and_voltage(float &voltage, float &current)
+// computes (total) instantaneous current
+float Frame::get_current_amp(void)
 {
-    float param_voltage = AP::sitl()->batt_voltage;
-    if (!is_equal(last_param_voltage,param_voltage)) {
-        battery->init_voltage(param_voltage);
-        last_param_voltage = param_voltage;
-    }
-    const float param_capacity = AP::sitl()->batt_capacity_ah;
-    if (!is_equal(battery->get_capacity(), param_capacity)) {
-        battery->init_capacity(param_capacity);
-    }
-    voltage = battery->get_voltage();
-    current = 0;
+    float current = 0;
     for (uint8_t i=0; i<num_motors; i++) {
         current += motors[i].get_current();
     }
+    return current;
 }
 #endif // AP_SIM_ENABLED