diff --git a/src/modules/simulation/battery_simulator/BatterySimulator.cpp b/src/modules/simulation/battery_simulator/BatterySimulator.cpp index 54588eac94..45e8fde168 100644 --- a/src/modules/simulation/battery_simulator/BatterySimulator.cpp +++ b/src/modules/simulation/battery_simulator/BatterySimulator.cpp @@ -70,6 +70,8 @@ void BatterySimulator::Run() updateParams(); } + updateCommands(); + if (_vehicle_status_sub.updated()) { vehicle_status_s vehicle_status; @@ -99,6 +101,11 @@ void BatterySimulator::Run() _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()); + + if (_force_empty_battery) { + vbatt = _battery.empty_cell_voltage(); + } + vbatt *= _battery.cell_count(); _battery.setConnected(true); @@ -109,6 +116,60 @@ void BatterySimulator::Run() perf_end(_loop_perf); } +void BatterySimulator::updateCommands() +{ + vehicle_command_s vehicle_command; + + while (_vehicle_command_sub.update(&vehicle_command)) { + if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) { + continue; + } + + bool handled = false; + bool supported = false; + + const int failure_unit = static_cast(vehicle_command.param1 + 0.5f); + const int failure_type = static_cast(vehicle_command.param2 + 0.5f); + const int instance = static_cast(vehicle_command.param3 + 0.5f); + + if (failure_unit == vehicle_command_s::FAILURE_UNIT_SYSTEM_BATTERY) { + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + handled = true; + PX4_INFO("CMD_INJECT_FAILURE, battery ok"); + supported = false; + + if (instance == 0) { + supported = true; + _force_empty_battery = false; + } + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + // Force battery empty for FAILURE_TYPE_OFF - not perfectly accurate, but what we want to achieve + handled = true; + PX4_WARN("CMD_INJECT_FAILURE, battery empty"); + supported = false; + + if (instance == 0) { + supported = true; + _force_empty_battery = true; + } + } + } + + if (handled) { + vehicle_command_ack_s ack{}; + ack.command = vehicle_command.command; + ack.from_external = false; + ack.result = supported ? + vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : + vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + ack.timestamp = hrt_absolute_time(); + _command_ack_pub.publish(ack); + } + } +} + 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 8adf69be59..a25151b749 100644 --- a/src/modules/simulation/battery_simulator/BatterySimulator.hpp +++ b/src/modules/simulation/battery_simulator/BatterySimulator.hpp @@ -45,6 +45,8 @@ #include #include #include +#include +#include using namespace time_literals; @@ -67,6 +69,7 @@ public: private: void Run() override; + void updateCommands(); 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,12 +79,17 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + Battery _battery; uint64_t _last_integration_us{0}; float _battery_percentage{1.f}; bool _armed{false}; + bool _force_empty_battery{false}; + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; DEFINE_PARAMETERS(