fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 20:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

battery: fix initialization for internal resistance estimation

Browse Source 
Don't run/initialize if number of battery cells is zero and reinitialize whenever this parameter changes.
This commit is contained in:
chfriedrich98
2024-07-30 16:18:47 +02:00
committed by chfriedrich98
parent ba579245fb
commit 9ca9ae5b24
2 changed files with 22 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/battery/battery.cpp
+21 -13
View File
@@ -94,16 +94,6 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
_param_handles.bat_avrg_current = param_find("BAT_AVRG_CURRENT");
updateParams();
// Internal resistance estimation initializations
_RLS_est(0) = OCV_DEFAULT * _params.n_cells;
_RLS_est(1) = R_DEFAULT * _params.n_cells;
_estimation_covariance(0, 0) = OCV_COVARIANCE * _params.n_cells;
_estimation_covariance(0, 1) = 0.f;
_estimation_covariance(1, 0) = 0.f;
_estimation_covariance(1, 1) = R_COVARIANCE * _params.n_cells;
_estimation_covariance_norm = sqrtf(powf(_estimation_covariance(0, 0), 2.f) + 2.f * powf(_estimation_covariance(1, 0),
2.f) + powf(_estimation_covariance(1, 1), 2.f));
}
void Battery::updateVoltage(const float voltage_v)
@@ -118,7 +108,7 @@ void Battery::updateCurrent(const float current_a)
void Battery::updateBatteryStatus(const hrt_abstime &timestamp)
{
if (!_battery_initialized) {
if (!_battery_initialized && _internal_resistance_initialized && _params.n_cells > 0) {
resetInternalResistanceEstimation(_voltage_v, _current_a);
}
@@ -172,8 +162,9 @@ battery_status_s Battery::getBatteryStatus()
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_filtered = _ocv_filter_v.getState();
battery_status.volt_based_soc_estimate = math::interpolate(_ocv_filter_v.getState() / _params.n_cells,
_params.v_empty, _params.v_charged, 0.f, 1.f);
battery_status.volt_based_soc_estimate = _params.n_cells > 0 ? math::interpolate(_ocv_filter_v.getState() /
_params.n_cells,
_params.v_empty, _params.v_charged, 0.f, 1.f) : -1.f;
battery_status.voltage_prediction = _voltage_prediction;
battery_status.prediction_error = _prediction_error;
battery_status.estimation_covariance_norm = _estimation_covariance_norm;
@@ -397,6 +388,7 @@ float Battery::computeRemainingTime(float current_a)
void Battery::updateParams()
{
const int n_cells = _first_parameter_update ? 0 : _params.n_cells;
param_get(_param_handles.v_empty, &_params.v_empty);
param_get(_param_handles.v_charged, &_params.v_charged);
param_get(_param_handles.n_cells, &_params.n_cells);
@@ -408,6 +400,22 @@ void Battery::updateParams()
param_get(_param_handles.emergen_thr, &_params.emergen_thr);
param_get(_param_handles.bat_avrg_current, &_params.bat_avrg_current);
if (n_cells != _params.n_cells) {
_internal_resistance_initialized = false;
}
if (!_internal_resistance_initialized && _params.n_cells > 0) {
_RLS_est(0) = OCV_DEFAULT * _params.n_cells;
_RLS_est(1) = R_DEFAULT * _params.n_cells;
_estimation_covariance(0, 0) = OCV_COVARIANCE * _params.n_cells;
_estimation_covariance(0, 1) = 0.f;
_estimation_covariance(1, 0) = 0.f;
_estimation_covariance(1, 1) = R_COVARIANCE * _params.n_cells;
_estimation_covariance_norm = sqrtf(powf(_estimation_covariance(0, 0), 2.f) + 2.f * powf(_estimation_covariance(1, 0),
2.f) + powf(_estimation_covariance(1, 1), 2.f));
_internal_resistance_initialized = true;
}
ModuleParams::updateParams();
_first_parameter_update = false;
src/lib/battery/battery.h
+1
View File
@@ -184,6 +184,7 @@ private:
void resetInternalResistanceEstimation(const float voltage_v, const float current_a);
matrix::Vector2f _RLS_est; // [Open circuit voltage estimate [V], Total internal resistance estimate [Ohm]]^T
matrix::Matrix2f _estimation_covariance;
bool _internal_resistance_initialized{false};
float _estimation_covariance_norm{0.f};
float _internal_resistance_estimate{0.005f}; // [Ohm] Per cell estimate of the internal resistance
float _voltage_prediction{0.f}; // [V] Predicted voltage of the estimator