From 9ca9ae5b24772794edb129627dd4bc1b413ef08c Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 29 Jul 2024 17:19:44 +0200 Subject: [PATCH] battery: fix initialization for internal resistance estimation Don't run/initialize if number of battery cells is zero and reinitialize whenever this parameter changes. --- src/lib/battery/battery.cpp | 34 +++++++++++++++++++++------------- src/lib/battery/battery.h | 1 + 2 files changed, 22 insertions(+), 13 deletions(-) diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 302cbca056..4d4715a0d5 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -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 ×tamp) { - 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; diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index 9d24b7bd44..04d0189602 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -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