All vehicles use sim battery

libraries/AP_HAL_SITL/SITL_State.cpp

@@ -213,6 +213,7 @@ void SITL_State::_fdm_input_local(void)
 
     // construct servos structure for FDM
     _simulator_servos(input);
+    _set_voltage_and_current_pins(input);
 
 #if AP_SIM_JSON_MASTER_ENABLED
     // read servo inputs from ride along flight controllers
@@ -368,8 +369,29 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
         }
     }
     _sitl->throttle = throttle;
+}
+
+void SITL_State::_set_voltage_and_current_pins(struct sitl_input &input)
+{
+    float voltage = 0;
+    float current = 0;
+
+    if (_sitl->state.battery_voltage > 0) {
+        // FDM provides voltage and current
+        voltage = _sitl->state.battery_voltage;
+        current = _sitl->state.battery_current;
+    } else {
+        // internal sitl model provides voltage and current
+        voltage = sitl_model->get_battery_voltage();
+        current = sitl_model->get_battery_current();
+    }
 
-    update_voltage_current(input, throttle);
+    // assume 3DR power brick
+    voltage_pin_voltage = voltage / 10.1f;
+    current_pin_voltage = current / 17.0f;
+    // fake battery2 as just a 25% gain on the first one
+    voltage2_pin_voltage = voltage_pin_voltage * 0.25f;
+    current2_pin_voltage = current_pin_voltage * 0.25f;
 }
 
 void SITL_State::init(int argc, char * const argv[])

libraries/AP_HAL_SITL/SITL_State.h

@@ -70,6 +70,7 @@ class HALSITL::SITL_State : public SITL_State_Common {
     void _fdm_input_local(void);
     void _output_to_flightgear(void);
     void _simulator_servos(struct sitl_input &input);
+    void _set_voltage_and_current_pins(struct sitl_input &input);
     void _fdm_input_step(void);
 
     void wait_clock(uint64_t wait_time_usec);

libraries/AP_HAL_SITL/SITL_State_common.cpp

@@ -432,47 +432,4 @@ void SITL_State_Common::sim_update(void)
 /*
   update voltage and current pins
  */
-void SITL_State_Common::update_voltage_current(struct sitl_input &input, float throttle)
-{
-    float voltage = 0;
-    float current = 0;
-
-    if (_sitl != nullptr) {
-        if (_sitl->state.battery_voltage <= 0) {
-            if (_vehicle == ArduSub) {
-                voltage = _sitl->batt_voltage;
-                for (uint8_t i=0; i<6; i++) {
-                    float pwm = input.servos[i];
-                    //printf("i: %d, pwm: %.2f\n", i, pwm);
-                    float fraction = fabsf((pwm - 1500) / 500.0f);
-
-                    voltage -= fraction * 0.5f;
-
-                    float draw = fraction * 15;
-                    current += draw;
-                }
-            } else {
-                // simulate simple battery setup
-                // lose 0.7V at full throttle
-                voltage = _sitl->batt_voltage - 0.7f * throttle;
-
-                // assume 50A at full throttle
-                current = 50.0f * throttle;
-            }
-        } else {
-            // FDM provides voltage and current
-            voltage = _sitl->state.battery_voltage;
-            current = _sitl->state.battery_current;
-        }
-    }
-
-    // assume 3DR power brick
-    voltage_pin_voltage = (voltage / 10.1f);
-    current_pin_voltage = current/17.0f;
-    // fake battery2 as just a 25% gain on the first one
-    voltage2_pin_voltage = voltage_pin_voltage * 0.25f;
-    current2_pin_voltage = current_pin_voltage * 0.25f;
-}
-
 #endif // HAL_BOARD_SITL
-

libraries/AP_HAL_SITL/SITL_State_common.h

@@ -218,8 +218,6 @@ class HALSITL::SITL_State_Common {
     SITL::Aircraft *sitl_model;
 
     SITL::SIM *_sitl;
-
-    void update_voltage_current(struct sitl_input &input, float throttle);
 };
 
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL

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 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
@@ -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: 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;
 
@@ -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;