mirror of https://github.com/ArduPilot/ardupilot
95 lines
2.9 KiB
C++
95 lines
2.9 KiB
C++
#include <AP_HAL/AP_HAL.h>
|
|
#include <AP_Common/AP_Common.h>
|
|
#include <AP_Math/AP_Math.h>
|
|
#include "AP_BattMonitor.h"
|
|
#include "AP_BattMonitor_Sum.h"
|
|
|
|
/*
|
|
battery monitor that is the sum of other battery monitors after this one
|
|
|
|
This can be used to combined other current/voltage sensors into a
|
|
single backend
|
|
*/
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
const AP_Param::GroupInfo AP_BattMonitor_Sum::var_info[] = {
|
|
|
|
// Param indexes must be between 20 and 29 to avoid conflict with other battery monitor param tables loaded by pointer
|
|
|
|
// @Param: SUM_MASK
|
|
// @DisplayName: Battery Sum mask
|
|
// @Description: 0: sum of remaining battery monitors, If none 0 sum of specified monitors. Current will be summed and voltages averaged.
|
|
// @Bitmask: 0:monitor 1, 1:monitor 2, 2:monitor 3, 3:monitor 4, 4:monitor 5, 5:monitor 6, 6:monitor 7, 7:monitor 8, 8:monitor 9
|
|
// @User: Standard
|
|
AP_GROUPINFO("SUM_MASK", 20, AP_BattMonitor_Sum, _sum_mask, 0),
|
|
|
|
// Param indexes must be between 20 and 29 to avoid conflict with other battery monitor param tables loaded by pointer
|
|
|
|
AP_GROUPEND
|
|
};
|
|
|
|
/// Constructor
|
|
AP_BattMonitor_Sum::AP_BattMonitor_Sum(AP_BattMonitor &mon,
|
|
AP_BattMonitor::BattMonitor_State &mon_state,
|
|
AP_BattMonitor_Params ¶ms,
|
|
uint8_t instance) :
|
|
AP_BattMonitor_Backend(mon, mon_state, params),
|
|
_instance(instance)
|
|
{
|
|
AP_Param::setup_object_defaults(this, var_info);
|
|
_state.var_info = var_info;
|
|
}
|
|
|
|
// read - read the voltage and current
|
|
void
|
|
AP_BattMonitor_Sum::read()
|
|
{
|
|
float voltage_sum = 0;
|
|
uint8_t voltage_count = 0;
|
|
float current_sum = 0;
|
|
uint8_t current_count = 0;
|
|
|
|
for (uint8_t i=0; i<_mon.num_instances(); i++) {
|
|
if (i == _instance) {
|
|
// never include self
|
|
continue;
|
|
}
|
|
if ((_sum_mask == 0) && (i <= _instance)) {
|
|
// sum of remaining, skip lower instances
|
|
continue;
|
|
}
|
|
if ((_sum_mask != 0) && ((_sum_mask & 1U<<i) == 0)) {
|
|
// mask param, skip if mask bit not set
|
|
continue;
|
|
}
|
|
if (!_mon.healthy(i)) {
|
|
continue;
|
|
}
|
|
voltage_sum += _mon.voltage(i);
|
|
voltage_count++;
|
|
float current;
|
|
if (_mon.current_amps(current, i)) {
|
|
current_sum += current;
|
|
current_count++;
|
|
}
|
|
}
|
|
const uint32_t tnow_us = AP_HAL::micros();
|
|
const uint32_t dt_us = tnow_us - _state.last_time_micros;
|
|
|
|
if (voltage_count > 0) {
|
|
_state.voltage = voltage_sum / voltage_count;
|
|
}
|
|
if (current_count > 0) {
|
|
_state.current_amps = current_sum;
|
|
}
|
|
|
|
update_consumed(_state, dt_us);
|
|
|
|
_has_current = (current_count > 0);
|
|
_state.healthy = (voltage_count > 0);
|
|
|
|
if (_state.healthy) {
|
|
_state.last_time_micros = tnow_us;
|
|
}
|
|
}
|