ardupilot/libraries/SITL/SIM_Battery.cpp

/*
   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
/*
  battery model for electric aircraft
*/

#include "SIM_Battery.h"
#include <float.h>
#include <AP_Math/AP_Math.h>

using namespace SITL;

/*
  state of charge table for a single cell battery.
 */
static const struct {
    float volt_per_cell;
    float soc_pct;
} soc_table[] = {
    { 4.173, 100 },
    { 4.112, 96.15 },
    { 4.085, 92.31 },
    { 4.071, 88.46 },
    { 4.039, 84.62 },
    { 3.987, 80.77 },
    { 3.943, 76.92 },
    { 3.908, 73.08 },
    { 3.887, 69.23 },
    { 3.854, 65.38 },
    { 3.833, 61.54 },
    { 3.801, 57.69 },
    { 3.783, 53.85 },
    { 3.742, 50 },
    { 3.715, 46.15 },
    { 3.679, 42.31 },
    { 3.636, 38.46 },
    { 3.588, 34.62 },
    { 3.543, 30.77 },
    { 3.503, 26.92 },
    { 3.462, 23.08 },
    { 3.379, 19.23 },
    { 3.296, 15.38 },
    { 3.218, 11.54 },
    { 3.165, 7.69 },
    { 3.091, 3.85 },
    { 2.977, 2.0 },
    { 2.8,   1.5 },
    { 2.7,   1.3 },
    { 2.5,   1.2 },
    { 2.3,   1.1 },
    { 2.1,   1.0 },
    { 1.9,   0.9 },
    { 1.6,   0.8 },
    { 1.3,   0.7 },
    { 1.0,   0.6 },
    { 0.6,   0.4 },
    { 0.3,   0.2 },
    { 0.01,  0.01},
    { 0.001, 0.001 }};

constexpr float maximum_permissible_dt = 0.1f; // seconds

/*
  use table to get resting voltage from remaining capacity
 */
float Battery::get_resting_voltage(void) const
{
    if (capacity_is_unlimited()) {
        return voltage_set;
    }
    const 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
            const float dv1 = charge_pct - soc_table[i].soc_pct;
            const float dv2 = soc_table[i-1].soc_pct - soc_table[i].soc_pct;
            const float vpc1 = soc_table[i].volt_per_cell;
            const float vpc2 = soc_table[i-1].volt_per_cell;
            const float cell_volt = vpc1 + (dv1 / dv2) * (vpc2 - vpc1);
            return (cell_volt / max_cell_voltage) * max_voltage;
        }
    }
    // off the bottom of the table
    return min_cell_voltage;
}

/*
  return remaining Amp-hours (aka "charge", "state of charge") corresponding to a 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;
    const float cell_volt = (voltage / max_voltage) * max_cell_voltage;

    for (uint8_t i=1; i<ARRAY_SIZE(soc_table); i++) {
        if (cell_volt >= soc_table[i].volt_per_cell) {
            // linear interpolation between table rows
            const float dv1 = cell_volt - soc_table[i].volt_per_cell;
            const float dv2 = soc_table[i-1].volt_per_cell - soc_table[i].volt_per_cell;
            const float soc1 = soc_table[i].soc_pct;
            const float soc2 = soc_table[i-1].soc_pct;
            const float soc = soc1 + (dv1 / dv2) * (soc2 - soc1);
            return capacity_Ah * (soc * 0.01);
        }
    }
    // off the bottom of the table
    return 0.0f;
}

// Reminder: capacity <= 0 means **unlimited**
void Battery::setup(float _capacity_Ah, float _resistance_ohm, float _max_voltage, float _ambient_temperature_degC)
{
    capacity_Ah = _capacity_Ah;
    resistance_ohm = _resistance_ohm;
    max_voltage = _max_voltage;
    ambient_temperature_degC = _ambient_temperature_degC;

    voltage_set = max_voltage;
    voltage_filter.reset(voltage_set);
    remaining_Ah = compute_remaining_Ah(voltage_set);
}

// A negative value for desired_resistance means "no change".
void Battery::maybe_reset(float desired_voltage, float desired_capacity_Ah, float desired_resistance_ohm)
{
    if (!is_negative(desired_resistance_ohm)) {
        resistance_ohm = desired_resistance_ohm;
    }

    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);
    remaining_Ah = compute_remaining_Ah(voltage_set);
}

void Battery::consume_energy(float attempted_current_amp, uint64_t now_us)
{
    constexpr float microsec_to_sec = 1.0e-6f;
    const float dt = static_cast<float>(now_us - last_us) * microsec_to_sec;
    if (dt <= 0.0f) {
        return;
    }
    last_us = now_us;
    if (dt > maximum_permissible_dt) {
        return;
    }
    constexpr float hours_per_second = 1.0f / 3600.0f;
    const float dt_hr = dt * hours_per_second;
    const float delta_Ah = MIN(attempted_current_amp * dt_hr, remaining_Ah);
    if (!capacity_is_unlimited()) {
        remaining_Ah -= delta_Ah;
    }
    const float current_amp = delta_Ah / dt_hr;
    const float voltage_delta = current_amp * resistance_ohm;
    const float sagged_voltage = get_resting_voltage() - voltage_delta;
    voltage_filter.apply(sagged_voltage, dt);

    update_temperature(current_amp, dt);
}

// A first-order temperature growth & decay model
void Battery::update_temperature(float current_amp, float dt)
{
    // Reasonable thermal_capacity value for a commonly sized (500g) Li-ion battery
    // (reminder: thermal_capacity = mass * specific_heat)
    constexpr float thermal_capacity = 500.0f;  // J/degC
    constexpr float inverse_of_thermal_capacity = 1 / thermal_capacity;  // use inverse so we can multiply, not divide
    const float temp_increase = (current_amp * current_amp) * resistance_ohm * inverse_of_thermal_capacity * dt;
    // Account for ambient dissipation. Rate chosen to match previous (unjustified) steady-state behavior
    constexpr float temperature_decay_coefficient = 5.6e-4f;
    const float temp_decrease = (temperature_degC - ambient_temperature_degC) * temperature_decay_coefficient * dt;
    temperature_degC += (temp_increase - temp_decrease);
}