diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 5fbcc6ac30..121a4fb4db 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -87,6 +87,9 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us, snprintf(param_name, sizeof(param_name), "BAT%d_SOURCE", _index); _param_handles.source = param_find(param_name); + snprintf(param_name, sizeof(param_name), "BAT%d_I_OVERWRITE", _index); + _param_handles.i_overwrite = param_find(param_name); + _param_handles.low_thr = param_find("BAT_LOW_THR"); _param_handles.crit_thr = param_find("BAT_CRIT_THR"); _param_handles.emergen_thr = param_find("BAT_EMERGEN_THR"); @@ -103,7 +106,13 @@ void Battery::updateVoltage(const float voltage_v) void Battery::updateCurrent(const float current_a) { - _current_a = current_a; + // Overwrite the measured current if current overwrite is defined and vehicle is unarmed + if (!_armed && _params.i_overwrite > FLT_EPSILON) { + _current_a = _params.i_overwrite; + + } else { + _current_a = current_a; + } } void Battery::updateTemperature(const float temperature_c) @@ -144,6 +153,14 @@ void Battery::updateBatteryStatus(const hrt_abstime ×tamp) if (_connected && _battery_initialized) { _warning = determineWarning(_state_of_charge); } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + } + } } battery_status_s Battery::getBatteryStatus() @@ -359,7 +376,6 @@ float Battery::computeRemainingTime(float current_a) vehicle_status_s vehicle_status; if (_vehicle_status_sub.copy(&vehicle_status)) { - _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_vehicle_status_is_fw) { reset_current_avg_filter = true; @@ -412,6 +428,7 @@ void Battery::updateParams() param_get(_param_handles.crit_thr, &_params.crit_thr); param_get(_param_handles.emergen_thr, &_params.emergen_thr); param_get(_param_handles.bat_avrg_current, &_params.bat_avrg_current); + param_get(_param_handles.i_overwrite, &_params.i_overwrite); float capacity{0.f}; param_get(_param_handles.capacity, &capacity); diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index b432c80e27..7699ffab3e 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -147,6 +147,7 @@ protected: param_t emergen_thr; param_t source; param_t bat_avrg_current; + param_t i_overwrite; } _param_handles{}; struct { @@ -159,6 +160,7 @@ protected: float emergen_thr; int32_t source; float bat_avrg_current; + float i_overwrite; } _params{}; const int _index; diff --git a/src/lib/battery/module.yaml b/src/lib/battery/module.yaml index adaaf72395..68e5d12a79 100644 --- a/src/lib/battery/module.yaml +++ b/src/lib/battery/module.yaml @@ -126,6 +126,23 @@ parameters: instance_start: 1 default: [0, -1, -1] + BAT${i}_I_OVERWRITE: + description: + short: Battery ${i} idle current overwrite + long: | + This parameter allows to overwrite the current measured during + idle (unarmed) state with a user-defined constant value (expressed in amperes). + When the system is armed, the measured current is used. This is useful + because on certain ESCs current measurements are inaccurate in case of no load. + Negative values are ignored and will cause the measured current to be used. + The default value of 0 disables the overwrite, in which case the measured value + is always used. + type: float + decimal: 2 + num_instances: *max_num_config_instances + instance_start: 1 + default: [0, 0, 0] + BAT_LOW_THR: description: short: Low threshold. diff --git a/src/modules/battery_status/analog_battery.cpp b/src/modules/battery_status/analog_battery.cpp index 7f52d7911f..68a9a4ad13 100644 --- a/src/modules/battery_status/analog_battery.cpp +++ b/src/modules/battery_status/analog_battery.cpp @@ -67,9 +67,6 @@ AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_i snprintf(param_name, sizeof(param_name), "BAT%d_V_CHANNEL", index); _analog_param_handles.v_channel = param_find(param_name); - snprintf(param_name, sizeof(param_name), "BAT%d_I_OVERWRITE", index); - _analog_param_handles.i_overwrite = param_find(param_name); - snprintf(param_name, sizeof(param_name), "BAT%d_V_FILT", index); _analog_param_handles.v_filt = param_find(param_name); @@ -102,15 +99,6 @@ AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, } } - // Overwrite the measured current if current overwrite is defined and vehicle is unarmed - if (_analog_params.i_overwrite > 0) { - updateTopics(); - - if (_arming_state == vehicle_status_s::ARMING_STATE_DISARMED) { - current_a = _analog_params.i_overwrite; - } - } - Battery::setConnected(connected); Battery::updateVoltage(voltage_v); Battery::updateCurrent(current_a); @@ -153,7 +141,6 @@ AnalogBattery::updateParams() param_get(_analog_param_handles.v_div, &_analog_params.v_div); param_get(_analog_param_handles.a_per_v, &_analog_params.a_per_v); param_get(_analog_param_handles.v_channel, &_analog_params.v_channel); - param_get(_analog_param_handles.i_overwrite, &_analog_params.i_overwrite); param_get(_analog_param_handles.v_offs_cur, &_analog_params.v_offs_cur); param_get(_analog_param_handles.v_filt, &_analog_params.v_filt); param_get(_analog_param_handles.i_filt, &_analog_params.i_filt); @@ -168,12 +155,3 @@ AnalogBattery::updateParams() Battery::updateParams(); } - -void AnalogBattery::updateTopics() -{ - vehicle_status_s vehicle_status; - - if (_vehicle_status_sub.update(&vehicle_status)) { - _arming_state = vehicle_status.arming_state; - } -} diff --git a/src/modules/battery_status/analog_battery.h b/src/modules/battery_status/analog_battery.h index 9e91fba4ef..8659adac05 100644 --- a/src/modules/battery_status/analog_battery.h +++ b/src/modules/battery_status/analog_battery.h @@ -36,8 +36,6 @@ #include #include #include -#include - class AnalogBattery : public Battery { public: @@ -78,7 +76,6 @@ protected: param_t v_div; param_t a_per_v; param_t v_channel; - param_t i_overwrite; param_t v_filt; param_t i_filt; } _analog_param_handles; @@ -88,7 +85,6 @@ protected: float v_div; float a_per_v; int32_t v_channel; - float i_overwrite; float v_filt; float i_filt; } _analog_params; @@ -99,12 +95,7 @@ private: static constexpr int V_CHANNEL_DISABLED = -2; - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uint8_t _arming_state{0}; - hrt_abstime _last_timestamp{0}; AlphaFilter _voltage_filter{}; AlphaFilter _current_filter{}; - - void updateTopics(); }; diff --git a/src/modules/battery_status/module.yaml b/src/modules/battery_status/module.yaml index 11f8a633a8..a908a87ff0 100644 --- a/src/modules/battery_status/module.yaml +++ b/src/modules/battery_status/module.yaml @@ -51,25 +51,6 @@ parameters: instance_start: 1 default: [-1, -1] - BAT${i}_I_OVERWRITE: - description: - short: Battery ${i} idle current overwrite - long: | - This parameter allows to overwrite the current measured during - idle (unarmed) state with a user-defined constant value (expressed in amperes). - When the system is armed, the measured current is used. This is useful - because on certain ESCs current measurements are inaccurate in case of no load. - Negative values are ignored and will cause the measured current to be used. - The default value of 0 disables the overwrite, in which case the measured value - is always used. - - type: float - decimal: 8 - reboot_required: true - num_instances: *max_num_config_instances - instance_start: 1 - default: [0, 0] - BAT${i}_V_FILT: description: short: Battery ${i} voltage filter time constant