Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : battery model for electric aircraft
17 : */
18 :
19 : #include "SIM_Battery.h"
20 : #include <float.h>
21 : #include <AP_Math/AP_Math.h>
22 :
23 : using namespace SITL;
24 :
25 : /*
26 : state of charge table for a single cell battery.
27 : */
28 : static const struct {
29 : float volt_per_cell;
30 : float soc_pct;
31 : } soc_table[] = {
32 : { 4.173, 100 },
33 : { 4.112, 96.15 },
34 : { 4.085, 92.31 },
35 : { 4.071, 88.46 },
36 : { 4.039, 84.62 },
37 : { 3.987, 80.77 },
38 : { 3.943, 76.92 },
39 : { 3.908, 73.08 },
40 : { 3.887, 69.23 },
41 : { 3.854, 65.38 },
42 : { 3.833, 61.54 },
43 : { 3.801, 57.69 },
44 : { 3.783, 53.85 },
45 : { 3.742, 50 },
46 : { 3.715, 46.15 },
47 : { 3.679, 42.31 },
48 : { 3.636, 38.46 },
49 : { 3.588, 34.62 },
50 : { 3.543, 30.77 },
51 : { 3.503, 26.92 },
52 : { 3.462, 23.08 },
53 : { 3.379, 19.23 },
54 : { 3.296, 15.38 },
55 : { 3.218, 11.54 },
56 : { 3.165, 7.69 },
57 : { 3.091, 3.85 },
58 : { 2.977, 2.0 },
59 : { 2.8, 1.5 },
60 : { 2.7, 1.3 },
61 : { 2.5, 1.2 },
62 : { 2.3, 1.1 },
63 : { 2.1, 1.0 },
64 : { 1.9, 0.9 },
65 : { 1.6, 0.8 },
66 : { 1.3, 0.7 },
67 : { 1.0, 0.6 },
68 : { 0.6, 0.4 },
69 : { 0.3, 0.2 },
70 : { 0.01, 0.01},
71 : { 0.001, 0.001 }};
72 :
73 : constexpr float maximum_permissible_dt = 0.1f; // seconds
74 :
75 : /*
76 : use table to get resting voltage from remaining capacity
77 : */
78 177104555 : float Battery::get_resting_voltage(void) const
79 : {
80 177104555 : if (capacity_is_unlimited()) {
81 176312215 : return voltage_set;
82 : }
83 792340 : const float charge_pct = 100 * remaining_Ah / capacity_Ah;
84 792340 : const float max_cell_voltage = soc_table[0].volt_per_cell;
85 792340 : const float min_cell_voltage = soc_table[ARRAY_SIZE(soc_table) - 1].volt_per_cell;
86 1156389 : for (uint8_t i=1; i<ARRAY_SIZE(soc_table); i++) {
87 1156389 : if (charge_pct >= soc_table[i].soc_pct) {
88 : // linear interpolation between table rows
89 792340 : const float dv1 = charge_pct - soc_table[i].soc_pct;
90 792340 : const float dv2 = soc_table[i-1].soc_pct - soc_table[i].soc_pct;
91 792340 : const float vpc1 = soc_table[i].volt_per_cell;
92 792340 : const float vpc2 = soc_table[i-1].volt_per_cell;
93 792340 : const float cell_volt = vpc1 + (dv1 / dv2) * (vpc2 - vpc1);
94 792340 : return (cell_volt / max_cell_voltage) * max_voltage;
95 : }
96 : }
97 : // off the bottom of the table
98 0 : return min_cell_voltage;
99 : }
100 :
101 : /*
102 : return remaining Amp-hours (aka "charge", "state of charge") corresponding to a voltage
103 : */
104 3437 : float Battery::compute_remaining_Ah(float voltage) const
105 : {
106 3437 : if (capacity_is_unlimited()) {
107 3330 : return FLT_MAX;
108 : }
109 :
110 107 : const float max_cell_voltage = soc_table[0].volt_per_cell;
111 107 : const float cell_volt = (voltage / max_voltage) * max_cell_voltage;
112 :
113 404 : for (uint8_t i=1; i<ARRAY_SIZE(soc_table); i++) {
114 398 : if (cell_volt >= soc_table[i].volt_per_cell) {
115 : // linear interpolation between table rows
116 101 : const float dv1 = cell_volt - soc_table[i].volt_per_cell;
117 101 : const float dv2 = soc_table[i-1].volt_per_cell - soc_table[i].volt_per_cell;
118 101 : const float soc1 = soc_table[i].soc_pct;
119 101 : const float soc2 = soc_table[i-1].soc_pct;
120 101 : const float soc = soc1 + (dv1 / dv2) * (soc2 - soc1);
121 101 : return capacity_Ah * (soc * 0.01);
122 : }
123 : }
124 : // off the bottom of the table
125 6 : return 0.0f;
126 : }
127 :
128 : // Reminder: capacity <= 0 means **unlimited**
129 3345 : void Battery::setup(float _capacity_Ah, float _resistance_ohm, float _max_voltage, float _ambient_temperature_degC)
130 : {
131 3345 : capacity_Ah = _capacity_Ah;
132 3345 : resistance_ohm = _resistance_ohm;
133 3345 : max_voltage = _max_voltage;
134 3345 : ambient_temperature_degC = _ambient_temperature_degC;
135 :
136 3345 : voltage_set = max_voltage;
137 3345 : voltage_filter.reset(voltage_set);
138 3345 : remaining_Ah = compute_remaining_Ah(voltage_set);
139 3345 : }
140 :
141 : // A negative value for desired_resistance means "no change".
142 176991608 : void Battery::maybe_reset(float desired_voltage, float desired_capacity_Ah, float desired_resistance_ohm)
143 : {
144 176991608 : if (!is_negative(desired_resistance_ohm)) {
145 1559791 : resistance_ohm = desired_resistance_ohm;
146 : }
147 :
148 176991608 : const bool reset_not_needed = (is_equal(voltage_set, desired_voltage)
149 176991608 : && is_equal(capacity_Ah, desired_capacity_Ah));
150 176991608 : if (reset_not_needed) {
151 176991516 : return;
152 : }
153 :
154 92 : capacity_Ah = desired_capacity_Ah;
155 : // a negative desired voltage is unexpected, but not problematic
156 92 : voltage_set = MIN(desired_voltage, max_voltage);
157 92 : voltage_filter.reset(voltage_set);
158 92 : remaining_Ah = compute_remaining_Ah(voltage_set);
159 : }
160 :
161 177104631 : void Battery::consume_energy(float attempted_current_amp, uint64_t now_us)
162 : {
163 177104631 : constexpr float microsec_to_sec = 1.0e-6f;
164 177104631 : const float dt = static_cast<float>(now_us - last_us) * microsec_to_sec;
165 177104631 : if (dt <= 0.0f) {
166 76 : return;
167 : }
168 177104611 : last_us = now_us;
169 177104611 : if (dt > maximum_permissible_dt) {
170 56 : return;
171 : }
172 177104555 : constexpr float hours_per_second = 1.0f / 3600.0f;
173 177104555 : const float dt_hr = dt * hours_per_second;
174 177104555 : const float delta_Ah = MIN(attempted_current_amp * dt_hr, remaining_Ah);
175 177104555 : if (!capacity_is_unlimited()) {
176 792340 : remaining_Ah -= delta_Ah;
177 : }
178 177104555 : const float current_amp = delta_Ah / dt_hr;
179 177104555 : const float voltage_delta = current_amp * resistance_ohm;
180 177104555 : const float sagged_voltage = get_resting_voltage() - voltage_delta;
181 177104555 : voltage_filter.apply(sagged_voltage, dt);
182 :
183 177104555 : update_temperature(current_amp, dt);
184 : }
185 :
186 : // A first-order temperature growth & decay model
187 177104555 : void Battery::update_temperature(float current_amp, float dt)
188 : {
189 : // Reasonable thermal_capacity value for a commonly sized (500g) Li-ion battery
190 : // (reminder: thermal_capacity = mass * specific_heat)
191 177104555 : constexpr float thermal_capacity = 500.0f; // J/degC
192 177104555 : constexpr float inverse_of_thermal_capacity = 1 / thermal_capacity; // use inverse so we can multiply, not divide
193 177104555 : const float temp_increase = (current_amp * current_amp) * resistance_ohm * inverse_of_thermal_capacity * dt;
194 : // Account for ambient dissipation. Rate chosen to match previous (unjustified) steady-state behavior
195 177104555 : constexpr float temperature_decay_coefficient = 5.6e-4f;
196 177104555 : const float temp_decrease = (temperature_degC - ambient_temperature_degC) * temperature_decay_coefficient * dt;
197 177104555 : temperature_degC += (temp_increase - temp_decrease);
198 177104555 : }
|
