fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

simulator: break out standalone battery simulator module

Browse Source 
- this is a small step towards unifying SITL & HITL
This commit is contained in:
Daniel Agar 2020-06-24 10:39:21 -04:00 committed by GitHub
parent ae4ed87a31
commit 55808ed2f9
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
14 changed files with 378 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ROMFS/px4fmu_common/init.d-posix/rcS
View File

@ -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

1
posix-configs/SITL/init/test/cmd_template.in
View File

@ -11,6 +11,7 @@ param set SYS_RESTART_TYPE 0
dataman start
battery_simulator start
simulator start
tone_alarm start
pwm_out_sim start

1
posix-configs/SITL/init/test/test_mavlink
View File

@ -11,6 +11,7 @@ param set SYS_RESTART_TYPE 0
dataman start
battery_simulator start
simulator start
tone_alarm start
pwm_out_sim start

1
posix-configs/SITL/init/test/test_shutdown
View File

@ -15,6 +15,7 @@ param set SYS_RESTART_TYPE 0
dataman start
battery_simulator start
simulator start
tone_alarm start
pwm_out_sim start

1
posix-configs/SITL/init/test/test_template.in
View File

@ -11,6 +11,7 @@ param set SYS_RESTART_TYPE 0
dataman start
battery_simulator start
simulator start
tone_alarm start
pwm_out_sim start

1
posix-configs/eagle/hil/mainapphil.config
View File

@ -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

3
src/modules/simulator/CMakeLists.txt
View File

@ -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)

158
src/modules/simulator/battery_simulator/BatterySimulator.cpp Normal file
View File

@ -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(&param_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);
}

109
src/modules/simulator/battery_simulator/BatterySimulator.hpp Normal file
View File

@ -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 <lib/battery/battery.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
using namespace time_literals;
class BatterySimulator : public ModuleBase<BatterySimulator>, 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_status_s> _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<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
)
};

45
src/modules/simulator/battery_simulator/CMakeLists.txt Normal file
View File

@ -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
)

58
src/modules/simulator/battery_simulator/battery_simulator_params.c Normal file
View File

@ -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);

30
src/modules/simulator/simulator.h
View File

@ -42,7 +42,6 @@
#pragma once
#include <battery/battery.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_range_finder.h>
@ -59,7 +58,6 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/ekf2_timestamps.h>
@ -79,7 +77,6 @@
#include <v2.0/common/mavlink.h>
#include <v2.0/mavlink_types.h>
#include <lib/battery/battery.h>
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_status_s> _battery_pub{ORB_ID(battery_status)};
uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
uORB::PublicationMulti<optical_flow_s> _flow_pub{ORB_ID(optical_flow)};
uORB::Publication<irlock_report_s> _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<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
(ParamBool<px4::params::SIM_GPS_BLOCK>) _param_sim_gps_block,
(ParamBool<px4::params::SIM_ACCEL_BLOCK>) _param_sim_accel_block,
(ParamBool<px4::params::SIM_GYRO_BLOCK>) _param_sim_gyro_block,

34
src/modules/simulator/simulator_mavlink.cpp
View File

@ -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()) {

26
src/modules/simulator/simulator_params.c
View File

@ -39,32 +39,6 @@
* @author Mohamed Abdelkader <mohamedashraf123@gmail.com>
*/
/**
* 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.
*