diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 677122634d..2222e5e0c3 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -245,6 +245,7 @@ if ! replay tryapplyparams then simulator start -c $simulator_tcp_port fi +battery_simulator start tone_alarm start rc_update start sensors start diff --git a/posix-configs/SITL/init/test/cmd_template.in b/posix-configs/SITL/init/test/cmd_template.in index fd9f8d8452..78a3c7726d 100644 --- a/posix-configs/SITL/init/test/cmd_template.in +++ b/posix-configs/SITL/init/test/cmd_template.in @@ -11,6 +11,7 @@ param set SYS_RESTART_TYPE 0 dataman start +battery_simulator start simulator start tone_alarm start pwm_out_sim start diff --git a/posix-configs/SITL/init/test/test_mavlink b/posix-configs/SITL/init/test/test_mavlink index 41972df9a3..540eb03096 100644 --- a/posix-configs/SITL/init/test/test_mavlink +++ b/posix-configs/SITL/init/test/test_mavlink @@ -11,6 +11,7 @@ param set SYS_RESTART_TYPE 0 dataman start +battery_simulator start simulator start tone_alarm start pwm_out_sim start diff --git a/posix-configs/SITL/init/test/test_shutdown b/posix-configs/SITL/init/test/test_shutdown index a1baf35c36..81f474a0a9 100644 --- a/posix-configs/SITL/init/test/test_shutdown +++ b/posix-configs/SITL/init/test/test_shutdown @@ -15,6 +15,7 @@ param set SYS_RESTART_TYPE 0 dataman start +battery_simulator start simulator start tone_alarm start pwm_out_sim start diff --git a/posix-configs/SITL/init/test/test_template.in b/posix-configs/SITL/init/test/test_template.in index c55a98ece2..a3bda8d9ab 100644 --- a/posix-configs/SITL/init/test/test_template.in +++ b/posix-configs/SITL/init/test/test_template.in @@ -11,6 +11,7 @@ param set SYS_RESTART_TYPE 0 dataman start +battery_simulator start simulator start tone_alarm start pwm_out_sim start diff --git a/posix-configs/eagle/hil/mainapphil.config b/posix-configs/eagle/hil/mainapphil.config index 61aac3cf47..30ed3728fc 100644 --- a/posix-configs/eagle/hil/mainapphil.config +++ b/posix-configs/eagle/hil/mainapphil.config @@ -9,4 +9,5 @@ sleep 1 mavlink stream -u 14556 -s HIGHRES_IMU -r 50 mavlink stream -u 14556 -s ATTITUDE -r 50 mavlink boot_complete +battery_simulator start simulator start diff --git a/src/modules/simulator/CMakeLists.txt b/src/modules/simulator/CMakeLists.txt index 639ce2096e..fdc6837154 100644 --- a/src/modules/simulator/CMakeLists.txt +++ b/src/modules/simulator/CMakeLists.txt @@ -66,7 +66,6 @@ px4_add_module( ${SIMULATOR_SRCS} DEPENDS git_mavlink_v2 - battery conversion git_ecl ecl_geo @@ -76,3 +75,5 @@ px4_add_module( drivers_magnetometer ) target_include_directories(modules__simulator INTERFACE ${PX4_SOURCE_DIR}/mavlink/include/mavlink) + +add_subdirectory(battery_simulator) diff --git a/src/modules/simulator/battery_simulator/BatterySimulator.cpp b/src/modules/simulator/battery_simulator/BatterySimulator.cpp new file mode 100644 index 0000000000..4c64875610 --- /dev/null +++ b/src/modules/simulator/battery_simulator/BatterySimulator.cpp @@ -0,0 +1,158 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "BatterySimulator.hpp" + +BatterySimulator::BatterySimulator() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) +{ +} + +BatterySimulator::~BatterySimulator() +{ + perf_free(_loop_perf); +} + +bool BatterySimulator::init() +{ + ScheduleOnInterval(SimulatorBattery::SIMLATOR_BATTERY_SAMPLE_INTERVAL_US); + return true; +} + +void BatterySimulator::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + } + } + + const hrt_abstime now_us = hrt_absolute_time(); + + const float discharge_interval_us = _param_sim_bat_drain.get() * 1000 * 1000; + + if (_armed) { + if (_last_integration_us != 0) { + _battery_percentage -= (now_us - _last_integration_us) / discharge_interval_us; + } + + _last_integration_us = now_us; + + } else { + _last_integration_us = 0; + } + + 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::gradual(_battery_percentage, 0.f, 1.f, _battery.empty_cell_voltage(), _battery.full_cell_voltage()); + vbatt *= _battery.cell_count(); + + const float throttle = 0.0f; // simulate no throttle compensation to make the estimate predictable + _battery.updateBatteryStatus(now_us, vbatt, ibatt, true, battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, throttle); + + perf_end(_loop_perf); +} + +int BatterySimulator::task_spawn(int argc, char *argv[]) +{ + BatterySimulator *instance = new BatterySimulator(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int BatterySimulator::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int BatterySimulator::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("battery_simulator", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int battery_simulator_main(int argc, char *argv[]) +{ + return BatterySimulator::main(argc, argv); +} diff --git a/src/modules/simulator/battery_simulator/BatterySimulator.hpp b/src/modules/simulator/battery_simulator/BatterySimulator.hpp new file mode 100644 index 0000000000..04cdecbed2 --- /dev/null +++ b/src/modules/simulator/battery_simulator/BatterySimulator.hpp @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class BatterySimulator : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + BatterySimulator(); + ~BatterySimulator() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + uORB::Publication _battery_pub{ORB_ID(battery_status)}; + + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + class SimulatorBattery : public Battery + { + public: + static constexpr uint32_t SIMLATOR_BATTERY_SAMPLE_FREQUENCY_HZ = 100; // Hz + static constexpr uint32_t SIMLATOR_BATTERY_SAMPLE_INTERVAL_US = 1_s / SIMLATOR_BATTERY_SAMPLE_FREQUENCY_HZ; + + SimulatorBattery() : Battery(1, nullptr, SIMLATOR_BATTERY_SAMPLE_INTERVAL_US) {} + + virtual void updateParams() override + { + Battery::updateParams(); + _params.v_empty = 3.5f; + _params.v_charged = 4.05f; + _params.n_cells = 4; + _params.capacity = 10.0f; + _params.v_load_drop = 0.0f; + _params.r_internal = 0.0f; + _params.low_thr = 0.15f; + _params.crit_thr = 0.07f; + _params.emergen_thr = 0.05f; + _params.source = 0; + } + } _battery; + + uint64_t _last_integration_us{0}; + float _battery_percentage{1.f}; + bool _armed{false}; + + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_sim_bat_drain, ///< battery drain interval + (ParamFloat) _param_bat_min_pct //< minimum battery percentage + ) +}; diff --git a/src/modules/simulator/battery_simulator/CMakeLists.txt b/src/modules/simulator/battery_simulator/CMakeLists.txt new file mode 100644 index 0000000000..c4ba81329e --- /dev/null +++ b/src/modules/simulator/battery_simulator/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2020 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE modules__simulator__battery_simulator + MAIN battery_simulator + COMPILE_FLAGS + SRCS + BatterySimulator.cpp + BatterySimulator.hpp + DEPENDS + battery + mathlib + px4_work_queue + ) diff --git a/src/modules/simulator/battery_simulator/battery_simulator_params.c b/src/modules/simulator/battery_simulator/battery_simulator_params.c new file mode 100644 index 0000000000..a77fe0e9a4 --- /dev/null +++ b/src/modules/simulator/battery_simulator/battery_simulator_params.c @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Simulator Battery drain interval + * + * @min 1 + * @max 86400 + * @increment 1 + * @unit s + * + * @group SITL + */ +PARAM_DEFINE_FLOAT(SIM_BAT_DRAIN, 60); + +/** + * Simulator Battery minimal percentage. Can be used to alter + * the battery level during SITL- or HITL-simulation on the fly. + * Particularly useful for testing different low-battery behaviour. + * + * @min 0 + * @max 100 + * @increment 0.1 + * @unit % + * + * @group SITL + */ +PARAM_DEFINE_FLOAT(SIM_BAT_MIN_PCT, 50.0f); diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 161e5d9f00..67d863dfb7 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -42,7 +42,6 @@ #pragma once -#include #include #include #include @@ -59,7 +58,6 @@ #include #include #include -#include #include #include #include @@ -79,7 +77,6 @@ #include #include -#include using namespace time_literals; @@ -168,7 +165,6 @@ private: perf_counter_t _perf_sim_interval{perf_alloc(PC_INTERVAL, MODULE_NAME": network interval")}; // uORB publisher handlers - uORB::Publication _battery_pub{ORB_ID(battery_status)}; uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::PublicationMulti _flow_pub{ORB_ID(optical_flow)}; uORB::Publication _irlock_report_pub{ORB_ID(irlock_report)}; @@ -188,31 +184,7 @@ private: hrt_abstime _last_sim_timestamp{0}; hrt_abstime _last_sitl_timestamp{0}; - hrt_abstime _last_battery_timestamp{0}; - class SimulatorBattery : public Battery - { - public: - static constexpr uint32_t SIMLATOR_BATTERY_SAMPLE_FREQUENCY_HZ = 100; // Hz - static constexpr uint32_t SIMLATOR_BATTERY_SAMPLE_INTERVAL_US = 1_s / SIMLATOR_BATTERY_SAMPLE_FREQUENCY_HZ; - - SimulatorBattery() : Battery(1, nullptr, SIMLATOR_BATTERY_SAMPLE_INTERVAL_US) {} - - virtual void updateParams() override - { - Battery::updateParams(); - _params.v_empty = 3.5f; - _params.v_charged = 4.05f; - _params.n_cells = 4; - _params.capacity = 10.0f; - _params.v_load_drop = 0.0f; - _params.r_internal = 0.0f; - _params.low_thr = 0.15f; - _params.crit_thr = 0.07f; - _params.emergen_thr = 0.05f; - _params.source = 0; - } - } _battery; void run(); void handle_message(const mavlink_message_t *msg); @@ -276,8 +248,6 @@ private: #endif DEFINE_PARAMETERS( - (ParamFloat) _param_sim_bat_drain, ///< battery drain interval - (ParamFloat) _param_bat_min_pct, //< minimum battery percentage (ParamBool) _param_sim_gps_block, (ParamBool) _param_sim_accel_block, (ParamBool) _param_sim_gyro_block, diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index a9cb457242..d294506a8e 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -355,40 +355,6 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg) update_sensors(now_us, imu); - static float battery_percentage = 1.0f; - static uint64_t last_integration_us = 0; - - // battery simulation (limit update to 100Hz) - if (hrt_elapsed_time(&_last_battery_timestamp) >= SimulatorBattery::SIMLATOR_BATTERY_SAMPLE_INTERVAL_US) { - - const float discharge_interval_us = _param_sim_bat_drain.get() * 1000 * 1000; - - bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); - - if (armed) { - if (last_integration_us != 0) { - battery_percentage -= (now_us - last_integration_us) / discharge_interval_us; - } - - last_integration_us = now_us; - - } else { - last_integration_us = 0; - } - - 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::gradual(battery_percentage, 0.f, 1.f, _battery.empty_cell_voltage(), _battery.full_cell_voltage()); - vbatt *= _battery.cell_count(); - - const float throttle = 0.0f; // simulate no throttle compensation to make the estimate predictable - _battery.updateBatteryStatus(now_us, vbatt, ibatt, true, battery_status_s::BATTERY_SOURCE_POWER_MODULE, - 0, throttle); - - _last_battery_timestamp = now_us; - } - #if defined(ENABLE_LOCKSTEP_SCHEDULER) if (!_has_initialized.load()) { diff --git a/src/modules/simulator/simulator_params.c b/src/modules/simulator/simulator_params.c index 7b66e20a0f..a4474db109 100644 --- a/src/modules/simulator/simulator_params.c +++ b/src/modules/simulator/simulator_params.c @@ -39,32 +39,6 @@ * @author Mohamed Abdelkader */ -/** - * Simulator Battery drain interval - * - * @min 1 - * @max 86400 - * @increment 1 - * @unit s - * - * @group SITL - */ -PARAM_DEFINE_FLOAT(SIM_BAT_DRAIN, 60); - -/** - * Simulator Battery minimal percentage. Can be used to alter - * the battery level during SITL- or HITL-simulation on the fly. - * Particularly useful for testing different low-battery behaviour. - * - * @min 0 - * @max 100 - * @increment 0.1 - * @unit % - * - * @group SITL - */ -PARAM_DEFINE_FLOAT(SIM_BAT_MIN_PCT, 50.0f); - /** * Simulator block GPS data. *