/* 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/>. */ #include "AP_BattMonitor_config.h" #if AP_BATTERY_ESC_ENABLED #include "AP_BattMonitor_ESC.h" void AP_BattMonitor_ESC::init(void) { } void AP_BattMonitor_ESC::read(void) { AP_ESC_Telem& telem = AP::esc_telem(); uint8_t voltage_escs = 0; // number of ESCs with valid voltage uint8_t temperature_escs = 0; // number of ESCs with valid temperature float voltage_sum = 0; float current_sum = 0; float temperature_sum = 0; uint32_t highest_ms = 0; _state.consumed_mah = delta_mah; for (uint8_t i=0; i<ESC_TELEM_MAX_ESCS; i++) { int16_t temperature_cdeg; float voltage; float current; float consumption_mah; if (telem.get_consumption_mah(i, consumption_mah)) { // accumulate consumed_sum regardless of age, to cope with ESC // dropping out _state.consumed_mah += consumption_mah; } if (telem.get_voltage(i, voltage)) { voltage_sum += voltage; voltage_escs++; } if (telem.get_current(i, current)) { current_sum += current; } if (telem.get_temperature(i, temperature_cdeg)) { temperature_sum += float(temperature_cdeg) * 0.01f; temperature_escs++; } if (telem.get_last_telem_data_ms(i) > highest_ms) { highest_ms = telem.get_last_telem_data_ms(i); } } if (voltage_escs > 0) { _state.voltage = voltage_sum / voltage_escs; _state.healthy = true; } else { _state.voltage = 0; _state.healthy = false; } if (temperature_escs > 0) { _state.temperature = temperature_sum / temperature_escs; have_temperature = true; } else { _state.temperature = 0; } _state.current_amps = current_sum; _state.last_time_micros = highest_ms * 1000; _state.temperature_time = highest_ms; if (current_sum > 0) { // if we have ever got a current value then we know we have a // current sensor have_current = true; } } bool AP_BattMonitor_ESC::reset_remaining(float percentage) { delta_mah = 0.0f; read(); const float current_mah = _state.consumed_mah; if (AP_BattMonitor_Backend::reset_remaining(percentage)) { delta_mah = _state.consumed_mah - current_mah; return true; } return false; } #endif // AP_BATTERY_ESC_ENABLED