UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-09-25 12:09:56 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat(battery_simulator): simulate up to 3 battery instances (#28385)

Browse Source 
* feat(battery_simulator): simulate all battery instances with per-battery drain

Simulate one battery per battery_status instance instead of a single
hardcoded one. The Battery array is constructed in place from
battery_status_s::MAX_INSTANCES, so the simulator follows the maximum
number of batteries without code changes. A battery is only published
when its BAT<N>_SOURCE is set to power module, which keeps the default
SITL behavior of a single battery unchanged.

Add SIM_BAT<N>_DRAIN and SIM_BAT<N>_MIN_PCT to override the shared
SIM_BAT_DRAIN and SIM_BAT_MIN_PCT per battery (-1 inherits the shared
value), and integrate the state of charge per battery. This allows
testing multi-battery behavior such as one battery draining faster
than the others.

* docs(simulation): describe simulating multiple batteries

Document how additional simulated batteries are enabled through
BAT<N>_SOURCE and how SIM_BAT<N>_DRAIN and SIM_BAT<N>_MIN_PCT override
the shared drain parameters per battery.

* docs(simulation): clarify per-battery fallback rules
This commit is contained in:
Anil Kircaliali
2026-09-01 00:09:37 -07:00
committed by GitHub
parent f0bddfa4db
commit ed772c0c3d
4 changed files with 150 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
docs/en/simulation/failsafes.md
View File

@@ -49,6 +49,17 @@ By changing [SIM_BAT_MIN_PCT](../advanced_config/parameter_reference.md#SIM_BAT_
The simulated battery can be completely disabled by setting [SIM_BAT_DRAIN](../advanced_config/parameter_reference.md#SIM_BAT_DRAIN) to 0. This is useful, for example, if you provide an external battery simulation via MAVLink.
### Multiple Batteries
By default only one battery is simulated.
Additional batteries are simulated by enabling them like on a real vehicle: set the source of the battery to `Power Module` and configure it, e.g. [BAT2_SOURCE](../advanced_config/parameter_reference.md#BAT2_SOURCE) and [BAT2_N_CELLS](../advanced_config/parameter_reference.md#BAT2_N_CELLS) for a second battery.
Each simulated battery is published as its own `battery_status` instance.
All batteries deplete according to [SIM_BAT_DRAIN](../advanced_config/parameter_reference.md#SIM_BAT_DRAIN) and [SIM_BAT_MIN_PCT](../advanced_config/parameter_reference.md#SIM_BAT_MIN_PCT) unless overridden for an individual battery with [SIM_BATx_DRAIN](../advanced_config/parameter_reference.md#SIM_BAT1_DRAIN) and [SIM_BATx_MIN_PCT](../advanced_config/parameter_reference.md#SIM_BAT1_MIN_PCT).
Setting `SIM_BATx_DRAIN` to a non-positive value uses the shared drain time. Setting `SIM_BATx_MIN_PCT` to a negative value
uses the shared minimum charge.
This can be used to test multi-battery behaviour, for example one battery depleting faster than the others.
## Sensor/System Failure
[Failure injection](../debug/failure_injection.md) can be used to simulate different types of failures in many sensors and systems.

114
src/modules/simulation/battery_simulator/BatterySimulator.cpp
View File

@@ -38,18 +38,54 @@ ModuleBase::Descriptor BatterySimulator::desc{task_spawn, custom_command, print_
BatterySimulator::BatterySimulator() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
_battery(1, this, BATTERY_SIMLATOR_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE)
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
{
for (int i = 0; i < battery_status_s::MAX_INSTANCES; i++) {
_batteries[i] = new Battery(i + 1, this, BATTERY_SIMLATOR_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE);
if (_batteries[i] == nullptr) {
PX4_ERR("battery %d alloc failed", i + 1);
}
char param_name[17]; // 16 chars for parameter name + null terminator
snprintf(param_name, sizeof(param_name), "SIM_BAT%d_DRAIN", i + 1);
_drain_handles[i] = param_find(param_name);
if (_drain_handles[i] == PARAM_INVALID) {
PX4_ERR("Could not find parameter with name %s", param_name);
}
snprintf(param_name, sizeof(param_name), "SIM_BAT%d_MIN_PCT", i + 1);
_min_pct_handles[i] = param_find(param_name);
if (_min_pct_handles[i] == PARAM_INVALID) {
PX4_ERR("Could not find parameter with name %s", param_name);
}
_battery_percentage[i] = 1.f;
}
updateParams();
}
BatterySimulator::~BatterySimulator()
{
for (Battery *battery : _batteries) {
delete battery;
}
perf_free(_loop_perf);
}
bool BatterySimulator::init()
{
for (Battery *battery : _batteries) {
if (battery == nullptr) {
return false;
}
}
ScheduleOnInterval(BATTERY_SIMLATOR_SAMPLE_INTERVAL_US);
return true;
}
@@ -83,37 +119,69 @@ void BatterySimulator::Run()
const hrt_abstime now_us = hrt_absolute_time();
// Limit to +1.0 s to guard against division by 0
const float discharge_interval_us = math::max(_param_sim_bat_drain.get(), 1.0f) * 1000 * 1000;
const float ibatt = -1.0f; // no current sensor in simulation
if (_armed) {
if (_last_integration_us != 0) {
_battery_percentage -= (now_us - _last_integration_us) / discharge_interval_us;
for (int i = 0; i < battery_status_s::MAX_INSTANCES; i++) {
// Limit to +1.0 s to guard against division by 0
const float discharge_interval_us = math::max(fullDischargeTime(i), 1.0f) * 1000 * 1000;
if (_armed) {
if (_last_integration_us != 0) {
_battery_percentage[i] -= (now_us - _last_integration_us) / discharge_interval_us;
}
} else {
_battery_percentage[i] = 1.f;
}
_last_integration_us = now_us;
_battery_percentage[i] = math::max(_battery_percentage[i], minimumPercentage(i) / 100.f);
} else {
_battery_percentage = 1.f;
_last_integration_us = 0;
Battery &battery = *_batteries[i];
float vbatt = math::interpolate(_battery_percentage[i], 0.f, 1.f, battery.empty_cell_voltage(),
battery.full_cell_voltage());
vbatt *= battery.cell_count();
battery.setConnected(true);
battery.updateVoltage(vbatt);
battery.updateCurrent(ibatt);
battery.updateAndPublishBatteryStatus(now_us);
}
float ibatt = -1.0f; // no current sensor in simulation
_battery_percentage = math::max(_battery_percentage, _param_bat_min_pct.get() / 100.f);
float vbatt = math::interpolate(_battery_percentage, 0.f, 1.f, _battery.empty_cell_voltage(),
_battery.full_cell_voltage());
vbatt *= _battery.cell_count();
_battery.setConnected(true);
_battery.updateVoltage(vbatt);
_battery.updateCurrent(ibatt);
_battery.updateAndPublishBatteryStatus(now_us);
// All batteries integrate over the same interval, so this is only advanced once they are all done
_last_integration_us = _armed ? now_us : 0;
perf_end(_loop_perf);
}
void BatterySimulator::updateParams()
{
ModuleParams::updateParams();
for (int i = 0; i < battery_status_s::MAX_INSTANCES; i++) {
if (_drain_handles[i] != PARAM_INVALID) {
param_get(_drain_handles[i], &_drain_override_s[i]);
}
if (_min_pct_handles[i] != PARAM_INVALID) {
param_get(_min_pct_handles[i], &_min_pct_override[i]);
}
}
}
float BatterySimulator::fullDischargeTime(int battery_index) const
{
// A non-positive override means: use the drain time shared by all batteries
return (_drain_override_s[battery_index] > 0.f) ? _drain_override_s[battery_index] : _param_sim_bat_drain.get();
}
float BatterySimulator::minimumPercentage(int battery_index) const
{
// Only a negative override means: use the floor shared by all batteries. Zero is a valid floor.
return (_min_pct_override[battery_index] >= 0.f) ? _min_pct_override[battery_index] : _param_bat_min_pct.get();
}
int BatterySimulator::task_spawn(int argc, char *argv[])
{
BatterySimulator *instance = new BatterySimulator();

21
src/modules/simulation/battery_simulator/BatterySimulator.hpp
View File

@@ -34,6 +34,7 @@
#pragma once
#include <lib/battery/battery.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
@@ -69,6 +70,13 @@ public:
private:
void Run() override;
void updateParams() override;
// Time in seconds for the given battery to drain from 100% to 0% while armed
float fullDischargeTime(int battery_index) const;
// Lowest state of charge in percent the given battery drains down to
float minimumPercentage(int battery_index) const;
static constexpr uint32_t BATTERY_SIMLATOR_SAMPLE_FREQUENCY_HZ = 100; // Hz
static constexpr uint32_t BATTERY_SIMLATOR_SAMPLE_INTERVAL_US = 1_s / BATTERY_SIMLATOR_SAMPLE_FREQUENCY_HZ;
@@ -76,10 +84,19 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
Battery _battery;
// A battery only publishes if its BAT<N>_SOURCE is set to "Power Module / Analog".
// Allocated once in the constructor: Battery is not copyable and older compilers (GCC < 9) do not elide the copy
// when constructing an array of them in place.
Battery *_batteries[battery_status_s::MAX_INSTANCES] {};
// Per-battery overrides of SIM_BAT_DRAIN and SIM_BAT_MIN_PCT, negative when unset
param_t _drain_handles[battery_status_s::MAX_INSTANCES] {};
param_t _min_pct_handles[battery_status_s::MAX_INSTANCES] {};
float _drain_override_s[battery_status_s::MAX_INSTANCES] {};
float _min_pct_override[battery_status_s::MAX_INSTANCES] {};
uint64_t _last_integration_us{0};
float _battery_percentage{1.f};
float _battery_percentage[battery_status_s::MAX_INSTANCES] {};
bool _armed{false};
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};

29
src/modules/simulation/battery_simulator/battery_simulator_params.yaml
View File

@@ -12,6 +12,20 @@ parameters:
min: 0
increment: 1
unit: s
SIM_BAT${i}_DRAIN:
description:
short: Simulated battery ${i} full-discharge time
long: |-
Time in seconds for simulated battery ${i} to drain from 100% to 0% while armed,
overriding SIM_BAT_DRAIN for this battery only. A non-positive value uses SIM_BAT_DRAIN.
Only has an effect if battery ${i} is enabled through BAT${i}_SOURCE.
type: float
default: [-1.0, -1.0, -1.0]
min: -1
increment: 1
unit: s
num_instances: 3
instance_start: 1
SIM_BAT_MIN_PCT:
description:
short: Simulator Battery minimal percentage
@@ -24,3 +38,18 @@ parameters:
max: 100
increment: 0.1
unit: '%'
SIM_BAT${i}_MIN_PCT:
description:
short: Simulated battery ${i} minimal percentage
long: |-
Lowest state of charge simulated battery ${i} drains down to, overriding
SIM_BAT_MIN_PCT for this battery only. Set to -1 to use SIM_BAT_MIN_PCT.
Only has an effect if battery ${i} is enabled through BAT${i}_SOURCE.
type: float
default: [-1.0, -1.0, -1.0]
min: -1
max: 100
increment: 0.1
unit: '%'
num_instances: 3
instance_start: 1