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 :
17 : #include "AP_BattMonitor_config.h"
18 :
19 : #if AP_BATTERY_ESC_ENABLED
20 :
21 : #include "AP_BattMonitor_ESC.h"
22 :
23 : const AP_Param::GroupInfo AP_BattMonitor_ESC::var_info[] = {
24 :
25 : // @Param: ESC_MASK
26 : // @DisplayName: ESC mask
27 : // @Description: If 0 all connected ESCs will be used. If non-zero, only those selected in will be used.
28 : // @Bitmask: 0: ESC 1, 1: ESC 2, 2: ESC 3, 3: ESC 4, 4: ESC 5, 5: ESC 6, 6: ESC 7, 7: ESC 8, 8: ESC 9, 9: ESC 10, 10: ESC 11, 11: ESC 12, 12: ESC 13, 13: ESC 14, 14: ESC 15, 15: ESC 16, 16: ESC 17, 17: ESC 18, 18: ESC 19, 19: ESC 20, 20: ESC 21, 21: ESC 22, 22: ESC 23, 23: ESC 24, 24: ESC 25, 25: ESC 26, 26: ESC 27, 27: ESC 28, 28: ESC 29, 29: ESC 30, 30: ESC 31, 31: ESC 32
29 : // @User: Standard
30 : AP_GROUPINFO("ESC_MASK", 36, AP_BattMonitor_ESC, _mask, 0),
31 :
32 : // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
33 :
34 : AP_GROUPEND
35 : };
36 :
37 : // constructor. This incorporates initialisation as well.
38 0 : AP_BattMonitor_ESC::AP_BattMonitor_ESC(AP_BattMonitor &mon,
39 : AP_BattMonitor::BattMonitor_State &mon_state,
40 0 : AP_BattMonitor_Params ¶ms):
41 0 : AP_BattMonitor_Backend(mon, mon_state, params)
42 : {
43 0 : AP_Param::setup_object_defaults(this, var_info);
44 0 : _state.var_info = var_info;
45 0 : };
46 :
47 0 : void AP_BattMonitor_ESC::init(void)
48 : {
49 0 : }
50 :
51 0 : void AP_BattMonitor_ESC::read(void)
52 : {
53 0 : AP_ESC_Telem& telem = AP::esc_telem();
54 :
55 0 : uint8_t voltage_escs = 0; // number of ESCs with valid voltage
56 0 : uint8_t temperature_escs = 0; // number of ESCs with valid temperature
57 0 : float voltage_sum = 0;
58 0 : float current_sum = 0;
59 0 : float temperature_sum = 0;
60 0 : float consumed_mah_sum = 0.0;
61 0 : uint32_t highest_ms = 0;
62 :
63 0 : const bool all_enabled = _mask == 0;
64 0 : for (uint8_t i=0; i<ESC_TELEM_MAX_ESCS; i++) {
65 0 : if (!all_enabled && ((_mask & (1U<<i)) == 0)) {
66 : // Only include ESCs set in mask
67 0 : continue;
68 : }
69 :
70 : int16_t temperature_cdeg;
71 : float voltage;
72 : float current;
73 : float consumption_mah;
74 :
75 0 : if (telem.get_consumption_mah(i, consumption_mah)) {
76 : // accumulate consumed_sum regardless of age, to cope with ESC
77 : // dropping out
78 0 : consumed_mah_sum += consumption_mah;
79 0 : have_consumed_mah = true;
80 : }
81 :
82 0 : if (telem.get_voltage(i, voltage)) {
83 0 : voltage_sum += voltage;
84 0 : voltage_escs++;
85 : }
86 :
87 0 : if (telem.get_current(i, current)) {
88 0 : current_sum += current;
89 0 : have_current = true;
90 : }
91 :
92 0 : if (telem.get_temperature(i, temperature_cdeg)) {
93 0 : temperature_sum += float(temperature_cdeg) * 0.01f;
94 0 : temperature_escs++;
95 : }
96 :
97 0 : if (telem.get_last_telem_data_ms(i) > highest_ms) {
98 0 : highest_ms = telem.get_last_telem_data_ms(i);
99 : }
100 : }
101 :
102 0 : if (voltage_escs > 0) {
103 0 : _state.voltage = voltage_sum / voltage_escs;
104 0 : _state.healthy = true;
105 : } else {
106 0 : _state.voltage = 0;
107 0 : _state.healthy = false;
108 : }
109 0 : if (temperature_escs > 0) {
110 0 : _state.temperature = temperature_sum / temperature_escs;
111 0 : have_temperature = true;
112 : } else {
113 0 : _state.temperature = 0;
114 : }
115 :
116 0 : _state.current_amps = current_sum;
117 0 : _state.last_time_micros = highest_ms * 1000;
118 0 : _state.temperature_time = highest_ms;
119 :
120 0 : const uint32_t now_us = AP_HAL::micros();
121 0 : const uint32_t dt_us = now_us - last_read_us;
122 0 : last_read_us = now_us;
123 :
124 0 : if (have_consumed_mah) {
125 : // Report the cumulative consumed mah as reported by the ESCs
126 : // delta_mah allows reset_remaining to function without being able to reset the values sent by the ESCs
127 0 : _state.consumed_mah = delta_mah + consumed_mah_sum;
128 :
129 0 : } else if (have_current) {
130 : // ESCs provide current but not consumed mah, integrate manually
131 0 : update_consumed(_state, dt_us);
132 :
133 : }
134 0 : }
135 :
136 0 : bool AP_BattMonitor_ESC::reset_remaining(float percentage)
137 : {
138 0 : delta_mah = 0.0f;
139 0 : read();
140 0 : const float current_mah = _state.consumed_mah;
141 0 : if (AP_BattMonitor_Backend::reset_remaining(percentage)) {
142 0 : delta_mah = _state.consumed_mah - current_mah;
143 0 : return true;
144 : }
145 :
146 0 : return false;
147 : }
148 :
149 : #endif // AP_BATTERY_ESC_ENABLED
|
