feat(simulation): simulate ESC and battery current in SIH

SITL had no battery current, so checks that need one could not be tested.
With SIM_BAT_I_MOT set (0, off by default), each SIH ESC reports a current
that grows with its motor output to the power of 1.5, up to SIM_BAT_I_MOT
at full output, and the simulated batteries share the sum of the ESC
currents plus SIM_BAT_I_IDLE evenly.

The exponent follows the SIH motor model: shaft power grows with the
propeller speed cubed, and SIH sets the speed to the max speed times
sqrt(output). The current uses the speed model of the reported rpm
(Thruster::throttle_to_speed_ratio), on the absolute output, so motors
running in reverse draw current too.

The battery current comes from the published ESC status, so motor
failures injected in the simulator lower it. Simulators without an ESC
current model keep reporting no current.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: jonas <jonspero@me.com>
This commit is contained in:
jonas committed 2026-11-20 11:31:22 +01:00
1 parent fe9005fea5
commit d9879734b9
8 files changed
+101 -3

No files matched your search

+1
View File
@@ -175,6 +175,7 @@ Please continue reading for [upgrade instructions](#upgrade-guide).
- jMAVSim has been removed in favour of [SIH](../sim_sih/index.md) with the [Hawkeye](../sim_hawkeye/index.md) visualizer. See the [Upgrade Guide](#upgrade-guide).
- Gazebo: the GNSS failure injection commands (`failure gps off`, `stuck` and `wrong`) now apply to the NavSat data published by the gz bridge, consistent with the other simulator paths. ([PX4-Autopilot#28398](https://github.com/PX4/PX4-Autopilot/pull/28398))
- [SIH](../sim_sih/index.md) can simulate ESC and battery current: with [SIM_BAT_I_MOT](../advanced_config/parameter_reference.md#SIM_BAT_I_MOT) set, each ESC reports that current times its motor output to the power of 1.5, and the simulated batteries share their sum plus [SIM_BAT_I_IDLE](../advanced_config/parameter_reference.md#SIM_BAT_I_IDLE).
### Debug & Logging
+9
View File
@@ -49,6 +49,15 @@ 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.
### Battery Current
By default the simulated battery reports no current.
With [SIM_BAT_I_MOT](../advanced_config/parameter_reference.md#SIM_BAT_I_MOT) set, simulators that model the ESC current report it in the ESC status, and the simulated batteries share the sum plus [SIM_BAT_I_IDLE](../advanced_config/parameter_reference.md#SIM_BAT_I_IDLE) evenly.
[SIH](../sim_sih/index.md) models each ESC current as `SIM_BAT_I_MOT` times the motor output to the power of 1.5.
Simulators without an ESC current model keep reporting no current.
Since the current comes from the ESC status, motor failures injected in the simulator (for example `failure motor wrong` in SIH) lower it.
### Multiple Batteries
By default only one battery is simulated.
@@ -56,6 +56,9 @@ BatterySimulator::BatterySimulator() :
PX4_ERR("Could not find parameter with name %s", param_name);
}
snprintf(param_name, sizeof(param_name), "BAT%d_SOURCE", i + 1);
_source_handles[i] = param_find(param_name);
snprintf(param_name, sizeof(param_name), "SIM_BAT%d_MIN_PCT", i + 1);
_min_pct_handles[i] = param_find(param_name);
@@ -119,7 +122,26 @@ void BatterySimulator::Run()
const hrt_abstime now_us = hrt_absolute_time();
const float ibatt = -1.0f; // no current sensor in simulation
// Current of each battery: no measurement, unless SIM_BAT_I_MOT is set and the simulator models the ESC current. It
// comes from the published ESC status, so failures injected in the simulator or its ESC telemetry change it too.
float ibatt = -1.0f;
esc_status_s esc_status;
if (_param_sim_bat_i_mot.get() > FLT_EPSILON && _esc_status_sub.copy(&esc_status)
&& hrt_elapsed_time(&esc_status.timestamp) < 1_s) {
float esc_current = 0.f;
for (int esc = 0; esc < math::min(static_cast<int>(esc_status.esc_count), static_cast<int>(esc_status_s::CONNECTED_ESC_MAX)); esc++) {
esc_current += esc_status.esc[esc].esc_current;
}
// simulators without an ESC current model report none, and the batteries then report no measurement
_esc_current_reported = _esc_current_reported || (esc_current > FLT_EPSILON);
if (_esc_current_reported) {
ibatt = (_param_sim_bat_i_idle.get() + esc_current) / _enabled_batteries;
}
}
for (int i = 0; i < battery_status_s::MAX_INSTANCES; i++) {
// Limit to +1.0 s to guard against division by 0
@@ -159,7 +181,16 @@ void BatterySimulator::updateParams()
{
ModuleParams::updateParams();
_enabled_batteries = 0;
for (int i = 0; i < battery_status_s::MAX_INSTANCES; i++) {
int32_t source = -1;
if (_source_handles[i] != PARAM_INVALID && param_get(_source_handles[i], &source) == PX4_OK
&& source == battery_status_s::SOURCE_POWER_MODULE) {
_enabled_batteries++;
}
if (_drain_handles[i] != PARAM_INVALID) {
param_get(_drain_handles[i], &_drain_override_s[i]);
}
@@ -168,6 +199,8 @@ void BatterySimulator::updateParams()
param_get(_min_pct_handles[i], &_min_pct_override[i]);
}
}
_enabled_batteries = math::max(_enabled_batteries, 1);
}
float BatterySimulator::fullDischargeTime(int battery_index) const
@@ -44,6 +44,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
@@ -83,6 +84,11 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
bool _esc_current_reported{false}; ///< the simulator models the ESC current
int _enabled_batteries{1}; ///< batteries with BAT<N>_SOURCE "Power Module", which share the current
param_t _source_handles[battery_status_s::MAX_INSTANCES] {};
// 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
@@ -103,6 +109,8 @@ private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::SIM_BAT_DRAIN>) _param_sim_bat_drain, ///< battery drain interval
(ParamFloat<px4::params::SIM_BAT_MIN_PCT>) _param_bat_min_pct //< minimum battery percentage
(ParamFloat<px4::params::SIM_BAT_MIN_PCT>) _param_bat_min_pct, //< minimum battery percentage
(ParamFloat<px4::params::SIM_BAT_I_MOT>) _param_sim_bat_i_mot,
(ParamFloat<px4::params::SIM_BAT_I_IDLE>) _param_sim_bat_i_idle
)
};
@@ -26,6 +26,33 @@ parameters:
unit: s
num_instances: 3
instance_start: 1
SIM_BAT_I_MOT:
description:
short: Simulated motor current at full output
long: |-
Current a simulated motor draws at full output. Simulators that model the ESC current
(SIH) report it in the ESC status, and the simulated batteries share the sum of the ESC
currents plus SIM_BAT_I_IDLE evenly. Set to 0 for no current measurement.
type: float
default: 0.0
min: 0.0
max: 200.0
increment: 0.5
decimal: 1
unit: A
SIM_BAT_I_IDLE:
description:
short: Simulated current without the motors
long: |-
Current drawn by everything other than the motors, e.g. avionics and payload.
Only used with SIM_BAT_I_MOT set.
type: float
default: 0.5
min: 0.0
max: 50.0
increment: 0.1
decimal: 1
unit: A
SIM_BAT_MIN_PCT:
description:
short: Simulator Battery minimal percentage
+10 -1
View File
@@ -259,6 +259,15 @@ public:
return n_rev_s * 60.0f;
}
/** throttle_to_speed_ratio(float u)
* compute the propeller speed as a share of the max speed,
* given the unit throttle u (in range [0,1]).
*/
static float throttle_to_speed_ratio(float u)
{
return sqrtf(fminf(fmaxf(u, 0.0f), 1.0f));
}
/** throttle_to_rpm(float throttle, const float rpm_max)
* compute the propeller rpm,
* given the unit throttle u (in range [0,1])
@@ -266,7 +275,7 @@ public:
*/
static float throttle_to_rpm(float u, const float rpm_max)
{
return rpm_max * sqrtf(fminf(fmaxf(u, 0.0f), 1.0f));
return rpm_max * throttle_to_speed_ratio(u);
}
/** throttle_to_rev_s(float throttle, const float rpm_max)
@@ -370,6 +370,13 @@ void Sih::sensor_step()
void Sih::parameters_updated()
{
// motor current at full output from the battery simulator, none without it
const param_t sim_bat_i_mot = param_find("SIM_BAT_I_MOT");
if (sim_bat_i_mot == PARAM_INVALID || param_get(sim_bat_i_mot, &_motor_current_max) != PX4_OK) {
_motor_current_max = 0.f;
}
_T_MAX = _sih_t_max.get();
_Q_MAX = _sih_q_max.get();
_L_ROLL = _sih_l_roll.get();
@@ -546,6 +553,9 @@ void Sih::publish_esc_status()
_esc_status.esc[motor_idx].actuator_function = esc_report_s::ACTUATOR_FUNCTION_MOTOR1 + motor_idx;
_esc_status.esc[motor_idx].esc_temperature = 50.f;
_esc_status.esc[motor_idx].esc_rpm = (int32_t)roundf(Thruster::throttle_to_rpm(_u[i], max_rpm));
// shaft power, so the current at constant voltage, grows with the propeller speed cubed
const float speed_ratio = Thruster::throttle_to_speed_ratio(fabsf(_u[i]));
_esc_status.esc[motor_idx].esc_current = _motor_current_max * speed_ratio * speed_ratio * speed_ratio;
_esc_status.esc_online_flags |= 1u << motor_idx;
if (_u[i] > FLT_EPSILON) {
@@ -373,6 +373,7 @@ private:
float _lpos_ref_alt;
float _ground_alt{0.f}; // ground altitude below the vehicle [m AMSL]
float _MASS, _T_MAX, _Q_MAX, _L_ROLL, _L_PITCH, _KDV, _KDW, _T_TAU, _F_T_MAX, _F_Q_MAX;
float _motor_current_max{0.f}; ///< [A] SIM_BAT_I_MOT
matrix::Matrix3f _I; // vehicle inertia matrix
matrix::Matrix3f _Im1; // inverse of the inertia matrix