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

Battery add configurable filter params (#26524)

Browse Source 
* ark fpv board_config: update battery ADC current filter time constant to 0.5s

* battery: add configurable voltage and current filter time constants

---------

Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>
This commit is contained in:
Alex Klimaj 2026-02-18 20:20:50 -07:00 committed by GitHub
parent 64e996b475
commit b8877c4cfc
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
5 changed files with 73 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
boards/ark/fpv/init/rc.board_defaults
View File

@ -26,6 +26,8 @@ then
fi
param set-default BAT1_V_DIV 21.0
param set-default BAT1_V_FILT 0.075
param set-default BAT1_I_FILT 0.5
param set-default RC_CRSF_PRT_CFG 300
param set-default RC_SBUS_PRT_CFG 0

3
boards/ark/fpv/src/board_config.h
View File

@ -176,9 +176,6 @@
#define BOARD_BATTERY1_V_DIV (21.0f) // (20k + 1k) / 1k = 21
#define BOARD_BATTERY_ADC_VOLTAGE_FILTER_S 0.075f
#define BOARD_BATTERY_ADC_CURRENT_FILTER_S 0.125f
#define ADC_SCALED_PAYLOAD_SENSE ADC_SCALED_12V_CHANNEL
/* HW has to large of R termination on ADC todo:change when HW value is chosen */

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

@ -32,6 +32,7 @@
****************************************************************************/
#include <stdio.h>
#include <float.h>
#include <lib/battery/battery.h>
#include "analog_battery.h"
@ -71,6 +72,12 @@ AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_i
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);
snprintf(param_name, sizeof(param_name), "BAT%d_I_FILT", index);
_analog_param_handles.i_filt = param_find(param_name);
}
void
@ -81,24 +88,23 @@ AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw,
(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;
#if defined(BOARD_BATTERY_ADC_VOLTAGE_FILTER_S) || defined(BOARD_BATTERY_ADC_CURRENT_FILTER_S)
if (_analog_params.v_filt > FLT_EPSILON || _analog_params.i_filt > FLT_EPSILON) {
if (_last_timestamp == 0) {
_last_timestamp = timestamp;
}
if (_last_timestamp == 0) {
const float dt = (timestamp - _last_timestamp) / 1e6f;
_last_timestamp = timestamp;
if (_analog_params.v_filt > FLT_EPSILON) {
voltage_v = _voltage_filter.update(fmaxf(voltage_v, 0.f), dt);
}
if (_analog_params.i_filt > FLT_EPSILON) {
current_a = _current_filter.update(fmaxf(current_a, 0.f), dt);
}
}
const float dt = (timestamp - _last_timestamp) / 1e6f;
_last_timestamp = timestamp;
#endif
#ifdef BOARD_BATTERY_ADC_VOLTAGE_FILTER_S
voltage_v = _voltage_filter.update(fmaxf(voltage_v, 0.f), dt);
#endif
#ifdef BOARD_BATTERY_ADC_CURRENT_FILTER_S
current_a = _current_filter.update(fmaxf(current_a, 0.f), dt);
#endif
// Overwrite the measured current if current overwrite is defined and vehicle is unarmed
if (_analog_params.i_overwrite > 0) {
updateTopics();
@ -158,6 +164,16 @@ AnalogBattery::updateParams()
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);
param_get(_analog_param_handles.v_filt, &_analog_params.v_filt);
param_get(_analog_param_handles.i_filt, &_analog_params.i_filt);
if (_analog_params.v_filt > FLT_EPSILON) {
_voltage_filter = AlphaFilter<float>(_analog_params.v_filt);
}
if (_analog_params.i_filt > FLT_EPSILON) {
_current_filter = AlphaFilter<float>(_analog_params.i_filt);
}
Battery::updateParams();
}

18
src/modules/battery_status/analog_battery.h
View File

@ -80,6 +80,8 @@ protected:
param_t v_channel;
param_t i_channel;
param_t i_overwrite;
param_t v_filt;
param_t i_filt;
} _analog_param_handles;
struct {
@ -89,6 +91,8 @@ protected:
int32_t v_channel;
int32_t i_channel;
float i_overwrite;
float v_filt;
float i_filt;
} _analog_params;
virtual void updateParams() override;
@ -100,17 +104,9 @@ private:
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uint8_t _arming_state{0};
#if defined(BOARD_BATTERY_ADC_VOLTAGE_FILTER_S) || defined(BOARD_BATTERY_ADC_CURRENT_FILTER_S)
hrt_abstime _last_timestamp {0};
#endif
#ifdef BOARD_BATTERY_ADC_VOLTAGE_FILTER_S
AlphaFilter<float> _voltage_filter {BOARD_BATTERY_ADC_VOLTAGE_FILTER_S};
#endif
#ifdef BOARD_BATTERY_ADC_CURRENT_FILTER_S
AlphaFilter<float> _current_filter {BOARD_BATTERY_ADC_CURRENT_FILTER_S};
#endif
hrt_abstime _last_timestamp{0};
AlphaFilter<float> _voltage_filter{};
AlphaFilter<float> _current_filter{};
void updateTopics();
};

34
src/modules/battery_status/module.yaml
View File

@ -82,3 +82,37 @@ parameters:
num_instances: *max_num_config_instances
instance_start: 1
default: [0, 0]
BAT${i}_V_FILT:
description:
short: Battery ${i} voltage filter time constant
long: |
Low-pass filter time constant for the battery voltage ADC reading (in seconds).
A higher value results in more smoothing and less noise, but slower response.
A value of 0 disables the filter.
type: float
decimal: 3
min: 0.0
max: 5.0
unit: s
num_instances: *max_num_config_instances
instance_start: 1
default: [0.0, 0.0]
BAT${i}_I_FILT:
description:
short: Battery ${i} current filter time constant
long: |
Low-pass filter time constant for the battery current ADC reading (in seconds).
A higher value results in more smoothing and less noise, but slower response.
A value of 0 disables the filter.
type: float
decimal: 3
min: 0.0
max: 5.0
unit: s
num_instances: *max_num_config_instances
instance_start: 1
default: [0.0, 0.0]