diff --git a/docs/en/releases/main.md b/docs/en/releases/main.md index 4ff87ac1e03..57798f22e08 100644 --- a/docs/en/releases/main.md +++ b/docs/en/releases/main.md @@ -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 diff --git a/docs/en/simulation/failsafes.md b/docs/en/simulation/failsafes.md index 5dc34fd53ba..0dd4d260ee2 100644 --- a/docs/en/simulation/failsafes.md +++ b/docs/en/simulation/failsafes.md @@ -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. diff --git a/src/modules/simulation/battery_simulator/BatterySimulator.cpp b/src/modules/simulation/battery_simulator/BatterySimulator.cpp index 9763f277858..f9365483401 100644 --- a/src/modules/simulation/battery_simulator/BatterySimulator.cpp +++ b/src/modules/simulation/battery_simulator/BatterySimulator.cpp @@ -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(esc_status.esc_count), static_cast(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 diff --git a/src/modules/simulation/battery_simulator/BatterySimulator.hpp b/src/modules/simulation/battery_simulator/BatterySimulator.hpp index fc432f4da1d..67d38125390 100644 --- a/src/modules/simulation/battery_simulator/BatterySimulator.hpp +++ b/src/modules/simulation/battery_simulator/BatterySimulator.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -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_SOURCE "Power Module", which share the current + param_t _source_handles[battery_status_s::MAX_INSTANCES] {}; // 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 @@ -103,6 +109,8 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_sim_bat_drain, ///< battery drain interval - (ParamFloat) _param_bat_min_pct //< minimum battery percentage + (ParamFloat) _param_bat_min_pct, //< minimum battery percentage + (ParamFloat) _param_sim_bat_i_mot, + (ParamFloat) _param_sim_bat_i_idle ) }; diff --git a/src/modules/simulation/battery_simulator/battery_simulator_params.yaml b/src/modules/simulation/battery_simulator/battery_simulator_params.yaml index 382690b24ce..21797a5171d 100644 --- a/src/modules/simulation/battery_simulator/battery_simulator_params.yaml +++ b/src/modules/simulation/battery_simulator/battery_simulator_params.yaml @@ -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 diff --git a/src/modules/simulation/simulator_sih/aero.hpp b/src/modules/simulation/simulator_sih/aero.hpp index 1b353be7ff0..70f3cbef952 100644 --- a/src/modules/simulation/simulator_sih/aero.hpp +++ b/src/modules/simulation/simulator_sih/aero.hpp @@ -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) diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 50ac59d03ed..4fc4f7f998d 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -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) { diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index 60b1606c804..91baf2b7785 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -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