diff --git a/src/modules/battery_status/analog_battery.cpp b/src/modules/battery_status/analog_battery.cpp index e7ca828ce0..d46e5de290 100644 --- a/src/modules/battery_status/analog_battery.cpp +++ b/src/modules/battery_status/analog_battery.cpp @@ -68,16 +68,27 @@ AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_i snprintf(param_name, sizeof(param_name), "BAT%d_I_CHANNEL", index); _analog_param_handles.i_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); } void AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw) { const float voltage_v = voltage_raw * _analog_params.v_div; - const float current_a = (current_raw - _analog_params.v_offs_cur) * _analog_params.a_per_v; - const bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V && (BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid()); + float current_a = (current_raw - _analog_params.v_offs_cur) * _analog_params.a_per_v; + + // 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); @@ -123,7 +134,17 @@ AnalogBattery::updateParams() 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_channel, &_analog_params.i_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); 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 ce2e677d74..b532f54d0d 100644 --- a/src/modules/battery_status/analog_battery.h +++ b/src/modules/battery_status/analog_battery.h @@ -35,6 +35,7 @@ #include #include +#include class AnalogBattery : public Battery { @@ -77,6 +78,7 @@ protected: param_t a_per_v; param_t v_channel; param_t i_channel; + param_t i_overwrite; } _analog_param_handles; struct { @@ -85,7 +87,14 @@ protected: float a_per_v; int32_t v_channel; int32_t i_channel; + float i_overwrite; } _analog_params; virtual void updateParams() override; + +private: + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uint8_t _arming_state{0}; + + void updateTopics(); }; diff --git a/src/modules/battery_status/module.yaml b/src/modules/battery_status/module.yaml index 85b7bb2ecc..70ce948f89 100644 --- a/src/modules/battery_status/module.yaml +++ b/src/modules/battery_status/module.yaml @@ -61,3 +61,22 @@ parameters: num_instances: *max_num_config_instances 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]