diff --git a/docs/en/simulation/failsafes.md b/docs/en/simulation/failsafes.md index a9599552fff..4ae31c5316f 100644 --- a/docs/en/simulation/failsafes.md +++ b/docs/en/simulation/failsafes.md @@ -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. diff --git a/src/modules/simulation/battery_simulator/BatterySimulator.cpp b/src/modules/simulation/battery_simulator/BatterySimulator.cpp index 1f05dffd767..9763f277858 100644 --- a/src/modules/simulation/battery_simulator/BatterySimulator.cpp +++ b/src/modules/simulation/battery_simulator/BatterySimulator.cpp @@ -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(); diff --git a/src/modules/simulation/battery_simulator/BatterySimulator.hpp b/src/modules/simulation/battery_simulator/BatterySimulator.hpp index 0571ad0f982..fc432f4da1d 100644 --- a/src/modules/simulation/battery_simulator/BatterySimulator.hpp +++ b/src/modules/simulation/battery_simulator/BatterySimulator.hpp @@ -34,6 +34,7 @@ #pragma once #include +#include #include #include #include @@ -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_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")}; diff --git a/src/modules/simulation/battery_simulator/battery_simulator_params.yaml b/src/modules/simulation/battery_simulator/battery_simulator_params.yaml index db3ac91ce8e..382690b24ce 100644 --- a/src/modules/simulation/battery_simulator/battery_simulator_params.yaml +++ b/src/modules/simulation/battery_simulator/battery_simulator_params.yaml @@ -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