mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
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:
1 parent
fe9005fea5
commit
d9879734b9
8 files changed
+101
-3
No files matched your search
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
Reference in new issue
Block a user