fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

battery: cleanup setter functions and added hysteresis for connected state

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2024-09-24 10:01:21 +03:00
parent 3fdd054566
commit c4d2ec2654
15 changed files with 216 additions and 194 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
boards/bitcraze/crazyflie/syslink/syslink_main.cpp
View File

@ -402,11 +402,12 @@ Syslink::handle_message(syslink_message_t *msg)
float vbat; //, iset;
memcpy(&vbat, &msg->data[1], sizeof(float));
//memcpy(&iset, &msg->data[5], sizeof(float));
_battery.setConnected(true);
_battery.updateVoltage(vbat);
_battery.updateAndPublishBatteryStatus(t);
Battery::InputSample sample{
.timestamp = t,
.voltage_v = vbat,
};
_battery.updateAndPublishBatteryStatus(sample);
// Update battery charge state
if (charging) {

11
src/drivers/actuators/voxl_esc/voxl_esc.cpp
View File

@ -569,15 +569,16 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
// Limit the frequency of battery status reports
if (_parameters.publish_battery_status) {
_battery.setConnected(true);
_battery.updateVoltage(voltage);
_battery.updateCurrent(current);
hrt_abstime current_time = hrt_absolute_time();
if ((current_time - _last_battery_report_time) >= _battery_report_interval) {
_last_battery_report_time = current_time;
_battery.updateAndPublishBatteryStatus(current_time);
Battery::InputSample sample{
.timestamp = current_time,
.voltage_v = voltage,
.current_a = current
};
_battery.updateAndPublishBatteryStatus(sample);
}
}

30
src/drivers/power_monitor/ina220/ina220.cpp
View File

@ -87,10 +87,10 @@ INA220::INA220(const I2CSPIDriverConfig &config, int battery_index) :
if (_ch_type == PM_CH_TYPE_VBATT) {
// We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0
_battery.setConnected(false);
_battery.updateVoltage(0.f);
_battery.updateCurrent(0.f);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
Battery::InputSample invalid_sample {
.timestamp = hrt_absolute_time(),
};
_battery.updateAndPublishBatteryStatus(invalid_sample);
}
}
@ -246,10 +246,16 @@ INA220::collect()
switch (_ch_type) {
case PM_CH_TYPE_VBATT: {
_battery.setConnected(success);
_battery.updateVoltage(_voltage);
_battery.updateCurrent(_current);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
Battery::InputSample sample {
.timestamp = hrt_absolute_time(),
};
if (success) {
sample.voltage_v = _voltage;
sample.current_a = _current;
}
_battery.updateAndPublishBatteryStatus(sample);
}
break;
@ -328,10 +334,10 @@ INA220::RunImpl()
} else {
if (_ch_type == PM_CH_TYPE_VBATT) {
_battery.setConnected(false);
_battery.updateVoltage(0.f);
_battery.updateCurrent(0.f);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
Battery::InputSample invalid_sample {
.timestamp = hrt_absolute_time()
};
_battery.updateAndPublishBatteryStatus(invalid_sample);
}
if (init() != PX4_OK) {

42
src/drivers/power_monitor/ina226/ina226.cpp
View File

@ -83,10 +83,10 @@ INA226::INA226(const I2CSPIDriverConfig &config, int battery_index) :
_power_lsb = 25 * _current_lsb;
// We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0
_battery.setConnected(false);
_battery.updateVoltage(0.f);
_battery.updateCurrent(0.f);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
Battery::InputSample invalid_sample {
.timestamp = hrt_absolute_time()
};
_battery.updateAndPublishBatteryStatus(invalid_sample);
}
INA226::~INA226()
@ -222,28 +222,25 @@ INA226::collect()
// Note: If the power module is connected backwards, then the values of _power, _current, and _shunt will be negative but otherwise valid.
bool success{true};
success = success && (read(INA226_REG_BUSVOLTAGE, _bus_voltage) == PX4_OK);
// success = success && (read(INA226_REG_POWER, _power) == PX4_OK);
success = success && (read(INA226_REG_CURRENT, _current) == PX4_OK);
// success = success && (read(INA226_REG_SHUNTVOLTAGE, _shunt) == PX4_OK);
if (!success) {
Battery::InputSample sample{
.timestamp = hrt_absolute_time()
};
if (success) {
sample.voltage_v = _bus_voltage * INA226_VSCALE;
sample.current_a = _current * _current_lsb;
} else {
PX4_DEBUG("error reading from sensor");
_bus_voltage = _power = _current = _shunt = 0;
}
_battery.setConnected(success);
_battery.updateVoltage(static_cast<float>(_bus_voltage * INA226_VSCALE));
_battery.updateCurrent(static_cast<float>(_current * _current_lsb));
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
_battery.updateAndPublishBatteryStatus(sample);
perf_end(_sample_perf);
if (success) {
return PX4_OK;
} else {
return PX4_ERROR;
}
return success ? PX4_OK : PX4_ERROR;
}
void
@ -297,10 +294,11 @@ INA226::RunImpl()
ScheduleDelayed(INA226_CONVERSION_INTERVAL);
} else {
_battery.setConnected(false);
_battery.updateVoltage(0.f);
_battery.updateCurrent(0.f);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
Battery::InputSample invalid_sample {
.timestamp = hrt_absolute_time()
};
_battery.updateAndPublishBatteryStatus(invalid_sample);
if (init() != PX4_OK) {
ScheduleDelayed(INA226_INIT_RETRY_INTERVAL_US);

2
src/drivers/power_monitor/ina226/ina226.h
View File

@ -188,9 +188,7 @@ private:
perf_counter_t _measure_errors;
int16_t _bus_voltage{0};
int16_t _power{0};
int16_t _current{0};
int16_t _shunt{0};
int16_t _cal{0};
bool _mode_triggered{false};

46
src/drivers/power_monitor/ina228/ina228.cpp
View File

@ -85,11 +85,11 @@ INA228::INA228(const I2CSPIDriverConfig &config, int battery_index) :
_power_lsb = 3.2f * _current_lsb;
// We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0
_battery.setConnected(false);
_battery.updateVoltage(0.f);
_battery.updateCurrent(0.f);
_battery.updateTemperature(0.f);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
Battery::InputSample invalid_sample {
.timestamp = hrt_absolute_time()
};
_battery.updateAndPublishBatteryStatus(invalid_sample);
}
INA228::~INA228()
@ -309,25 +309,23 @@ INA228::collect()
//success = success && (read(INA228_REG_VSHUNT, _shunt) == PX4_OK);
success = success && (read(INA228_REG_DIETEMP, _temperature) == PX4_OK);
if (!success) {
PX4_DEBUG("error reading from sensor");
_bus_voltage = _power = _current = _shunt = 0;
}
_battery.setConnected(success);
_battery.updateVoltage(static_cast<float>(_bus_voltage * INA228_VSCALE));
_battery.updateCurrent(static_cast<float>(_current * _current_lsb));
_battery.updateTemperature(static_cast<float>(_temperature * INA228_TSCALE));
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
perf_end(_sample_perf);
Battery::InputSample sample{
.timestamp = hrt_absolute_time()
};
if (success) {
return PX4_OK;
sample.voltage_v = static_cast<float>(_bus_voltage * INA228_VSCALE);
sample.current_a = static_cast<float>(_current * _current_lsb);
sample.temperature_c = static_cast<float>(_temperature * INA228_TSCALE);
} else {
return PX4_ERROR;
PX4_DEBUG("error reading from sensor");
}
_battery.updateAndPublishBatteryStatus(sample);
perf_end(_sample_perf);
return success ? PX4_OK : PX4_ERROR;
}
void
@ -381,11 +379,11 @@ INA228::RunImpl()
ScheduleDelayed(INA228_CONVERSION_INTERVAL);
} else {
_battery.setConnected(false);
_battery.updateVoltage(0.f);
_battery.updateCurrent(0.f);
_battery.updateTemperature(0.f);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
Battery::InputSample invalid_sample {
.timestamp = hrt_absolute_time()
};
_battery.updateAndPublishBatteryStatus(invalid_sample);
if (init() != PX4_OK) {
ScheduleDelayed(INA228_INIT_RETRY_INTERVAL_US);

1
src/drivers/power_monitor/ina228/ina228.h
View File

@ -341,7 +341,6 @@ private:
int64_t _power{0};
int32_t _current{0};
int16_t _temperature{0};
int32_t _shunt{0};
int16_t _cal{0};
int16_t _range{INA228_ADCRANGE_HIGH};
bool _mode_triggered{false};

45
src/drivers/power_monitor/ina238/ina238.cpp
View File

@ -77,11 +77,11 @@ INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) :
_register_cfg[2].clear_bits = ~_shunt_calibration;
// We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0
_battery.setConnected(false);
_battery.updateVoltage(0.f);
_battery.updateCurrent(0.f);
_battery.updateTemperature(0.f);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
Battery::InputSample invalid_sample {
.timestamp = hrt_absolute_time()
};
_battery.updateAndPublishBatteryStatus(invalid_sample);
}
INA238::~INA238()
@ -245,25 +245,24 @@ int INA238::collect()
}
}
if (!success) {
Battery::InputSample sample{
.timestamp = hrt_absolute_time()
};
if (success) {
sample.voltage_v = bus_voltage * INA238_VSCALE;
sample.current_a = current * _current_lsb;
sample.temperature_c = temperature * INA238_TSCALE;
} else {
PX4_DEBUG("error reading from sensor");
bus_voltage = current = 0;
}
_battery.setConnected(success);
_battery.updateVoltage(static_cast<float>(bus_voltage * INA238_VSCALE));
_battery.updateCurrent(static_cast<float>(current * _current_lsb));
_battery.updateTemperature(static_cast<float>(temperature * INA238_TSCALE));
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
_battery.updateAndPublishBatteryStatus(sample);
perf_end(_sample_perf);
if (success) {
return PX4_OK;
} else {
return PX4_ERROR;
}
return success ? PX4_OK : PX4_ERROR;
}
void INA238::start()
@ -310,11 +309,11 @@ void INA238::RunImpl()
ScheduleDelayed(INA238_CONVERSION_INTERVAL);
} else {
_battery.setConnected(false);
_battery.updateVoltage(0.f);
_battery.updateCurrent(0.f);
_battery.updateTemperature(0.f);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
Battery::InputSample invalid_sample {
.timestamp = hrt_absolute_time()
};
_battery.updateAndPublishBatteryStatus(invalid_sample);
if (init() != PX4_OK) {
ScheduleDelayed(INA238_INIT_RETRY_INTERVAL_US);

29
src/drivers/power_monitor/voxlpm/voxlpm.cpp
View File

@ -70,12 +70,6 @@ VOXLPM::init()
_initialized = false;
int ret = PX4_ERROR;
if (_ch_type == VOXLPM_CH_TYPE_VBATT) {
_battery.setConnected(false);
_battery.updateVoltage(0.f);
_battery.updateCurrent(0.f);
}
/* do I2C init, it will probe the bus for two possible configurations, LTC2946 or INA231 */
if (I2C::init() != OK) {
return ret;
@ -83,7 +77,10 @@ VOXLPM::init()
// Don't actually publish anything unless we have had a successful probe
if (_ch_type == VOXLPM_CH_TYPE_VBATT) {
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
Battery::InputSample invalid_sample {
.timestamp = hrt_absolute_time()
};
_battery.updateAndPublishBatteryStatus(invalid_sample);
}
/* If we've probed and succeeded we'll have an accurate address here for the VBat addr */
@ -345,11 +342,13 @@ VOXLPM::measure()
if (ret == PX4_OK) {
switch (_ch_type) {
case VOXLPM_CH_TYPE_VBATT: {
Battery::InputSample sample {
.timestamp = tnow,
.voltage_v = _voltage,
.current_a = _amperage
};
_battery.setConnected(true);
_battery.updateVoltage(_voltage);
_battery.updateCurrent(_amperage);
_battery.updateAndPublishBatteryStatus(tnow);
_battery.updateAndPublishBatteryStatus(sample);
}
// fallthrough
@ -372,10 +371,10 @@ VOXLPM::measure()
switch (_ch_type) {
case VOXLPM_CH_TYPE_VBATT: {
_battery.setConnected(true);
_battery.updateVoltage(0.f);
_battery.updateCurrent(0.f);
_battery.updateAndPublishBatteryStatus(tnow);
Battery::InputSample invalid_sample {
.timestamp = tnow
};
_battery.updateAndPublishBatteryStatus(invalid_sample);
}
break;

10
src/drivers/uavcan/sensors/battery.cpp
View File

@ -230,10 +230,12 @@ void
UavcanBatteryBridge::filterData(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg,
uint8_t instance)
{
_battery[instance]->setConnected(true);
_battery[instance]->updateVoltage(msg.voltage);
_battery[instance]->updateCurrent(msg.current);
_battery[instance]->updateBatteryStatus(hrt_absolute_time());
Battery::InputSample sample {
.timestamp = hrt_absolute_time(),
.voltage_v = msg.voltage,
.current_a = msg.current
};
_battery[instance]->updateBatteryStatus(sample);
/* Override data that is expected to arrive from UAVCAN msg*/
_battery_status[instance] = _battery[instance]->getBatteryStatus();

98
src/lib/battery/battery.cpp
View File

@ -58,6 +58,8 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
_ocv_filter_v.setParameters(expected_filter_dt, 1.f);
_cell_voltage_filter_v.setParameters(expected_filter_dt, 1.f);
_connected_state_hysteresis.set_hysteresis_time_from(true, 2_s);
if (index > 9 || index < 1) {
PX4_ERR("Battery index must be between 1 and 9 (inclusive). Received %d. Defaulting to 1.", index);
}
@ -96,42 +98,37 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
updateParams();
}
void Battery::updateVoltage(const float voltage_v)
void Battery::updateBatteryStatus(const InputSample &sample)
{
_voltage_v = voltage_v;
}
_connected_state_hysteresis.set_state_and_update(sample.valid(), sample.timestamp);
void Battery::updateCurrent(const float current_a)
{
_current_a = current_a;
}
void Battery::updateTemperature(const float temperature_c)
{
_temperature_c = temperature_c;
}
void Battery::updateBatteryStatus(const hrt_abstime &timestamp)
{
// Require minimum voltage otherwise override connected status
if (_voltage_v < LITHIUM_BATTERY_RECOGNITION_VOLTAGE) {
_connected = false;
if (!sample.valid()) {
_last_valid_current_timestamp = 0;
return;
}
if (!_connected || (_last_unconnected_timestamp == 0)) {
_last_unconnected_timestamp = timestamp;
}
_last_valid_sample = sample;
// Wait with initializing filters to avoid relying on a voltage sample from the rising edge
_battery_initialized = _connected && (timestamp > _last_unconnected_timestamp + 2_s);
_battery_initialized = _connected_state_hysteresis.get_state()
&& (sample.timestamp - _connected_state_hysteresis.get_last_time_to_change_state()) > 2_s;
if (_connected && !_battery_initialized && _internal_resistance_initialized && _params.n_cells > 0) {
resetInternalResistanceEstimation(_voltage_v, _current_a);
if (sample.currentAndVoltageValid() && _connected_state_hysteresis.get_state() && !_battery_initialized
&& _internal_resistance_initialized
&& _params.n_cells > 0) {
resetInternalResistanceEstimation(sample.voltage_v, sample.current_a);
}
if (sample.currentValid()) {
sumDischarged(sample.timestamp, sample.current_a);
_last_valid_current_timestamp = sample.timestamp;
} else {
_last_valid_current_timestamp = 0;
}
sumDischarged(timestamp, _current_a);
_state_of_charge_volt_based =
calculateStateOfChargeVoltageBased(_voltage_v, _current_a);
calculateStateOfChargeVoltageBased(sample);
if (!_external_state_of_charge) {
estimateStateOfCharge();
@ -139,24 +136,25 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp)
computeScale();
if (_connected && _battery_initialized) {
if (_connected_state_hysteresis.get_state() && _battery_initialized) {
_warning = determineWarning(_state_of_charge);
}
}
battery_status_s Battery::getBatteryStatus()
{
battery_status_s battery_status{};
battery_status.voltage_v = _voltage_v;
battery_status.current_a = _current_a;
battery_status.voltage_v = _last_valid_sample.voltage_v;
battery_status.current_a = _last_valid_sample.current_a;
battery_status.current_average_a = _current_average_filter_a.getState();
battery_status.discharged_mah = _discharged_mah;
battery_status.remaining = _state_of_charge;
battery_status.scale = _scale;
battery_status.time_remaining_s = computeRemainingTime(_current_a);
battery_status.temperature = _temperature_c;
battery_status.time_remaining_s = computeRemainingTime(_last_valid_sample.current_a);
battery_status.temperature = _last_valid_sample.temperature_c;
battery_status.cell_count = _params.n_cells;
battery_status.connected = _connected;
battery_status.connected = _connected_state_hysteresis.get_state();
battery_status.source = _source;
battery_status.priority = _priority;
battery_status.capacity = _params.capacity > 0.f ? static_cast<uint16_t>(_params.capacity) : 0;
@ -165,7 +163,8 @@ battery_status_s Battery::getBatteryStatus()
battery_status.timestamp = hrt_absolute_time();
battery_status.faults = determineFaults();
battery_status.internal_resistance_estimate = _internal_resistance_estimate;
battery_status.ocv_estimate = _voltage_v + _internal_resistance_estimate * _params.n_cells * _current_a;
battery_status.ocv_estimate = _last_valid_sample.voltage_v + _internal_resistance_estimate * _params.n_cells *
_last_valid_sample.current_a;
battery_status.ocv_estimate_filtered = _ocv_filter_v.getState();
battery_status.volt_based_soc_estimate = _params.n_cells > 0 ? math::interpolate(_ocv_filter_v.getState() /
_params.n_cells,
@ -183,51 +182,42 @@ void Battery::publishBatteryStatus(const battery_status_s &battery_status)
}
}
void Battery::updateAndPublishBatteryStatus(const hrt_abstime &timestamp)
void Battery::updateAndPublishBatteryStatus(const InputSample &sample)
{
updateBatteryStatus(timestamp);
updateBatteryStatus(sample);
publishBatteryStatus(getBatteryStatus());
}
void Battery::sumDischarged(const hrt_abstime &timestamp, float current_a)
{
// Not a valid measurement
if (current_a < 0.f) {
// Because the measurement was invalid we need to stop integration
// and re-initialize with the next valid measurement
_last_timestamp = 0;
return;
}
// Ignore first update because we don't know dt.
if (_last_timestamp != 0) {
const float dt = (timestamp - _last_timestamp) / 1e6;
if (_last_valid_current_timestamp != 0) {
const float dt = (timestamp - _last_valid_current_timestamp) / 1e6;
// mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h])
_discharged_mah_loop = (current_a * 1e3f) * (dt / 3600.f);
_discharged_mah += _discharged_mah_loop;
}
_last_timestamp = timestamp;
}
float Battery::calculateStateOfChargeVoltageBased(const float voltage_v, const float current_a)
float Battery::calculateStateOfChargeVoltageBased(const InputSample &sample)
{
if (_params.n_cells == 0) {
if (_params.n_cells == 0 || !sample.voltageValid()) {
return -1.0f;
}
// remaining battery capacity based on voltage
float cell_voltage = voltage_v / _params.n_cells;
float cell_voltage = sample.voltage_v / _params.n_cells;
// correct battery voltage locally for load drop according to internal resistance and current
if (current_a > FLT_EPSILON) {
updateInternalResistanceEstimation(voltage_v, current_a);
if (sample.currentValid()) {
updateInternalResistanceEstimation(sample.voltage_v, sample.current_a);
if (_params.r_internal >= 0.f) { // Use user specified internal resistance value
cell_voltage += _params.r_internal * current_a;
cell_voltage += _params.r_internal * sample.current_a;
} else { // Use estimated internal resistance value
cell_voltage += _internal_resistance_estimate * current_a;
cell_voltage += _internal_resistance_estimate * sample.current_a;
}
}
@ -325,7 +315,7 @@ uint16_t Battery::determineFaults()
uint16_t faults{0};
if ((_params.n_cells > 0)
&& (_voltage_v > (_params.n_cells * _params.v_charged * 1.05f))) {
&& (_last_valid_sample.voltage_v > (_params.n_cells * _params.v_charged * 1.05f))) {
// Reported as a "spike" since "over-voltage" does not exist in MAV_BATTERY_FAULT
faults |= (1 << battery_status_s::BATTERY_FAULT_SPIKES);
}

52
src/lib/battery/battery.h
View File

@ -58,6 +58,7 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/flight_phase_estimation.h>
#include <uORB/topics/vehicle_status.h>
#include <lib/hysteresis/hysteresis.h>
/**
* BatteryBase is a base class for any type of battery.
@ -71,6 +72,32 @@ public:
Battery(int index, ModuleParams *parent, const int sample_interval_us, const uint8_t source);
~Battery() = default;
struct InputSample {
hrt_abstime timestamp{0};
float voltage_v{NAN};
float current_a{NAN};
float temperature_c{NAN};
bool valid() const
{
return timestamp > 0 && voltageValid();
}
bool voltageValid() const
{
return PX4_ISFINITE(voltage_v);
}
bool currentValid() const
{
return PX4_ISFINITE(current_a);
}
bool currentAndVoltageValid() const
{
return currentValid() && voltageValid();
}
};
/**
* Get the battery cell count
*/
@ -87,18 +114,12 @@ public:
float full_cell_voltage() { return _params.v_charged; }
void setPriority(const uint8_t priority) { _priority = priority; }
void setConnected(const bool connected) { _connected = connected; }
void setStateOfCharge(const float soc) { _state_of_charge = soc; _external_state_of_charge = true; }
void updateVoltage(const float voltage_v);
void updateCurrent(const float current_a);
void updateTemperature(const float temperature_c);
/**
* Update state of charge calculations
*
* @param timestamp Time at which the battery data sample was measured
* @param sample Input sample containing timestamp, voltage, current and temperature
*/
void updateBatteryStatus(const hrt_abstime &timestamp);
void updateBatteryStatus(const InputSample &sample);
battery_status_s getBatteryStatus();
void publishBatteryStatus(const battery_status_s &battery_status);
@ -108,7 +129,9 @@ public:
* @see updateBatteryStatus()
* @see publishBatteryStatus()
*/
void updateAndPublishBatteryStatus(const hrt_abstime &timestamp);
void updateAndPublishBatteryStatus(const InputSample &sample);
protected:
static constexpr float LITHIUM_BATTERY_RECOGNITION_VOLTAGE = 2.1f;
@ -146,7 +169,7 @@ protected:
private:
void sumDischarged(const hrt_abstime &timestamp, float current_a);
float calculateStateOfChargeVoltageBased(const float voltage_v, const float current_a);
float calculateStateOfChargeVoltageBased(const InputSample &sample);
void estimateStateOfCharge();
uint8_t determineWarning(float state_of_charge);
uint16_t determineFaults();
@ -159,27 +182,24 @@ private:
bool _external_state_of_charge{false}; ///< inticates that the soc is injected and not updated by this library
bool _connected{false};
InputSample _last_valid_sample {};
const uint8_t _source;
uint8_t _priority{0};
bool _battery_initialized{false};
float _voltage_v{0.f};
AlphaFilter<float> _ocv_filter_v;
AlphaFilter<float> _cell_voltage_filter_v;
float _current_a{-1};
AlphaFilter<float>
_current_average_filter_a; ///< averaging filter for current. For FW, it is the current in level flight.
float _temperature_c{NAN};
float _discharged_mah{0.f};
float _discharged_mah_loop{0.f};
float _state_of_charge_volt_based{-1.f}; // [0,1]
float _state_of_charge{-1.f}; // [0,1]
float _scale{1.f};
uint8_t _warning{battery_status_s::BATTERY_WARNING_NONE};
hrt_abstime _last_timestamp{0};
hrt_abstime _last_valid_current_timestamp{0};
bool _armed{false};
bool _vehicle_status_is_fw{false};
hrt_abstime _last_unconnected_timestamp{0};
systemlib::Hysteresis _connected_state_hysteresis{false};
// Internal Resistance estimation
void updateInternalResistanceEstimation(const float voltage_v, const float current_a);

14
src/modules/battery_status/analog_battery.cpp
View File

@ -79,10 +79,16 @@ AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw,
const bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V &&
(BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid());
Battery::setConnected(connected);
Battery::updateVoltage(voltage_v);
Battery::updateCurrent(current_a);
Battery::updateAndPublishBatteryStatus(timestamp);
Battery::InputSample sample{
.timestamp = timestamp,
};
if (connected) {
sample.voltage_v = voltage_v;
sample.current_a = current_a;
}
Battery::updateAndPublishBatteryStatus(sample);
}
bool AnalogBattery::is_valid()

11
src/modules/esc_battery/EscBattery.cpp
View File

@ -101,10 +101,13 @@ EscBattery::Run()
average_voltage_v /= online_esc_count;
_battery.setConnected(true);
_battery.updateVoltage(average_voltage_v);
_battery.updateCurrent(total_current_a);
_battery.updateAndPublishBatteryStatus(esc_status.timestamp);
Battery::InputSample sample {
.timestamp = esc_status.timestamp,
.voltage_v = average_voltage_v,
.current_a = total_current_a
};
_battery.updateAndPublishBatteryStatus(sample);
}
}

10
src/modules/simulation/battery_simulator/BatterySimulator.cpp
View File

@ -108,10 +108,12 @@ void BatterySimulator::Run()
vbatt *= _battery.cell_count();
_battery.setConnected(true);
_battery.updateVoltage(vbatt);
_battery.updateCurrent(ibatt);
_battery.updateAndPublishBatteryStatus(now_us);
Battery::InputSample sample{
.timestamp = hrt_absolute_time(),
.voltage_v = vbatt,
.current_a = ibatt
};
_battery.updateAndPublishBatteryStatus(sample);
perf_end(_loop_perf);
}