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 : #include "AP_BattMonitor_config.h"
17 :
18 : #if AP_BATTERY_ENABLED
19 :
20 : #include <AP_Common/AP_Common.h>
21 : #include <AP_HAL/AP_HAL.h>
22 : #include "AP_BattMonitor.h"
23 : #include "AP_BattMonitor_Backend.h"
24 :
25 : #if AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED
26 : #include "AP_ESC_Telem/AP_ESC_Telem.h"
27 : #endif
28 :
29 : /*
30 : All backends use the same parameter table and set of indices. Therefore, two
31 : backends must not use the same index. The list of used indices and
32 : corresponding backends is below.
33 :
34 : 1-6: AP_BattMonitor_Analog.cpp
35 : 10-11: AP_BattMonitor_SMBus.cpp
36 : 20: AP_BattMonitor_Sum.cpp
37 : 22-24: AP_BattMonitor_INA3221.cpp
38 : 25-26: AP_BattMonitor_INA2xx.cpp
39 : 27-28: AP_BattMonitor_INA2xx.cpp, AP_BattMonitor_INA239.cpp (legacy duplication)
40 : 30: AP_BattMonitor_DroneCAN.cpp
41 : 36: AP_BattMonitor_ESC.cpp
42 : 40-43: AP_BattMonitor_FuelLevel_Analog.cpp
43 : 45-48: AP_BattMonitor_FuelLevel_Analog.cpp
44 : 50-51: AP_BattMonitor_Synthetic_Current.cpp
45 : 56-61: AP_BattMonitor_AD7091R5.cpp
46 : 62: AP_BattMonitor_TIBQ76952.cpp
47 :
48 : Usage does not need to be contiguous. The maximum possible index is 63.
49 : */
50 :
51 : /*
52 : base class constructor.
53 : This incorporates initialisation as well.
54 : */
55 1897 : AP_BattMonitor_Backend::AP_BattMonitor_Backend(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state,
56 1897 : AP_BattMonitor_Params ¶ms) :
57 1897 : _mon(mon),
58 1897 : _state(mon_state),
59 1897 : _params(params)
60 : {
61 1897 : }
62 :
63 : // capacity_remaining_pct - returns true if the battery % is available and writes to the percentage argument
64 : // return false if the battery is unhealthy, does not have current monitoring, or the pack_capacity is too small
65 2939684 : bool AP_BattMonitor_Backend::capacity_remaining_pct(uint8_t &percentage) const
66 : {
67 : // we consider anything under 10 mAh as being an invalid capacity and so will be our measurement of remaining capacity
68 2939684 : if ( _params._pack_capacity <= 10) {
69 4 : return false;
70 : }
71 :
72 : // the monitor must have current readings in order to estimate consumed_mah and be healthy
73 2939680 : if (!has_current() || !_state.healthy) {
74 3167 : return false;
75 : }
76 2936513 : if (isnan(_state.consumed_mah) || _params._pack_capacity <= 0) {
77 0 : return false;
78 : }
79 :
80 2936513 : const float mah_remaining = _params._pack_capacity - _state.consumed_mah;
81 2936513 : percentage = constrain_float(100 * mah_remaining / _params._pack_capacity, 0, UINT8_MAX);
82 2936513 : return true;
83 : }
84 :
85 : // update battery resistance estimate
86 : // faster rates of change of the current and voltage readings cause faster updates to the resistance estimate
87 : // the battery resistance is calculated by comparing the latest current and voltage readings to a low-pass filtered current and voltage
88 : // high current steps are integrated into the resistance estimate by varying the time constant of the resistance filter
89 1260190 : void AP_BattMonitor_Backend::update_resistance_estimate()
90 : {
91 : // return immediately if no current
92 1260190 : if (!has_current() || !is_positive(_state.current_amps)) {
93 479474 : return;
94 : }
95 :
96 : // update maximum current seen since startup and protect against divide by zero
97 780716 : _current_max_amps = MAX(_current_max_amps, _state.current_amps);
98 780716 : float current_delta = _state.current_amps - _current_filt_amps;
99 780716 : if (is_zero(current_delta)) {
100 70432 : return;
101 : }
102 :
103 : // update reference voltage and current
104 710284 : if (_state.voltage > _resistance_voltage_ref) {
105 4514 : _resistance_voltage_ref = _state.voltage;
106 4514 : _resistance_current_ref = _state.current_amps;
107 : }
108 :
109 : // calculate time since last update
110 710284 : uint32_t now = AP_HAL::millis();
111 710284 : float loop_interval = (now - _resistance_timer_ms) * 0.001f;
112 710284 : _resistance_timer_ms = now;
113 :
114 : // estimate short-term resistance
115 710284 : float filt_alpha = constrain_float(loop_interval/(loop_interval + AP_BATT_MONITOR_RES_EST_TC_1), 0.0f, 0.5f);
116 710284 : float resistance_alpha = MIN(1, AP_BATT_MONITOR_RES_EST_TC_2*fabsf((_state.current_amps-_current_filt_amps)/_current_max_amps));
117 710284 : float resistance_estimate = -(_state.voltage-_voltage_filt)/current_delta;
118 710284 : if (is_positive(resistance_estimate)) {
119 604496 : _state.resistance = _state.resistance*(1-resistance_alpha) + resistance_estimate*resistance_alpha;
120 : }
121 :
122 : // calculate maximum resistance
123 710284 : if ((_resistance_voltage_ref > _state.voltage) && (_state.current_amps > _resistance_current_ref)) {
124 694069 : float resistance_max = (_resistance_voltage_ref - _state.voltage) / (_state.current_amps - _resistance_current_ref);
125 694069 : _state.resistance = MIN(_state.resistance, resistance_max);
126 : }
127 :
128 : // update the filtered voltage and currents
129 710284 : _voltage_filt = _voltage_filt*(1-filt_alpha) + _state.voltage*filt_alpha;
130 710284 : _current_filt_amps = _current_filt_amps*(1-filt_alpha) + _state.current_amps*filt_alpha;
131 :
132 : // update estimated voltage without sag
133 710284 : _state.voltage_resting_estimate = _state.voltage + _state.current_amps * _state.resistance;
134 : }
135 :
136 : // return true if state of health can be provided and fills in soh_pct argument
137 1214233 : bool AP_BattMonitor_Backend::get_state_of_health_pct(uint8_t &soh_pct) const
138 : {
139 1214233 : if (!_state.has_state_of_health_pct) {
140 1214233 : return false;
141 : }
142 0 : soh_pct = _state.state_of_health_pct;
143 0 : return true;
144 : }
145 :
146 28831805 : float AP_BattMonitor_Backend::voltage_resting_estimate() const
147 : {
148 : // resting voltage should always be greater than or equal to the raw voltage
149 28831805 : return MAX(_state.voltage, _state.voltage_resting_estimate);
150 : }
151 :
152 823147 : AP_BattMonitor::Failsafe AP_BattMonitor_Backend::update_failsafes(void)
153 : {
154 823147 : const uint32_t now = AP_HAL::millis();
155 :
156 : bool low_voltage, low_capacity, critical_voltage, critical_capacity;
157 823147 : check_failsafe_types(low_voltage, low_capacity, critical_voltage, critical_capacity);
158 :
159 823147 : if (critical_voltage) {
160 : // this is the first time our voltage has dropped below minimum so start timer
161 3192 : if (_state.critical_voltage_start_ms == 0) {
162 4 : _state.critical_voltage_start_ms = now;
163 6376 : } else if (_params._low_voltage_timeout > 0 &&
164 3188 : now - _state.critical_voltage_start_ms > uint32_t(_params._low_voltage_timeout)*1000U) {
165 2788 : return AP_BattMonitor::Failsafe::Critical;
166 : }
167 : } else {
168 : // acceptable voltage so reset timer
169 819955 : _state.critical_voltage_start_ms = 0;
170 : }
171 :
172 820359 : if (critical_capacity) {
173 0 : return AP_BattMonitor::Failsafe::Critical;
174 : }
175 :
176 820359 : if (low_voltage) {
177 : // this is the first time our voltage has dropped below minimum so start timer
178 3257 : if (_state.low_voltage_start_ms == 0) {
179 10 : _state.low_voltage_start_ms = now;
180 6494 : } else if (_params._low_voltage_timeout > 0 &&
181 3247 : now - _state.low_voltage_start_ms > uint32_t(_params._low_voltage_timeout)*1000U) {
182 2247 : return AP_BattMonitor::Failsafe::Low;
183 : }
184 : } else {
185 : // acceptable voltage so reset timer
186 817102 : _state.low_voltage_start_ms = 0;
187 : }
188 :
189 818112 : if (low_capacity) {
190 824 : return AP_BattMonitor::Failsafe::Low;
191 : }
192 :
193 : // 5 second health timeout
194 817288 : if ((now - _state.last_healthy_ms) > 5000) {
195 212 : return AP_BattMonitor::Failsafe::Unhealthy;
196 : }
197 :
198 : // if we've gotten this far then battery is ok
199 817076 : return AP_BattMonitor::Failsafe::None;
200 : }
201 :
202 1146180 : static bool update_check(size_t buflen, char *buffer, bool failed, const char *message)
203 : {
204 1146180 : if (failed) {
205 347 : strncpy(buffer, message, buflen);
206 347 : return false;
207 : }
208 1145833 : return true;
209 : }
210 :
211 127487 : bool AP_BattMonitor_Backend::arming_checks(char * buffer, size_t buflen) const
212 : {
213 : bool low_voltage, low_capacity, critical_voltage, critical_capacity;
214 127487 : check_failsafe_types(low_voltage, low_capacity, critical_voltage, critical_capacity);
215 :
216 127487 : bool below_arming_voltage = is_positive(_params._arming_minimum_voltage) &&
217 0 : (_state.voltage < _params._arming_minimum_voltage);
218 127487 : bool below_arming_capacity = (_params._arming_minimum_capacity > 0) &&
219 0 : ((_params._pack_capacity - _state.consumed_mah) < _params._arming_minimum_capacity);
220 127884 : bool fs_capacity_inversion = is_positive(_params._critical_capacity) &&
221 127884 : is_positive(_params._low_capacity) &&
222 397 : !(_params._low_capacity > _params._critical_capacity);
223 128026 : bool fs_voltage_inversion = is_positive(_params._critical_voltage) &&
224 128026 : is_positive(_params._low_voltage) &&
225 539 : !(_params._low_voltage > _params._critical_voltage);
226 :
227 127487 : bool result = update_check(buflen, buffer, !_state.healthy, "unhealthy");
228 127487 : result = result && update_check(buflen, buffer, below_arming_voltage, "below minimum arming voltage");
229 127487 : result = result && update_check(buflen, buffer, below_arming_capacity, "below minimum arming capacity");
230 127487 : result = result && update_check(buflen, buffer, critical_voltage, "critical voltage failsafe");
231 127487 : result = result && update_check(buflen, buffer, critical_capacity, "critical capacity failsafe");
232 127487 : result = result && update_check(buflen, buffer, low_voltage, "low voltage failsafe");
233 127487 : result = result && update_check(buflen, buffer, low_capacity, "low capacity failsafe");
234 127487 : result = result && update_check(buflen, buffer, fs_capacity_inversion, "capacity failsafe critical >= low");
235 127487 : result = result && update_check(buflen, buffer, fs_voltage_inversion, "voltage failsafe critical >= low");
236 :
237 127487 : return result;
238 : }
239 :
240 950634 : void AP_BattMonitor_Backend::check_failsafe_types(bool &low_voltage, bool &low_capacity, bool &critical_voltage, bool &critical_capacity) const
241 : {
242 : // use voltage or sag compensated voltage
243 : float voltage_used;
244 950634 : switch (_params.failsafe_voltage_source()) {
245 950634 : case AP_BattMonitor_Params::BattMonitor_LowVoltageSource_Raw:
246 : default:
247 950634 : voltage_used = _state.voltage;
248 950634 : break;
249 0 : case AP_BattMonitor_Params::BattMonitor_LowVoltageSource_SagCompensated:
250 0 : voltage_used = voltage_resting_estimate();
251 0 : break;
252 : }
253 :
254 : // check critical battery levels
255 950634 : if ((voltage_used > 0) && (_params._critical_voltage > 0) && (voltage_used < _params._critical_voltage)) {
256 3197 : critical_voltage = true;
257 : } else {
258 947437 : critical_voltage = false;
259 : }
260 :
261 : // check capacity failsafe if current monitoring is enabled
262 953477 : if (has_current() && (_params._critical_capacity > 0) &&
263 2843 : ((_params._pack_capacity - _state.consumed_mah) < _params._critical_capacity)) {
264 1 : critical_capacity = true;
265 : } else {
266 950633 : critical_capacity = false;
267 : }
268 :
269 950634 : if ((voltage_used > 0) && (_params._low_voltage > 0) && (voltage_used < _params._low_voltage)) {
270 6356 : low_voltage = true;
271 : } else {
272 944278 : low_voltage = false;
273 : }
274 :
275 : // check capacity if current monitoring is enabled
276 954929 : if (has_current() && (_params._low_capacity > 0) &&
277 4295 : ((_params._pack_capacity - _state.consumed_mah) < _params._low_capacity)) {
278 829 : low_capacity = true;
279 : } else {
280 949805 : low_capacity = false;
281 : }
282 950634 : }
283 :
284 : #if AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED
285 : void AP_BattMonitor_Backend::update_esc_telem_outbound()
286 : {
287 : const uint8_t esc_index = _params._esc_telem_outbound_index;
288 : if (esc_index == 0 || !_state.healthy) {
289 : // Disabled if there's no ESC identified to route the data to or if the battery is unhealthy
290 : return;
291 : }
292 :
293 : AP_ESC_Telem_Backend::TelemetryData telem {};
294 :
295 : uint16_t type = AP_ESC_Telem_Backend::TelemetryType::VOLTAGE;
296 : telem.voltage = _state.voltage; // all battery backends have voltage
297 :
298 : if (has_current()) {
299 : telem.current = _state.current_amps;
300 : type |= AP_ESC_Telem_Backend::TelemetryType::CURRENT;
301 : }
302 :
303 : if (has_consumed_energy()) {
304 : telem.consumption_mah = _state.consumed_mah;
305 : type |= AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION;
306 : }
307 :
308 : float temperature_c;
309 : if (_mon.get_temperature(temperature_c, _state.instance)) {
310 : // get the temperature from the frontend so we check for external temperature
311 : telem.temperature_cdeg = temperature_c * 100;
312 : type |= AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE;
313 : }
314 :
315 : AP::esc_telem().update_telem_data(esc_index-1, telem, type);
316 : }
317 : #endif
318 :
319 : // returns true if battery monitor provides temperature
320 2082447 : bool AP_BattMonitor_Backend::get_temperature(float &temperature) const
321 : {
322 : #if AP_TEMPERATURE_SENSOR_ENABLED
323 2082447 : if (_state.temperature_external_use) {
324 0 : temperature = _state.temperature_external;
325 0 : return true;
326 : }
327 : #endif
328 :
329 2082447 : temperature = _state.temperature;
330 :
331 2082447 : return has_temperature();
332 : }
333 :
334 : /*
335 : default implementation for reset_remaining(). This sets consumed_wh
336 : and consumed_mah based on the given percentage. Use percentage=100
337 : for a full battery
338 : */
339 8 : bool AP_BattMonitor_Backend::reset_remaining(float percentage)
340 : {
341 8 : percentage = constrain_float(percentage, 0, 100);
342 8 : const float used_proportion = (100.0f - percentage) * 0.01f;
343 8 : _state.consumed_mah = used_proportion * _params._pack_capacity;
344 : // without knowing the history we can't do consumed_wh
345 : // accurately. Best estimate is based on current voltage. This
346 : // will be good when resetting the battery to a value close to
347 : // full charge
348 8 : _state.consumed_wh = _state.consumed_mah * 0.001f * _state.voltage;
349 :
350 : // reset failsafe state for this backend
351 8 : _state.failsafe = update_failsafes();
352 :
353 8 : return true;
354 : }
355 :
356 : /*
357 : update consumed mAh and Wh
358 : */
359 1237084 : void AP_BattMonitor_Backend::update_consumed(AP_BattMonitor::BattMonitor_State &state, uint32_t dt_us)
360 : {
361 : // update total current drawn since startup
362 1237084 : if (state.last_time_micros != 0 && dt_us < 2000000) {
363 1235204 : const float mah = calculate_mah(state.current_amps, dt_us);
364 1235204 : state.consumed_mah += mah;
365 1235204 : state.consumed_wh += 0.001 * mah * state.voltage;
366 : }
367 1237084 : }
368 :
369 : #endif // AP_BATTERY_ENABLED
|
