mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-23 09:27:34 +08:00
simulation organization and cleanup
- new modules/simulation directory to collect all simulators and related modules - new Tools/simulation directory to collect and organize scattered simulation submodules, scripts, etc - simulation module renamed to simulator_mavlink - sih renamed to simulator_sih (not a great name, but I wanted to be clear it was a simulator) - ignition_simulator renamed to simulator_ignition_bridge - large sitl_target.cmake split by simulation option and in some cases pushed to appropriate modules - sitl targets broken down to what's actually available (eg jmavsim only has 1 model and 1 world) - new Gazebo consistently referred to as Ignition for now (probably the least confusing thing until we fully drop Gazebo classic support someday)
This commit is contained in:
@@ -1,5 +0,0 @@
|
||||
menuconfig MODULES_IGNITION_SIMULATOR
|
||||
bool "ignition_simulator"
|
||||
default n
|
||||
---help---
|
||||
Enable support for ignition_simulator
|
||||
@@ -1,12 +0,0 @@
|
||||
menuconfig MODULES_SIH
|
||||
bool "sih"
|
||||
default n
|
||||
---help---
|
||||
Enable support for sih
|
||||
|
||||
menuconfig USER_SIH
|
||||
bool "sih running as userspace module"
|
||||
default y
|
||||
depends on BOARD_PROTECTED && MODULES_SIH
|
||||
---help---
|
||||
Put sih in userspace memory
|
||||
@@ -0,0 +1,15 @@
|
||||
menu "Simulation"
|
||||
menuconfig COMMON_SIMULATION
|
||||
bool "Common simulation modules"
|
||||
default n
|
||||
select MODULES_SIMULATION_BATTERY_SIMULATOR
|
||||
select MODULES_SIMULATION_PWM_OUT_SIM
|
||||
select MODULES_SIMULATION_SENSOR_BARO_SIM
|
||||
select MODULES_SIMULATION_SENSOR_GPS_SIM
|
||||
select MODULES_SIMULATION_SENSOR_MAG_SIM
|
||||
select MODULES_SIMULATION_SIMULATOR_MAVLINK
|
||||
select MODULES_SIMULATION_SIMULATOR_SIH
|
||||
---help---
|
||||
Enable default set of simulation modules
|
||||
rsource "*/Kconfig"
|
||||
endmenu
|
||||
+2
-2
@@ -1,6 +1,6 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2020-2022 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
|
||||
@@ -32,7 +32,7 @@
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__simulator__battery_simulator
|
||||
MODULE modules__simulation__battery_simulator
|
||||
MAIN battery_simulator
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
@@ -0,0 +1,5 @@
|
||||
menuconfig MODULES_SIMULATION_BATTERY_SIMULATOR
|
||||
bool "battery_simulator"
|
||||
default n
|
||||
---help---
|
||||
Enable support for battery_simulator
|
||||
@@ -0,0 +1,50 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
if(${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL} STREQUAL "px4_sitl")
|
||||
set(module_config "module_sim.yaml")
|
||||
else()
|
||||
set(module_config "module_hil.yaml")
|
||||
endif()
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__simulation__pwm_out_sim
|
||||
MAIN pwm_out_sim
|
||||
SRCS
|
||||
PWMSim.cpp
|
||||
MODULE_CONFIG
|
||||
${module_config}
|
||||
DEPENDS
|
||||
mixer
|
||||
mixer_module
|
||||
)
|
||||
@@ -0,0 +1,5 @@
|
||||
menuconfig MODULES_SIMULATION_PWM_OUT_SIM
|
||||
bool "pwm_out_sim"
|
||||
default n
|
||||
---help---
|
||||
Enable support for pwm_out_sim
|
||||
@@ -0,0 +1,330 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2022 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 "PWMSim.hpp"
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <px4_platform_common/getopt.h>
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
||||
#include <px4_platform_common/sem.hpp>
|
||||
|
||||
PWMSim::PWMSim(bool hil_mode_enabled) :
|
||||
CDev(PWM_OUTPUT0_DEVICE_PATH),
|
||||
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default)
|
||||
{
|
||||
_mixing_output.setAllDisarmedValues(PWM_SIM_DISARMED_MAGIC);
|
||||
_mixing_output.setAllFailsafeValues(PWM_SIM_FAILSAFE_MAGIC);
|
||||
_mixing_output.setAllMinValues(PWM_SIM_PWM_MIN_MAGIC);
|
||||
_mixing_output.setAllMaxValues(PWM_SIM_PWM_MAX_MAGIC);
|
||||
|
||||
_mixing_output.setIgnoreLockdown(hil_mode_enabled);
|
||||
|
||||
CDev::init();
|
||||
}
|
||||
|
||||
void
|
||||
PWMSim::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
_mixing_output.unregister();
|
||||
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
SmartLock lock_guard(_lock);
|
||||
|
||||
_mixing_output.update();
|
||||
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
updateParams();
|
||||
}
|
||||
|
||||
// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
|
||||
_mixing_output.updateSubscriptions(true);
|
||||
}
|
||||
|
||||
bool
|
||||
PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
|
||||
unsigned num_control_groups_updated)
|
||||
{
|
||||
// Only publish once we receive actuator_controls (important for lock-step to work correctly)
|
||||
if (num_control_groups_updated > 0) {
|
||||
actuator_outputs_s actuator_outputs{};
|
||||
actuator_outputs.noutputs = num_outputs;
|
||||
|
||||
const uint32_t reversible_outputs = _mixing_output.reversibleOutputs();
|
||||
|
||||
for (int i = 0; i < (int)num_outputs; i++) {
|
||||
if (outputs[i] != PWM_SIM_DISARMED_MAGIC) {
|
||||
|
||||
OutputFunction function = _mixing_output.outputFunction(i);
|
||||
bool is_reversible = reversible_outputs & (1u << i);
|
||||
float output = outputs[i];
|
||||
|
||||
if (((int)function >= (int)OutputFunction::Motor1 && (int)function <= (int)OutputFunction::MotorMax)
|
||||
&& !is_reversible) {
|
||||
// Scale non-reversible motors to [0, 1]
|
||||
actuator_outputs.output[i] = (output - PWM_SIM_PWM_MIN_MAGIC) / (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC);
|
||||
|
||||
} else {
|
||||
// Scale everything else to [-1, 1]
|
||||
const float pwm_center = (PWM_SIM_PWM_MAX_MAGIC + PWM_SIM_PWM_MIN_MAGIC) / 2.f;
|
||||
const float pwm_delta = (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC) / 2.f;
|
||||
actuator_outputs.output[i] = (output - pwm_center) / pwm_delta;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
actuator_outputs.timestamp = hrt_absolute_time();
|
||||
_actuator_outputs_sim_pub.publish(actuator_outputs);
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
int
|
||||
PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
SmartLock lock_guard(_lock);
|
||||
|
||||
int ret = OK;
|
||||
|
||||
switch (cmd) {
|
||||
case PWM_SERVO_ARM:
|
||||
break;
|
||||
|
||||
case PWM_SERVO_DISARM:
|
||||
break;
|
||||
|
||||
case PWM_SERVO_SET_MIN_PWM: {
|
||||
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
|
||||
|
||||
for (unsigned i = 0; i < pwm->channel_count; i++) {
|
||||
if (i < OutputModuleInterface::MAX_ACTUATORS && !_mixing_output.useDynamicMixing()) {
|
||||
_mixing_output.minValue(i) = pwm->values[i];
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case PWM_SERVO_SET_MAX_PWM: {
|
||||
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
|
||||
|
||||
for (unsigned i = 0; i < pwm->channel_count; i++) {
|
||||
if (i < OutputModuleInterface::MAX_ACTUATORS && !_mixing_output.useDynamicMixing()) {
|
||||
_mixing_output.maxValue(i) = pwm->values[i];
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case PWM_SERVO_SET_UPDATE_RATE:
|
||||
// PWMSim does not limit the update rate
|
||||
break;
|
||||
|
||||
case PWM_SERVO_SET_SELECT_UPDATE_RATE:
|
||||
break;
|
||||
|
||||
case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
|
||||
*(uint32_t *)arg = 9999;
|
||||
break;
|
||||
|
||||
case PWM_SERVO_GET_UPDATE_RATE:
|
||||
*(uint32_t *)arg = 9999;
|
||||
break;
|
||||
|
||||
case PWM_SERVO_GET_SELECT_UPDATE_RATE:
|
||||
*(uint32_t *)arg = 0;
|
||||
break;
|
||||
|
||||
case PWM_SERVO_GET_FAILSAFE_PWM: {
|
||||
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
|
||||
|
||||
for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
|
||||
pwm->values[i] = _mixing_output.failsafeValue(i);
|
||||
}
|
||||
|
||||
pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS;
|
||||
break;
|
||||
}
|
||||
|
||||
case PWM_SERVO_GET_DISARMED_PWM: {
|
||||
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
|
||||
|
||||
for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
|
||||
pwm->values[i] = _mixing_output.disarmedValue(i);
|
||||
}
|
||||
|
||||
pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS;
|
||||
break;
|
||||
}
|
||||
|
||||
case PWM_SERVO_GET_MIN_PWM: {
|
||||
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
|
||||
|
||||
for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
|
||||
pwm->values[i] = _mixing_output.minValue(i);
|
||||
}
|
||||
|
||||
pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS;
|
||||
break;
|
||||
}
|
||||
|
||||
case PWM_SERVO_GET_MAX_PWM: {
|
||||
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
|
||||
|
||||
for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
|
||||
pwm->values[i] = _mixing_output.maxValue(i);
|
||||
}
|
||||
|
||||
pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS;
|
||||
break;
|
||||
}
|
||||
|
||||
case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): {
|
||||
// no restrictions on output grouping
|
||||
unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
|
||||
|
||||
*(uint32_t *)arg = (1 << channel);
|
||||
break;
|
||||
}
|
||||
|
||||
case PWM_SERVO_GET_COUNT:
|
||||
*(unsigned *)arg = OutputModuleInterface::MAX_ACTUATORS;
|
||||
break;
|
||||
|
||||
case MIXERIOCRESET:
|
||||
_mixing_output.resetMixer();
|
||||
break;
|
||||
|
||||
case MIXERIOCLOADBUF: {
|
||||
const char *buf = (const char *)arg;
|
||||
unsigned buflen = strlen(buf);
|
||||
ret = _mixing_output.loadMixer(buf, buflen);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
ret = -ENOTTY;
|
||||
break;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
PWMSim::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
bool hil_mode = false;
|
||||
|
||||
int myoptind = 1;
|
||||
int ch;
|
||||
const char *myoptarg = nullptr;
|
||||
|
||||
while ((ch = px4_getopt(argc, argv, "m:", &myoptind, &myoptarg)) != EOF) {
|
||||
switch (ch) {
|
||||
case 'm':
|
||||
hil_mode = strcmp(myoptarg, "hil") == 0;
|
||||
break;
|
||||
|
||||
default:
|
||||
return print_usage("unrecognized flag");
|
||||
}
|
||||
}
|
||||
|
||||
PWMSim *instance = new PWMSim(hil_mode);
|
||||
|
||||
if (!instance) {
|
||||
PX4_ERR("alloc failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
instance->ScheduleNow();
|
||||
return 0;
|
||||
}
|
||||
|
||||
int PWMSim::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int PWMSim::print_status()
|
||||
{
|
||||
_mixing_output.printStatus();
|
||||
return 0;
|
||||
}
|
||||
|
||||
int PWMSim::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_WARN("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
Driver for simulated PWM outputs.
|
||||
|
||||
Its only function is to take `actuator_control` uORB messages,
|
||||
mix them with any loaded mixer and output the result to the
|
||||
`actuator_output` uORB topic.
|
||||
|
||||
It is used in SITL and HITL.
|
||||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("pwm_out_sim", "driver");
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the module");
|
||||
PRINT_MODULE_USAGE_PARAM_STRING('m', "sim", "hil|sim", "Mode", true);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int pwm_out_sim_main(int argc, char *argv[])
|
||||
{
|
||||
return PWMSim::main(argc, argv);
|
||||
}
|
||||
@@ -0,0 +1,94 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2019 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 <drivers/device/device.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_mixer.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <lib/mixer_module/mixer_module.hpp>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/tasks.h>
|
||||
#include <px4_platform_common/time.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
||||
#if defined(CONFIG_ARCH_BOARD_PX4_SITL)
|
||||
#define PARAM_PREFIX "PWM_MAIN"
|
||||
#else
|
||||
#define PARAM_PREFIX "HIL_ACT"
|
||||
#endif
|
||||
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class PWMSim : public cdev::CDev, public ModuleBase<PWMSim>, public OutputModuleInterface
|
||||
{
|
||||
public:
|
||||
PWMSim(bool hil_mode_enabled);
|
||||
|
||||
/** @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);
|
||||
|
||||
/** @see ModuleBase::print_status() */
|
||||
int print_status() override;
|
||||
|
||||
int ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
|
||||
|
||||
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
unsigned num_outputs, unsigned num_control_groups_updated) override;
|
||||
|
||||
private:
|
||||
|
||||
void Run() override;
|
||||
|
||||
static constexpr uint16_t PWM_SIM_DISARMED_MAGIC = 900;
|
||||
static constexpr uint16_t PWM_SIM_FAILSAFE_MAGIC = 600;
|
||||
static constexpr uint16_t PWM_SIM_PWM_MIN_MAGIC = 1000;
|
||||
static constexpr uint16_t PWM_SIM_PWM_MAX_MAGIC = 2000;
|
||||
|
||||
MixingOutput _mixing_output{PARAM_PREFIX, MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
uORB::Publication<actuator_outputs_s> _actuator_outputs_sim_pub{ORB_ID(actuator_outputs_sim)};
|
||||
};
|
||||
@@ -0,0 +1,8 @@
|
||||
|
||||
module_name: HIL
|
||||
actuator_output:
|
||||
show_subgroups_if: 'SYS_HITL>0'
|
||||
output_groups:
|
||||
- param_prefix: HIL_ACT
|
||||
channel_label: Channel
|
||||
num_channels: 16
|
||||
@@ -0,0 +1,7 @@
|
||||
|
||||
module_name: SIM
|
||||
actuator_output:
|
||||
output_groups:
|
||||
- param_prefix: PWM_MAIN
|
||||
channel_label: Channel
|
||||
num_channels: 16
|
||||
+2
-2
@@ -1,6 +1,6 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2021-2022 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
|
||||
@@ -32,7 +32,7 @@
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__simulator__sensor_baro_sim
|
||||
MODULE modules__simulation__sensor_baro_sim
|
||||
MAIN sensor_baro_sim
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
@@ -0,0 +1,5 @@
|
||||
menuconfig MODULES_SIMULATION_SENSOR_BARO_SIM
|
||||
bool "sensor_baro_sim"
|
||||
default n
|
||||
---help---
|
||||
Enable support for sensor_baro_sim
|
||||
+2
-2
@@ -1,6 +1,6 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2021-2022 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
|
||||
@@ -32,7 +32,7 @@
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__simulator__sensor_gps_sim
|
||||
MODULE modules__simulation__sensor_gps_sim
|
||||
MAIN sensor_gps_sim
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
@@ -0,0 +1,5 @@
|
||||
menuconfig MODULES_SIMULATION_SENSOR_GPS_SIM
|
||||
bool "sensor_gps_sim"
|
||||
default n
|
||||
---help---
|
||||
Enable support for sensor_gps_sim
|
||||
+1
-1
@@ -32,7 +32,7 @@
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__simulator__sensor_mag_sim
|
||||
MODULE modules__simulation__senosr_mag_sim
|
||||
MAIN sensor_mag_sim
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
@@ -0,0 +1,5 @@
|
||||
menuconfig MODULES_SIMULATION_SENSOR_MAG_SIM
|
||||
bool "sensor_mag_sim"
|
||||
default n
|
||||
---help---
|
||||
Enable support for sensor_mag_sim
|
||||
+46
-16
@@ -1,6 +1,6 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2022 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
|
||||
@@ -32,7 +32,6 @@
|
||||
############################################################################
|
||||
|
||||
add_compile_options(-frtti -fexceptions)
|
||||
#add_compile_options(-fexceptions)
|
||||
|
||||
# Find the Ignition_Transport library
|
||||
find_package(ignition-transport
|
||||
@@ -41,33 +40,64 @@ find_package(ignition-transport
|
||||
ignition-transport8
|
||||
ignition-transport10
|
||||
ignition-transport11
|
||||
|
||||
#QUIET
|
||||
)
|
||||
|
||||
message(STATUS "PACKAGE_FIND_NAME: ${ignition-transport_NAME}")
|
||||
message(STATUS "PACKAGE_FIND_VERSION_MAJOR: ${ignition-transport_VERSION_MAJOR}")
|
||||
message(STATUS "PACKAGE_FIND_VERSION_MINOR: ${ignition-transport_VERSION_MINOR}")
|
||||
message(STATUS "PACKAGE_FIND_VERSION_COUNT: ${ignition-transport_VERSION_COUNT}")
|
||||
|
||||
set(IGN_TRANSPORT_VER ${ignition-transport_VERSION_MAJOR})
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__ignition_simulator
|
||||
MAIN ignition_simulator
|
||||
MODULE modules__simulation__ignition_bridge
|
||||
MAIN simulator_ignition_bridge
|
||||
COMPILE_FLAGS
|
||||
#${MAX_CUSTOM_OPT_LEVEL}
|
||||
-O0
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
SRCS
|
||||
IgnitionSimulator.cpp
|
||||
IgnitionSimulator.hpp
|
||||
SimulatorIgnitionBridge.cpp
|
||||
SimulatorIgnitionBridge.hpp
|
||||
DEPENDS
|
||||
drivers_accelerometer
|
||||
drivers_gyroscope
|
||||
mixer_module
|
||||
px4_work_queue
|
||||
ignition-transport${IGN_TRANSPORT_VER}::core
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
)
|
||||
|
||||
target_link_libraries(modules__ignition_simulator PRIVATE ignition-transport${IGN_TRANSPORT_VER}::core)
|
||||
file(GLOB ign_models
|
||||
LIST_DIRECTORIES true
|
||||
RELATIVE ${PX4_SOURCE_DIR}/Tools/simulation/ignition/models
|
||||
CONFIGURE_DEPENDS
|
||||
${PX4_SOURCE_DIR}/Tools/simulation/ignition/models/*
|
||||
)
|
||||
|
||||
file(GLOB ign_worlds
|
||||
CONFIGURE_DEPENDS
|
||||
${PX4_SOURCE_DIR}/Tools/simulation/ignition/worlds/*.sdf
|
||||
)
|
||||
|
||||
foreach(model ${ign_models})
|
||||
foreach(world ${ign_worlds})
|
||||
|
||||
get_filename_component("world_name" ${world} NAME_WE)
|
||||
|
||||
if(world_name MATCHES "default")
|
||||
add_custom_target(ign_${model}
|
||||
COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=${model} $<TARGET_FILE:px4>
|
||||
WORKING_DIRECTORY ${SITL_WORKING_DIR}
|
||||
USES_TERMINAL
|
||||
DEPENDS px4
|
||||
)
|
||||
else()
|
||||
add_custom_target(ign_${model}_${world_name}
|
||||
COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=${model} PX4_SIM_WORLD=${world_name} $<TARGET_FILE:px4>
|
||||
WORKING_DIRECTORY ${SITL_WORKING_DIR}
|
||||
USES_TERMINAL
|
||||
DEPENDS px4
|
||||
)
|
||||
endif()
|
||||
endforeach()
|
||||
endforeach()
|
||||
|
||||
|
||||
# TODO: PX4_IGN_MODELS_PATH
|
||||
# PX4_IGN_WORLDS_PATH
|
||||
configure_file(gazebo_env.sh.in ${PX4_BINARY_DIR}/rootfs/gazebo_env.sh)
|
||||
@@ -0,0 +1,5 @@
|
||||
menuconfig MODULES_SIMULATION_SIMULATOR_IGNITION_BRIDGE
|
||||
bool "simulator_ignition_bridge"
|
||||
default n
|
||||
---help---
|
||||
Enable support for simulator_ignition_bridge
|
||||
+59
-23
@@ -31,7 +31,7 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "IgnitionSimulator.hpp"
|
||||
#include "SimulatorIgnitionBridge.hpp"
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
||||
@@ -42,7 +42,7 @@
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
|
||||
IgnitionSimulator::IgnitionSimulator(const char *world, const char *model) :
|
||||
SimulatorIgnitionBridge::SimulatorIgnitionBridge(const char *world, const char *model) :
|
||||
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
|
||||
_px4_accel(1310988), // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
|
||||
_px4_gyro(1310988), // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
|
||||
@@ -54,7 +54,7 @@ IgnitionSimulator::IgnitionSimulator(const char *world, const char *model) :
|
||||
updateParams();
|
||||
}
|
||||
|
||||
IgnitionSimulator::~IgnitionSimulator()
|
||||
SimulatorIgnitionBridge::~SimulatorIgnitionBridge()
|
||||
{
|
||||
// TODO: unsubscribe
|
||||
|
||||
@@ -63,18 +63,54 @@ IgnitionSimulator::~IgnitionSimulator()
|
||||
}
|
||||
}
|
||||
|
||||
int IgnitionSimulator::init()
|
||||
int SimulatorIgnitionBridge::init()
|
||||
{
|
||||
// service call to create model
|
||||
// ign service -s /world/${PX4_SIM_WORLD}/create --reqtype ignition.msgs.EntityFactory --reptype ignition.msgs.Boolean --timeout 1000 --req "sdf_filename: \"${PX4_SIM_MODEL}/model.sdf\""
|
||||
ignition::msgs::EntityFactory req{};
|
||||
req.set_sdf_filename(_model_name + "/model.sdf");
|
||||
|
||||
// TODO: support model instances?
|
||||
// req.set_name("model_instance_name"); // New name for the entity, overrides the name on the SDF.
|
||||
req.set_allow_renaming(false); // allowed to rename the entity in case of overlap with existing entities
|
||||
|
||||
// /world/$WORLD/create service.
|
||||
ignition::msgs::Boolean rep;
|
||||
bool result;
|
||||
std::string create_service = "/world/" + _world_name + "/create";
|
||||
|
||||
if (_node.Request(create_service, req, 1000, rep, result)) {
|
||||
if (!result) {
|
||||
PX4_ERR("Service call failed");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("Service call timed out");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
|
||||
// clock
|
||||
std::string clock_topic = "/world/" + _world_name + "/clock";
|
||||
_node.Subscribe(clock_topic, &IgnitionSimulator::clockCallback, this);
|
||||
|
||||
if (!_node.Subscribe(clock_topic, &SimulatorIgnitionBridge::clockCallback, this)) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
// pose: /world/$WORLD/pose/info
|
||||
std::string world_pose_topic = "/world/" + _world_name + "/pose/info";
|
||||
_node.Subscribe(world_pose_topic, &IgnitionSimulator::poseInfoCallback, this);
|
||||
|
||||
if (!_node.Subscribe(world_pose_topic, &SimulatorIgnitionBridge::poseInfoCallback, this)) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
// IMU: /world/$WORLD/model/$MODEL//link/base_link/sensor/imu_sensor/imu
|
||||
std::string imu_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/base_link/sensor/imu_sensor/imu";
|
||||
_node.EnableStats(imu_topic, true);
|
||||
_node.Subscribe(imu_topic, &IgnitionSimulator::imuCallback, this);
|
||||
|
||||
if (!_node.Subscribe(imu_topic, &SimulatorIgnitionBridge::imuCallback, this)) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
for (auto &sub_topic : _node.SubscribedTopics()) {
|
||||
PX4_INFO("subscribed: %s", sub_topic.c_str());
|
||||
@@ -93,9 +129,9 @@ int IgnitionSimulator::init()
|
||||
return OK;
|
||||
}
|
||||
|
||||
int IgnitionSimulator::task_spawn(int argc, char *argv[])
|
||||
int SimulatorIgnitionBridge::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
const char *world_name = "empty";
|
||||
const char *world_name = "default";
|
||||
const char *model_name = nullptr;
|
||||
|
||||
bool error_flag = false;
|
||||
@@ -132,7 +168,7 @@ int IgnitionSimulator::task_spawn(int argc, char *argv[])
|
||||
|
||||
PX4_INFO("world: %s, model: %s", world_name, model_name);
|
||||
|
||||
IgnitionSimulator *instance = new IgnitionSimulator(world_name, model_name);
|
||||
SimulatorIgnitionBridge *instance = new SimulatorIgnitionBridge(world_name, model_name);
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
@@ -153,7 +189,7 @@ int IgnitionSimulator::task_spawn(int argc, char *argv[])
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
void IgnitionSimulator::clockCallback(const ignition::msgs::Clock &clock)
|
||||
void SimulatorIgnitionBridge::clockCallback(const ignition::msgs::Clock &clock)
|
||||
{
|
||||
// uint64_t time_us = clock.sim().sec() * 1e6 + clock.sim().nsec() / 1000;
|
||||
// PX4_INFO("clock %lu ", time_us);
|
||||
@@ -164,7 +200,7 @@ void IgnitionSimulator::clockCallback(const ignition::msgs::Clock &clock)
|
||||
px4_clock_settime(CLOCK_MONOTONIC, &ts);
|
||||
}
|
||||
|
||||
void IgnitionSimulator::imuCallback(const ignition::msgs::IMU &imu)
|
||||
void SimulatorIgnitionBridge::imuCallback(const ignition::msgs::IMU &imu)
|
||||
{
|
||||
// FLU -> FRD
|
||||
static const auto q_FLU_to_FRD = ignition::math::Quaterniond(0, 1, 0, 0);
|
||||
@@ -188,7 +224,7 @@ void IgnitionSimulator::imuCallback(const ignition::msgs::IMU &imu)
|
||||
}
|
||||
}
|
||||
|
||||
void IgnitionSimulator::poseInfoCallback(const ignition::msgs::Pose_V &pose)
|
||||
void SimulatorIgnitionBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose)
|
||||
{
|
||||
//const uint64_t time_us = pose.header().stamp().sec() * 1e6 + pose.header().stamp().nsec() / 1000;
|
||||
const uint64_t time_us = hrt_absolute_time();
|
||||
@@ -300,8 +336,8 @@ void IgnitionSimulator::poseInfoCallback(const ignition::msgs::Pose_V &pose)
|
||||
}
|
||||
}
|
||||
|
||||
bool IgnitionSimulator::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
|
||||
unsigned num_control_groups_updated)
|
||||
bool SimulatorIgnitionBridge::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
|
||||
unsigned num_control_groups_updated)
|
||||
{
|
||||
ignition::msgs::Actuators rotor_velocity_message;
|
||||
rotor_velocity_message.mutable_velocity()->Resize(num_outputs, 0);
|
||||
@@ -317,7 +353,7 @@ bool IgnitionSimulator::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACT
|
||||
return false;
|
||||
}
|
||||
|
||||
void IgnitionSimulator::Run()
|
||||
void SimulatorIgnitionBridge::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
@@ -342,7 +378,7 @@ void IgnitionSimulator::Run()
|
||||
_mixing_output.updateSubscriptions(true);
|
||||
}
|
||||
|
||||
int IgnitionSimulator::print_status()
|
||||
int SimulatorIgnitionBridge::print_status()
|
||||
{
|
||||
//perf_print_counter(_cycle_perf);
|
||||
_mixing_output.printStatus();
|
||||
@@ -350,12 +386,12 @@ int IgnitionSimulator::print_status()
|
||||
return 0;
|
||||
}
|
||||
|
||||
int IgnitionSimulator::custom_command(int argc, char *argv[])
|
||||
int SimulatorIgnitionBridge::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int IgnitionSimulator::print_usage(const char *reason)
|
||||
int SimulatorIgnitionBridge::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_WARN("%s\n", reason);
|
||||
@@ -367,7 +403,7 @@ int IgnitionSimulator::print_usage(const char *reason)
|
||||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("ignition_simulator", "driver");
|
||||
PRINT_MODULE_USAGE_NAME("simulator_ignition_bridge", "driver");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_PARAM_STRING('m', nullptr, nullptr, "Model name", false);
|
||||
PRINT_MODULE_USAGE_PARAM_STRING('w', nullptr, nullptr, "World name", true);
|
||||
@@ -376,7 +412,7 @@ int IgnitionSimulator::print_usage(const char *reason)
|
||||
return 0;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int ignition_simulator_main(int argc, char *argv[])
|
||||
extern "C" __EXPORT int simulator_ignition_bridge_main(int argc, char *argv[])
|
||||
{
|
||||
return IgnitionSimulator::main(argc, argv);
|
||||
return SimulatorIgnitionBridge::main(argc, argv);
|
||||
}
|
||||
+7
-7
@@ -58,11 +58,11 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class IgnitionSimulator : public ModuleBase<IgnitionSimulator>, public OutputModuleInterface
|
||||
class SimulatorIgnitionBridge : public ModuleBase<SimulatorIgnitionBridge>, public OutputModuleInterface
|
||||
{
|
||||
public:
|
||||
IgnitionSimulator(const char *world, const char *model);
|
||||
~IgnitionSimulator() override;
|
||||
SimulatorIgnitionBridge(const char *world, const char *model);
|
||||
~SimulatorIgnitionBridge() override;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
@@ -112,14 +112,14 @@ private:
|
||||
const std::string _world_name;
|
||||
const std::string _model_name;
|
||||
|
||||
MixingOutput _mixing_output{"SIM_GZ", 8, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||
MixingOutput _mixing_output{"SIM_IGN", 8, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||
|
||||
ignition::transport::Node _node;
|
||||
ignition::transport::Node::Publisher _actuators_pub;
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::SIM_HOME_LAT>) _param_sim_home_lat,
|
||||
(ParamFloat<px4::params::SIM_HOME_LON>) _param_sim_home_lon,
|
||||
(ParamFloat<px4::params::SIM_HOME_ALT>) _param_sim_home_alt
|
||||
(ParamFloat<px4::params::SIM_IGN_HOME_LAT>) _param_sim_home_lat,
|
||||
(ParamFloat<px4::params::SIM_IGN_HOME_LON>) _param_sim_home_lon,
|
||||
(ParamFloat<px4::params::SIM_IGN_HOME_ALT>) _param_sim_home_alt
|
||||
)
|
||||
};
|
||||
@@ -0,0 +1,6 @@
|
||||
#!/usr/bin/env bash
|
||||
|
||||
export PX4_IGN_GAZEBO_MODELS=@PX4_SOURCE_DIR@/Tools/simulation/ignition/models
|
||||
export PX4_IGN_GAZEBO_WORLDS=@PX4_SOURCE_DIR@/Tools/simulation/ignition/worlds
|
||||
|
||||
export IGN_GAZEBO_RESOURCE_PATH=$IGN_GAZEBO_RESOURCE_PATH:$PX4_IGN_GAZEBO_MODELS
|
||||
+2
-2
@@ -1,7 +1,7 @@
|
||||
module_name: SIM_GZ
|
||||
module_name: SIM_IGN
|
||||
actuator_output:
|
||||
output_groups:
|
||||
- param_prefix: SIM_GZ
|
||||
- param_prefix: SIM_IGN
|
||||
channel_label: Channel
|
||||
num_channels: 8
|
||||
standard_params:
|
||||
+4
-4
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2022 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
|
||||
@@ -37,7 +37,7 @@
|
||||
* @unit deg
|
||||
* @group Simulator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIM_HOME_LAT, 47.397742f);
|
||||
PARAM_DEFINE_FLOAT(SIM_IGN_HOME_LAT, 47.397742f);
|
||||
|
||||
/**
|
||||
* simulator origin longitude
|
||||
@@ -45,7 +45,7 @@ PARAM_DEFINE_FLOAT(SIM_HOME_LAT, 47.397742f);
|
||||
* @unit deg
|
||||
* @group Simulator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIM_HOME_LON, 8.545594);
|
||||
PARAM_DEFINE_FLOAT(SIM_IGN_HOME_LON, 8.545594);
|
||||
|
||||
/**
|
||||
* simulator origin altitude
|
||||
@@ -53,4 +53,4 @@ PARAM_DEFINE_FLOAT(SIM_HOME_LON, 8.545594);
|
||||
* @unit m
|
||||
* @group Simulator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIM_HOME_ALT, 488.0);
|
||||
PARAM_DEFINE_FLOAT(SIM_IGN_HOME_ALT, 488.0);
|
||||
@@ -0,0 +1,59 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015-2022 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__simulation__simulator_mavlink
|
||||
MAIN simulator_mavlink
|
||||
COMPILE_FLAGS
|
||||
-Wno-double-promotion
|
||||
-Wno-cast-align
|
||||
-Wno-address-of-packed-member # TODO: fix in c_library_v2
|
||||
INCLUDES
|
||||
${CMAKE_BINARY_DIR}/mavlink
|
||||
${CMAKE_BINARY_DIR}/mavlink/development
|
||||
SRCS
|
||||
SimulatorMavlink.cpp
|
||||
SimulatorMavlink.hpp
|
||||
DEPENDS
|
||||
mavlink_c_generate
|
||||
conversion
|
||||
geo
|
||||
drivers_accelerometer
|
||||
drivers_gyroscope
|
||||
drivers_magnetometer
|
||||
)
|
||||
|
||||
include(sitl_targets_flightgear.cmake)
|
||||
include(sitl_targets_gazebo.cmake)
|
||||
include(sitl_targets_jmavsim.cmake)
|
||||
include(sitl_targets_jsbsim.cmake)
|
||||
@@ -0,0 +1,5 @@
|
||||
menuconfig MODULES_SIMULATION_SIMULATOR_MAVLINK
|
||||
bool "simulator_mavlink"
|
||||
default n
|
||||
---help---
|
||||
Enable support for mavlink simulator
|
||||
+157
-174
@@ -33,8 +33,7 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "simulator.h"
|
||||
#include <simulator_config.h>
|
||||
#include "SimulatorMavlink.hpp"
|
||||
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/time.h>
|
||||
@@ -59,18 +58,6 @@
|
||||
|
||||
#include <limits>
|
||||
|
||||
#ifdef ENABLE_UART_RC_INPUT
|
||||
#ifndef B460800
|
||||
#define B460800 460800
|
||||
#endif
|
||||
|
||||
#ifndef B921600
|
||||
#define B921600 921600
|
||||
#endif
|
||||
|
||||
static int openUart(const char *uart_name, int baud);
|
||||
#endif
|
||||
|
||||
static int _fd;
|
||||
static unsigned char _buf[2048];
|
||||
static sockaddr_in _srcaddr;
|
||||
@@ -81,6 +68,10 @@ const unsigned mode_flag_custom = 1;
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static px4_task_t g_sim_task = -1;
|
||||
|
||||
SimulatorMavlink *SimulatorMavlink::_instance = nullptr;
|
||||
|
||||
static constexpr vehicle_odometry_s vehicle_odometry_empty {
|
||||
.timestamp = 0,
|
||||
.timestamp_sample = 0,
|
||||
@@ -97,8 +88,8 @@ static constexpr vehicle_odometry_s vehicle_odometry_empty {
|
||||
.quality = 0
|
||||
};
|
||||
|
||||
Simulator::Simulator()
|
||||
: ModuleParams(nullptr)
|
||||
SimulatorMavlink::SimulatorMavlink() :
|
||||
ModuleParams(nullptr)
|
||||
{
|
||||
int32_t sys_ctrl_alloc = 0;
|
||||
param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc);
|
||||
@@ -111,7 +102,20 @@ Simulator::Simulator()
|
||||
}
|
||||
}
|
||||
|
||||
void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *msg)
|
||||
void SimulatorMavlink::parameters_update(bool force)
|
||||
{
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated() || force) {
|
||||
// clear update
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
|
||||
// update parameters from storage
|
||||
updateParams();
|
||||
}
|
||||
}
|
||||
|
||||
void SimulatorMavlink::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *msg)
|
||||
{
|
||||
memset(msg, 0, sizeof(mavlink_hil_actuator_controls_t));
|
||||
|
||||
@@ -216,7 +220,7 @@ void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *
|
||||
#endif
|
||||
}
|
||||
|
||||
void Simulator::send_esc_telemetry(mavlink_hil_actuator_controls_t hil_act_control)
|
||||
void SimulatorMavlink::send_esc_telemetry(mavlink_hil_actuator_controls_t hil_act_control)
|
||||
{
|
||||
esc_status_s esc_status{};
|
||||
esc_status.timestamp = hrt_absolute_time();
|
||||
@@ -226,7 +230,7 @@ void Simulator::send_esc_telemetry(mavlink_hil_actuator_controls_t hil_act_contr
|
||||
esc_status.esc_armed_flags = armed ? 255 : 0; // ugly
|
||||
|
||||
for (int i = 0; i < esc_status.esc_count; i++) {
|
||||
esc_status.esc[i].actuator_function = _output_functions[i]; // TODO: this should be in pwm_out_sim...
|
||||
esc_status.esc[i].actuator_function = _output_functions[i]; // TODO: this should be in pwm_sim...
|
||||
esc_status.esc[i].timestamp = esc_status.timestamp;
|
||||
esc_status.esc[i].esc_errorcount = 0; // TODO
|
||||
esc_status.esc[i].esc_voltage = _battery_status.voltage_v;
|
||||
@@ -239,7 +243,7 @@ void Simulator::send_esc_telemetry(mavlink_hil_actuator_controls_t hil_act_contr
|
||||
_esc_status_pub.publish(esc_status);
|
||||
}
|
||||
|
||||
void Simulator::send_controls()
|
||||
void SimulatorMavlink::send_controls()
|
||||
{
|
||||
orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuator_outputs);
|
||||
|
||||
@@ -258,7 +262,7 @@ void Simulator::send_controls()
|
||||
}
|
||||
}
|
||||
|
||||
void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &sensors)
|
||||
void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &sensors)
|
||||
{
|
||||
// temperature only updated with baro
|
||||
if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO) {
|
||||
@@ -402,7 +406,7 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
|
||||
}
|
||||
}
|
||||
|
||||
void Simulator::handle_message(const mavlink_message_t *msg)
|
||||
void SimulatorMavlink::handle_message(const mavlink_message_t *msg)
|
||||
{
|
||||
switch (msg->msgid) {
|
||||
case MAVLINK_MSG_ID_HIL_SENSOR:
|
||||
@@ -454,14 +458,14 @@ void Simulator::handle_message(const mavlink_message_t *msg)
|
||||
}
|
||||
}
|
||||
|
||||
void Simulator::handle_message_distance_sensor(const mavlink_message_t *msg)
|
||||
void SimulatorMavlink::handle_message_distance_sensor(const mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_distance_sensor_t dist;
|
||||
mavlink_msg_distance_sensor_decode(msg, &dist);
|
||||
publish_distance_topic(&dist);
|
||||
}
|
||||
|
||||
void Simulator::handle_message_hil_gps(const mavlink_message_t *msg)
|
||||
void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_hil_gps_t hil_gps;
|
||||
mavlink_msg_hil_gps_decode(msg, &hil_gps);
|
||||
@@ -531,7 +535,7 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg)
|
||||
}
|
||||
}
|
||||
|
||||
void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg)
|
||||
void SimulatorMavlink::handle_message_hil_sensor(const mavlink_message_t *msg)
|
||||
{
|
||||
if (_lockstep_component == -1) {
|
||||
_lockstep_component = px4_lockstep_register_component();
|
||||
@@ -572,7 +576,7 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg)
|
||||
px4_lockstep_progress(_lockstep_component);
|
||||
}
|
||||
|
||||
void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg)
|
||||
void SimulatorMavlink::handle_message_hil_state_quaternion(const mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_hil_state_quaternion_t hil_state;
|
||||
mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
|
||||
@@ -662,7 +666,7 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
|
||||
}
|
||||
}
|
||||
|
||||
void Simulator::handle_message_landing_target(const mavlink_message_t *msg)
|
||||
void SimulatorMavlink::handle_message_landing_target(const mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_landing_target_t landing_target_mavlink;
|
||||
mavlink_msg_landing_target_decode(msg, &landing_target_mavlink);
|
||||
@@ -683,7 +687,7 @@ void Simulator::handle_message_landing_target(const mavlink_message_t *msg)
|
||||
}
|
||||
}
|
||||
|
||||
void Simulator::handle_message_odometry(const mavlink_message_t *msg)
|
||||
void SimulatorMavlink::handle_message_odometry(const mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_odometry_t odom_in;
|
||||
mavlink_msg_odometry_decode(msg, &odom_in);
|
||||
@@ -900,7 +904,7 @@ void Simulator::handle_message_odometry(const mavlink_message_t *msg)
|
||||
}
|
||||
}
|
||||
|
||||
void Simulator::handle_message_optical_flow(const mavlink_message_t *msg)
|
||||
void SimulatorMavlink::handle_message_optical_flow(const mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_hil_optical_flow_t flow;
|
||||
mavlink_msg_hil_optical_flow_decode(msg, &flow);
|
||||
@@ -945,7 +949,7 @@ void Simulator::handle_message_optical_flow(const mavlink_message_t *msg)
|
||||
_sensor_optical_flow_pub.publish(sensor_optical_flow);
|
||||
}
|
||||
|
||||
void Simulator::handle_message_rc_channels(const mavlink_message_t *msg)
|
||||
void SimulatorMavlink::handle_message_rc_channels(const mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_rc_channels_t rc_channels;
|
||||
mavlink_msg_rc_channels_decode(msg, &rc_channels);
|
||||
@@ -979,7 +983,7 @@ void Simulator::handle_message_rc_channels(const mavlink_message_t *msg)
|
||||
_input_rc_pub.publish(rc_input);
|
||||
}
|
||||
|
||||
void Simulator::handle_message_vision_position_estimate(const mavlink_message_t *msg)
|
||||
void SimulatorMavlink::handle_message_vision_position_estimate(const mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_vision_position_estimate_t vpe;
|
||||
mavlink_msg_vision_position_estimate_decode(msg, &vpe);
|
||||
@@ -1016,7 +1020,7 @@ void Simulator::handle_message_vision_position_estimate(const mavlink_message_t
|
||||
_visual_odometry_pub.publish(odom);
|
||||
}
|
||||
|
||||
void Simulator::send_mavlink_message(const mavlink_message_t &aMsg)
|
||||
void SimulatorMavlink::send_mavlink_message(const mavlink_message_t &aMsg)
|
||||
{
|
||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
|
||||
uint16_t bufLen = 0;
|
||||
@@ -1037,13 +1041,13 @@ void Simulator::send_mavlink_message(const mavlink_message_t &aMsg)
|
||||
}
|
||||
}
|
||||
|
||||
void *Simulator::sending_trampoline(void * /*unused*/)
|
||||
void *SimulatorMavlink::sending_trampoline(void * /*unused*/)
|
||||
{
|
||||
_instance->send();
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
void Simulator::send()
|
||||
void SimulatorMavlink::send()
|
||||
{
|
||||
#ifdef __PX4_DARWIN
|
||||
pthread_setname_np("sim_send");
|
||||
@@ -1102,7 +1106,7 @@ void Simulator::send()
|
||||
orb_unsubscribe(_actuator_outputs_sub);
|
||||
}
|
||||
|
||||
void Simulator::request_hil_state_quaternion()
|
||||
void SimulatorMavlink::request_hil_state_quaternion()
|
||||
{
|
||||
mavlink_command_long_t cmd_long = {};
|
||||
mavlink_message_t message = {};
|
||||
@@ -1113,7 +1117,7 @@ void Simulator::request_hil_state_quaternion()
|
||||
send_mavlink_message(message);
|
||||
}
|
||||
|
||||
void Simulator::send_heartbeat()
|
||||
void SimulatorMavlink::send_heartbeat()
|
||||
{
|
||||
mavlink_heartbeat_t hb = {};
|
||||
mavlink_message_t message = {};
|
||||
@@ -1123,7 +1127,7 @@ void Simulator::send_heartbeat()
|
||||
send_mavlink_message(message);
|
||||
}
|
||||
|
||||
void Simulator::run()
|
||||
void SimulatorMavlink::run()
|
||||
{
|
||||
#ifdef __PX4_DARWIN
|
||||
pthread_setname_np("sim_rcv");
|
||||
@@ -1234,27 +1238,8 @@ void Simulator::run()
|
||||
fds[0].fd = _fd;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
#ifdef ENABLE_UART_RC_INPUT
|
||||
// setup serial connection to autopilot (used to get manual controls)
|
||||
int serial_fd = openUart(PIXHAWK_DEVICE, PIXHAWK_DEVICE_BAUD);
|
||||
|
||||
char serial_buf[1024];
|
||||
|
||||
if (serial_fd >= 0) {
|
||||
fds[1].fd = serial_fd;
|
||||
fds[1].events = POLLIN;
|
||||
fd_count++;
|
||||
|
||||
PX4_INFO("Start using %s for radio control input.", PIXHAWK_DEVICE);
|
||||
|
||||
} else {
|
||||
PX4_INFO("Not using %s for radio control input. Assuming joystick input via MAVLink.", PIXHAWK_DEVICE);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
// got data from simulator, now activate the sending thread
|
||||
pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, nullptr);
|
||||
pthread_create(&sender_thread, &sender_thread_attr, SimulatorMavlink::sending_trampoline, nullptr);
|
||||
pthread_attr_destroy(&sender_thread_attr);
|
||||
|
||||
mavlink_status_t mavlink_status = {};
|
||||
@@ -1292,125 +1277,10 @@ void Simulator::run()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef ENABLE_UART_RC_INPUT
|
||||
|
||||
// got data from PIXHAWK
|
||||
if (fd_count > 1 && fds[1].revents & POLLIN) {
|
||||
int len = ::read(serial_fd, serial_buf, sizeof(serial_buf));
|
||||
|
||||
if (len > 0) {
|
||||
mavlink_message_t msg;
|
||||
|
||||
mavlink_status_t serial_status = {};
|
||||
|
||||
for (int i = 0; i < len; ++i) {
|
||||
if (mavlink_parse_char(MAVLINK_COMM_1, serial_buf[i], &msg, &serial_status)) {
|
||||
handle_message(&msg);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef ENABLE_UART_RC_INPUT
|
||||
int openUart(const char *uart_name, int baud)
|
||||
{
|
||||
/* process baud rate */
|
||||
int speed;
|
||||
|
||||
switch (baud) {
|
||||
case 0: speed = B0; break;
|
||||
|
||||
case 50: speed = B50; break;
|
||||
|
||||
case 75: speed = B75; break;
|
||||
|
||||
case 110: speed = B110; break;
|
||||
|
||||
case 134: speed = B134; break;
|
||||
|
||||
case 150: speed = B150; break;
|
||||
|
||||
case 200: speed = B200; break;
|
||||
|
||||
case 300: speed = B300; break;
|
||||
|
||||
case 600: speed = B600; break;
|
||||
|
||||
case 1200: speed = B1200; break;
|
||||
|
||||
case 1800: speed = B1800; break;
|
||||
|
||||
case 2400: speed = B2400; break;
|
||||
|
||||
case 4800: speed = B4800; break;
|
||||
|
||||
case 9600: speed = B9600; break;
|
||||
|
||||
case 19200: speed = B19200; break;
|
||||
|
||||
case 38400: speed = B38400; break;
|
||||
|
||||
case 57600: speed = B57600; break;
|
||||
|
||||
case 115200: speed = B115200; break;
|
||||
|
||||
case 230400: speed = B230400; break;
|
||||
|
||||
case 460800: speed = B460800; break;
|
||||
|
||||
case 921600: speed = B921600; break;
|
||||
|
||||
default:
|
||||
PX4_ERR("Unsupported baudrate: %d", baud);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
/* open uart */
|
||||
int uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
|
||||
|
||||
if (uart_fd < 0) {
|
||||
return uart_fd;
|
||||
}
|
||||
|
||||
|
||||
/* Try to set baud rate */
|
||||
struct termios uart_config = {};
|
||||
|
||||
int termios_state;
|
||||
|
||||
/* Back up the original uart configuration to restore it after exit */
|
||||
if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 0) {
|
||||
PX4_ERR("tcgetattr failed for %s: %s\n", uart_name, strerror(errno));
|
||||
::close(uart_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* Set baud rate */
|
||||
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
|
||||
PX4_ERR("cfsetispeed or cfsetospeed failed for %s: %s\n", uart_name, strerror(errno));
|
||||
::close(uart_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Make raw
|
||||
cfmakeraw(&uart_config);
|
||||
|
||||
if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) {
|
||||
PX4_ERR("tcsetattr failed for %s: %s\n", uart_name, strerror(errno));
|
||||
::close(uart_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
return uart_fd;
|
||||
}
|
||||
#endif
|
||||
|
||||
void Simulator::check_failure_injections()
|
||||
void SimulatorMavlink::check_failure_injections()
|
||||
{
|
||||
vehicle_command_s vehicle_command;
|
||||
|
||||
@@ -1618,7 +1488,7 @@ void Simulator::check_failure_injections()
|
||||
}
|
||||
}
|
||||
|
||||
int Simulator::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavlink)
|
||||
int SimulatorMavlink::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavlink)
|
||||
{
|
||||
distance_sensor_s dist{};
|
||||
dist.timestamp = hrt_absolute_time();
|
||||
@@ -1694,3 +1564,116 @@ int Simulator::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavl
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int SimulatorMavlink::start(int argc, char *argv[])
|
||||
{
|
||||
_instance = new SimulatorMavlink();
|
||||
|
||||
if (_instance) {
|
||||
|
||||
if (argc == 5 && strcmp(argv[3], "-u") == 0) {
|
||||
_instance->set_ip(InternetProtocol::UDP);
|
||||
_instance->set_port(atoi(argv[4]));
|
||||
}
|
||||
|
||||
if (argc == 5 && strcmp(argv[3], "-c") == 0) {
|
||||
_instance->set_ip(InternetProtocol::TCP);
|
||||
_instance->set_port(atoi(argv[4]));
|
||||
}
|
||||
|
||||
if (argc == 6 && strcmp(argv[3], "-t") == 0) {
|
||||
PX4_INFO("using TCP on remote host %s port %s", argv[4], argv[5]);
|
||||
PX4_WARN("Please ensure port %s is not blocked by a firewall.", argv[5]);
|
||||
_instance->set_ip(InternetProtocol::TCP);
|
||||
_instance->set_tcp_remote_ipaddr(argv[4]);
|
||||
_instance->set_port(atoi(argv[5]));
|
||||
}
|
||||
|
||||
if (argc == 6 && strcmp(argv[3], "-h") == 0) {
|
||||
PX4_INFO("using TCP on remote host %s port %s", argv[4], argv[5]);
|
||||
PX4_WARN("Please ensure port %s is not blocked by a firewall.", argv[5]);
|
||||
_instance->set_ip(InternetProtocol::TCP);
|
||||
_instance->set_hostname(argv[4]);
|
||||
_instance->set_port(atoi(argv[5]));
|
||||
}
|
||||
|
||||
_instance->run();
|
||||
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
PX4_WARN("creation failed");
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
static void usage()
|
||||
{
|
||||
PX4_INFO("Usage: simulator_mavlink {start -[spt] [-u udp_port / -c tcp_port] |stop|status}");
|
||||
PX4_INFO("Start simulator: simulator_mavlink start");
|
||||
PX4_INFO("Connect using UDP: simulator_mavlink start -u udp_port");
|
||||
PX4_INFO("Connect using TCP: simulator_mavlink start -c tcp_port");
|
||||
PX4_INFO("Connect to a remote server using TCP: simulator_mavlink start -t ip_addr tcp_port");
|
||||
PX4_INFO("Connect to a remote server via hostname using TCP: simulator_mavlink start -h hostname tcp_port");
|
||||
}
|
||||
|
||||
__BEGIN_DECLS
|
||||
extern int simulator_mavlink_main(int argc, char *argv[]);
|
||||
__END_DECLS
|
||||
|
||||
int simulator_mavlink_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc > 1 && strcmp(argv[1], "start") == 0) {
|
||||
|
||||
if (g_sim_task >= 0) {
|
||||
PX4_WARN("Simulator already started");
|
||||
return 0;
|
||||
}
|
||||
|
||||
g_sim_task = px4_task_spawn_cmd("simulator_mavlink",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX,
|
||||
1500,
|
||||
SimulatorMavlink::start,
|
||||
argv);
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
|
||||
// We want to prevent the rest of the startup script from running until time
|
||||
// is initialized by the HIL_SENSOR messages from the simulator.
|
||||
while (true) {
|
||||
if (SimulatorMavlink::getInstance() && SimulatorMavlink::getInstance()->has_initialized()) {
|
||||
break;
|
||||
}
|
||||
|
||||
system_usleep(100);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
} else if (argc == 2 && strcmp(argv[1], "stop") == 0) {
|
||||
if (g_sim_task < 0) {
|
||||
PX4_WARN("Simulator not running");
|
||||
return 1;
|
||||
|
||||
} else {
|
||||
px4_task_delete(g_sim_task);
|
||||
g_sim_task = -1;
|
||||
}
|
||||
|
||||
} else if (argc == 2 && strcmp(argv[1], "status") == 0) {
|
||||
if (g_sim_task < 0) {
|
||||
PX4_WARN("Simulator not running");
|
||||
return 1;
|
||||
|
||||
} else {
|
||||
PX4_INFO("running");
|
||||
}
|
||||
|
||||
} else {
|
||||
usage();
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
+7
-8
@@ -113,10 +113,10 @@ static inline SensorSource operator &(A lhs, B rhs)
|
||||
);
|
||||
}
|
||||
|
||||
class Simulator : public ModuleParams
|
||||
class SimulatorMavlink : public ModuleParams
|
||||
{
|
||||
public:
|
||||
static Simulator *getInstance() { return _instance; }
|
||||
static SimulatorMavlink *getInstance() { return _instance; }
|
||||
|
||||
enum class InternetProtocol {
|
||||
TCP,
|
||||
@@ -127,7 +127,7 @@ public:
|
||||
|
||||
void set_ip(InternetProtocol ip) { _ip = ip; }
|
||||
void set_port(unsigned port) { _port = port; }
|
||||
void set_hostname(std::string hostname) { _hostname = hostname; }
|
||||
void set_hostname(const char *hostname) { _hostname = hostname; }
|
||||
void set_tcp_remote_ipaddr(char *tcp_remote_ipaddr) { _tcp_remote_ipaddr = tcp_remote_ipaddr; }
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
@@ -135,9 +135,9 @@ public:
|
||||
#endif
|
||||
|
||||
private:
|
||||
Simulator();
|
||||
SimulatorMavlink();
|
||||
|
||||
~Simulator()
|
||||
~SimulatorMavlink()
|
||||
{
|
||||
// free perf counters
|
||||
perf_free(_perf_sim_delay);
|
||||
@@ -161,7 +161,7 @@ private:
|
||||
|
||||
int publish_distance_topic(const mavlink_distance_sensor_t *dist);
|
||||
|
||||
static Simulator *_instance;
|
||||
static SimulatorMavlink *_instance;
|
||||
|
||||
// simulated sensor instances
|
||||
static constexpr uint8_t ACCEL_COUNT_MAX = 3;
|
||||
@@ -216,8 +216,8 @@ private:
|
||||
hrt_abstime _last_sim_timestamp{0};
|
||||
hrt_abstime _last_sitl_timestamp{0};
|
||||
|
||||
|
||||
void run();
|
||||
|
||||
void handle_message(const mavlink_message_t *msg);
|
||||
void handle_message_distance_sensor(const mavlink_message_t *msg);
|
||||
void handle_message_hil_gps(const mavlink_message_t *msg);
|
||||
@@ -243,7 +243,6 @@ private:
|
||||
|
||||
void actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *msg);
|
||||
|
||||
|
||||
// uORB publisher handlers
|
||||
uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
|
||||
uORB::Publication<vehicle_attitude_s> _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};
|
||||
@@ -0,0 +1,43 @@
|
||||
if(ENABLE_LOCKSTEP_SCHEDULER STREQUAL "no")
|
||||
|
||||
px4_add_git_submodule(TARGET git_flightgear_bridge PATH "${PX4_SOURCE_DIR}/Tools/simulation/flightgear/flightgear_bridge")
|
||||
|
||||
include(ExternalProject)
|
||||
ExternalProject_Add(flightgear_bridge
|
||||
SOURCE_DIR ${PX4_SOURCE_DIR}/Tools/simulation/flightgear/flightgear_bridge
|
||||
CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
|
||||
BINARY_DIR ${PX4_BINARY_DIR}/build_flightgear_bridge
|
||||
INSTALL_COMMAND ""
|
||||
DEPENDS git_flightgear_bridge
|
||||
USES_TERMINAL_CONFIGURE true
|
||||
USES_TERMINAL_BUILD true
|
||||
EXCLUDE_FROM_ALL true
|
||||
BUILD_ALWAYS 1
|
||||
)
|
||||
|
||||
# flighgear targets
|
||||
set(models
|
||||
rascal
|
||||
rascal-electric
|
||||
tf-g1
|
||||
tf-g2
|
||||
tf-r1
|
||||
)
|
||||
|
||||
# default flightgear target
|
||||
add_custom_target(flightgear
|
||||
COMMAND ${PX4_SOURCE_DIR}/Tools/simulation/flightgear/sitl_run.sh $<TARGET_FILE:px4> "rascal" ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
|
||||
WORKING_DIRECTORY ${SITL_WORKING_DIR}
|
||||
USES_TERMINAL
|
||||
DEPENDS px4 flightgear_bridge
|
||||
)
|
||||
|
||||
foreach(model ${models})
|
||||
add_custom_target(flightgear_${model}
|
||||
COMMAND ${PX4_SOURCE_DIR}/Tools/simulation/flightgear/sitl_run.sh $<TARGET_FILE:px4> ${model} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
|
||||
WORKING_DIRECTORY ${SITL_WORKING_DIR}
|
||||
USES_TERMINAL
|
||||
DEPENDS px4 flightgear_bridge
|
||||
)
|
||||
endforeach()
|
||||
endif()
|
||||
@@ -0,0 +1,171 @@
|
||||
# Estimate an appropriate number of parallel jobs
|
||||
cmake_host_system_information(RESULT AVAILABLE_PHYSICAL_MEMORY QUERY AVAILABLE_PHYSICAL_MEMORY)
|
||||
cmake_host_system_information(RESULT NUMBER_OF_LOGICAL_CORES QUERY NUMBER_OF_LOGICAL_CORES)
|
||||
|
||||
set(parallel_jobs 1)
|
||||
|
||||
if(NOT NUMBER_OF_LOGICAL_CORES)
|
||||
include(ProcessorCount)
|
||||
ProcessorCount(NUMBER_OF_LOGICAL_CORES)
|
||||
endif()
|
||||
|
||||
if(NOT AVAILABLE_PHYSICAL_MEMORY AND NUMBER_OF_LOGICAL_CORES GREATER_EQUAL 4)
|
||||
# Memory estimate unavailable, use N-2 jobs
|
||||
math(EXPR parallel_jobs "${NUMBER_OF_LOGICAL_CORES} - 2")
|
||||
endif()
|
||||
|
||||
if(AVAILABLE_PHYSICAL_MEMORY)
|
||||
# Allow an additional job for every 1.5GB of available physical memory
|
||||
math(EXPR parallel_jobs "${AVAILABLE_PHYSICAL_MEMORY}/(3*1024/2)")
|
||||
else()
|
||||
set(AVAILABLE_PHYSICAL_MEMORY "?")
|
||||
endif()
|
||||
|
||||
if(parallel_jobs GREATER NUMBER_OF_LOGICAL_CORES)
|
||||
set(parallel_jobs ${NUMBER_OF_LOGICAL_CORES})
|
||||
endif()
|
||||
|
||||
if(parallel_jobs LESS 1)
|
||||
set(parallel_jobs 1)
|
||||
endif()
|
||||
|
||||
message(DEBUG "${NUMBER_OF_LOGICAL_CORES} logical cores detected and ${AVAILABLE_PHYSICAL_MEMORY} megabytes of memory available.
|
||||
Limiting sitl_gazebo concurrent jobs to ${parallel_jobs}")
|
||||
|
||||
# project to build sitl_gazebo if necessary
|
||||
px4_add_git_submodule(TARGET git_gazebo PATH "${PX4_SOURCE_DIR}/Tools/simulation/gazebo/sitl_gazebo")
|
||||
include(ExternalProject)
|
||||
ExternalProject_Add(sitl_gazebo
|
||||
SOURCE_DIR ${PX4_SOURCE_DIR}/Tools/simulation/gazebo/sitl_gazebo
|
||||
CMAKE_ARGS
|
||||
-DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
|
||||
-DSEND_ODOMETRY_DATA=ON
|
||||
-DGENERATE_ROS_MODELS=ON
|
||||
BINARY_DIR ${PX4_BINARY_DIR}/build_gazebo
|
||||
INSTALL_COMMAND ""
|
||||
DEPENDS git_gazebo
|
||||
USES_TERMINAL_CONFIGURE true
|
||||
USES_TERMINAL_BUILD true
|
||||
EXCLUDE_FROM_ALL true
|
||||
BUILD_ALWAYS 1
|
||||
BUILD_COMMAND ${CMAKE_COMMAND} --build <BINARY_DIR> -- -j ${parallel_jobs}
|
||||
)
|
||||
|
||||
# create targets for each model/world/debugger combination
|
||||
|
||||
set(debuggers
|
||||
none
|
||||
gdb
|
||||
lldb
|
||||
valgrind
|
||||
callgrind
|
||||
)
|
||||
|
||||
set(models
|
||||
none
|
||||
believer
|
||||
boat
|
||||
cloudship
|
||||
glider
|
||||
iris
|
||||
iris_dual_gps
|
||||
iris_foggy_lidar
|
||||
iris_irlock
|
||||
iris_obs_avoid
|
||||
iris_opt_flow
|
||||
iris_opt_flow_mockup
|
||||
iris_rplidar
|
||||
iris_vision
|
||||
nxp_cupcar
|
||||
omnicopter
|
||||
plane
|
||||
plane_cam
|
||||
plane_catapult
|
||||
plane_lidar
|
||||
px4vision
|
||||
r1_rover
|
||||
rover
|
||||
shell
|
||||
standard_vtol
|
||||
standard_vtol_drop
|
||||
tailsitter
|
||||
techpod
|
||||
tiltrotor
|
||||
typhoon_h480
|
||||
uuv_bluerov2_heavy
|
||||
uuv_hippocampus
|
||||
)
|
||||
|
||||
set(worlds
|
||||
none
|
||||
baylands
|
||||
empty
|
||||
ksql_airport
|
||||
mcmillan_airfield
|
||||
sonoma_raceway
|
||||
warehouse
|
||||
windy
|
||||
yosemite
|
||||
)
|
||||
|
||||
foreach(debugger ${debuggers})
|
||||
foreach(model ${models})
|
||||
foreach(world ${worlds})
|
||||
if(world STREQUAL "none")
|
||||
if(debugger STREQUAL "none")
|
||||
if(model STREQUAL "none")
|
||||
set(_targ_name "gazebo")
|
||||
else()
|
||||
set(_targ_name "gazebo_${model}")
|
||||
endif()
|
||||
else()
|
||||
if(model STREQUAL "none")
|
||||
set(_targ_name "gazebo___${debugger}")
|
||||
else()
|
||||
set(_targ_name "gazebo_${model}_${debugger}")
|
||||
endif()
|
||||
endif()
|
||||
|
||||
add_custom_target(${_targ_name}
|
||||
COMMAND ${PX4_SOURCE_DIR}/Tools/simulation/gazebo/sitl_run.sh $<TARGET_FILE:px4> ${debugger} ${model} ${world} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
|
||||
WORKING_DIRECTORY ${SITL_WORKING_DIR}
|
||||
USES_TERMINAL
|
||||
DEPENDS px4 sitl_gazebo
|
||||
)
|
||||
else()
|
||||
if(debugger STREQUAL "none")
|
||||
if(model STREQUAL "none")
|
||||
set(_targ_name "gazebo___${world}")
|
||||
else()
|
||||
set(_targ_name "gazebo_${model}__${world}")
|
||||
endif()
|
||||
else()
|
||||
if(model STREQUAL "none")
|
||||
set(_targ_name "gazebo__${debugger}_${world}")
|
||||
else()
|
||||
set(_targ_name "gazebo_${model}_${debugger}_${world}")
|
||||
endif()
|
||||
endif()
|
||||
|
||||
add_custom_target(${_targ_name}
|
||||
COMMAND ${PX4_SOURCE_DIR}/Tools/simulation/gazebo/sitl_run.sh $<TARGET_FILE:px4> ${debugger} ${model} ${world} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
|
||||
WORKING_DIRECTORY ${SITL_WORKING_DIR}
|
||||
USES_TERMINAL
|
||||
DEPENDS px4 sitl_gazebo
|
||||
)
|
||||
endif()
|
||||
endforeach()
|
||||
endforeach()
|
||||
endforeach()
|
||||
|
||||
# mavsdk tests currently depend on sitl_gazebo
|
||||
ExternalProject_Add(mavsdk_tests
|
||||
SOURCE_DIR ${PX4_SOURCE_DIR}/test/mavsdk_tests
|
||||
CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
|
||||
BINARY_DIR ${PX4_BINARY_DIR}/mavsdk_tests
|
||||
INSTALL_COMMAND ""
|
||||
USES_TERMINAL_CONFIGURE true
|
||||
USES_TERMINAL_BUILD true
|
||||
EXCLUDE_FROM_ALL true
|
||||
BUILD_ALWAYS 1
|
||||
)
|
||||
@@ -0,0 +1,25 @@
|
||||
px4_add_git_submodule(TARGET git_jmavsim PATH "${PX4_SOURCE_DIR}/Tools/simulation/jmavsim/jMAVSim")
|
||||
|
||||
# create targets for each viewer/model/debugger combination
|
||||
set(debuggers
|
||||
none
|
||||
gdb
|
||||
lldb
|
||||
valgrind
|
||||
callgrind
|
||||
)
|
||||
|
||||
foreach(debugger ${debuggers})
|
||||
if(debugger STREQUAL "none")
|
||||
set(_targ_name "jmavsim")
|
||||
else()
|
||||
set(_targ_name "jmavsim_${debugger}")
|
||||
endif()
|
||||
|
||||
add_custom_target(${_targ_name}
|
||||
COMMAND ${PX4_SOURCE_DIR}/Tools/simulation/jmavsim/sitl_run.sh $<TARGET_FILE:px4> ${debugger} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
|
||||
WORKING_DIRECTORY ${SITL_WORKING_DIR}
|
||||
USES_TERMINAL
|
||||
DEPENDS px4 git_jmavsim
|
||||
)
|
||||
endforeach()
|
||||
@@ -0,0 +1,56 @@
|
||||
|
||||
px4_add_git_submodule(TARGET git_jsbsim_bridge PATH "${PX4_SOURCE_DIR}/Tools/simulation/jsbsim/jsbsim_bridge")
|
||||
|
||||
include(ExternalProject)
|
||||
ExternalProject_Add(jsbsim_bridge
|
||||
SOURCE_DIR ${PX4_SOURCE_DIR}/Tools/simulation/jsbsim/jsbsim_bridge
|
||||
CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
|
||||
BINARY_DIR ${PX4_BINARY_DIR}/build_jsbsim_bridge
|
||||
INSTALL_COMMAND ""
|
||||
DEPENDS git_jsbsim_bridge
|
||||
USES_TERMINAL_CONFIGURE true
|
||||
USES_TERMINAL_BUILD true
|
||||
EXCLUDE_FROM_ALL true
|
||||
BUILD_ALWAYS 1
|
||||
)
|
||||
|
||||
# jsbsim: create targets for jsbsim
|
||||
set(models
|
||||
rascal
|
||||
quadrotor_x
|
||||
hexarotor_x
|
||||
malolo
|
||||
)
|
||||
|
||||
set(worlds
|
||||
none
|
||||
LSZH
|
||||
)
|
||||
|
||||
# default jsbsim target
|
||||
add_custom_target(jsbsim
|
||||
COMMAND ${PX4_SOURCE_DIR}/Tools/simulation/jsbsim/sitl_run.sh $<TARGET_FILE:px4> "rascal" "LSZH" ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
|
||||
WORKING_DIRECTORY ${SITL_WORKING_DIR}
|
||||
USES_TERMINAL
|
||||
DEPENDS px4 jsbsim_bridge
|
||||
)
|
||||
|
||||
foreach(model ${models})
|
||||
foreach(world ${worlds})
|
||||
if(world STREQUAL "none")
|
||||
add_custom_target(jsbsim_${model}
|
||||
COMMAND ${PX4_SOURCE_DIR}/Tools/simulation/jsbsim/sitl_run.sh $<TARGET_FILE:px4> ${model} "LSZH" ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
|
||||
WORKING_DIRECTORY ${SITL_WORKING_DIR}
|
||||
USES_TERMINAL
|
||||
DEPENDS px4 jsbsim_bridge
|
||||
)
|
||||
else()
|
||||
add_custom_target(jsbsim_${model}__${world}
|
||||
COMMAND ${PX4_SOURCE_DIR}/Tools/simulation/jsbsim/sitl_run.sh $<TARGET_FILE:px4> ${model} ${world} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
|
||||
WORKING_DIRECTORY ${SITL_WORKING_DIR}
|
||||
USES_TERMINAL
|
||||
DEPENDS px4 jsbsim_bridge
|
||||
)
|
||||
endif()
|
||||
endforeach()
|
||||
endforeach()
|
||||
+21
-2
@@ -32,8 +32,8 @@
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__sih
|
||||
MAIN sih
|
||||
MODULE modules__simulation__simulator_sih
|
||||
MAIN simulator_sih
|
||||
COMPILE_FLAGS
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
SRCS
|
||||
@@ -46,3 +46,22 @@ px4_add_module(
|
||||
drivers_gyroscope
|
||||
drivers_magnetometer
|
||||
)
|
||||
|
||||
if(PX4_PLATFORM MATCHES "posix")
|
||||
# create targets for sihsim
|
||||
set(models
|
||||
airplane
|
||||
quadx
|
||||
xvert
|
||||
)
|
||||
|
||||
foreach(model ${models})
|
||||
add_custom_target(sihsim_${model}
|
||||
COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=${model} PX4_SIMULATOR=sihsim $<TARGET_FILE:px4>
|
||||
WORKING_DIRECTORY ${SITL_WORKING_DIR}
|
||||
USES_TERMINAL
|
||||
DEPENDS px4
|
||||
)
|
||||
endforeach()
|
||||
|
||||
endif()
|
||||
@@ -0,0 +1,12 @@
|
||||
menuconfig MODULES_SIMULATION_SIMULATOR_SIH
|
||||
bool "simulator_sih"
|
||||
default n
|
||||
---help---
|
||||
Enable support for simulator_sih
|
||||
|
||||
menuconfig USER_SIH
|
||||
bool "simulator_sih running as userspace module"
|
||||
default y
|
||||
depends on BOARD_PROTECTED && MODULES_SIMULATION_SIMULATOR_SIH
|
||||
---help---
|
||||
Put sih in userspace memory
|
||||
@@ -813,14 +813,14 @@ Most of the variables are declared global in the .hpp file to avoid stack overfl
|
||||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("sih", "simulation");
|
||||
PRINT_MODULE_USAGE_NAME("simulator_sih", "simulation");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int sih_main(int argc, char *argv[])
|
||||
extern "C" __EXPORT int simulator_sih_main(int argc, char *argv[])
|
||||
{
|
||||
return Sih::main(argc, argv);
|
||||
}
|
||||
@@ -1,76 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
option(ENABLE_UART_RC_INPUT "Enable RC Input from UART mavlink connection" OFF)
|
||||
|
||||
if(ENABLE_UART_RC_INPUT)
|
||||
if (APPLE)
|
||||
set(PIXHAWK_DEVICE "/dev/cu.usbmodem1")
|
||||
elseif (UNIX AND NOT APPLE)
|
||||
set(PIXHAWK_DEVICE "/dev/ttyACM0")
|
||||
elseif (WIN32)
|
||||
set(PIXHAWK_DEVICE "COM3")
|
||||
endif()
|
||||
|
||||
set(PIXHAWK_DEVICE_BAUD 115200)
|
||||
endif()
|
||||
configure_file(simulator_config.h.in simulator_config.h @ONLY)
|
||||
include_directories(${CMAKE_CURRENT_BINARY_DIR})
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__simulator
|
||||
MAIN simulator
|
||||
COMPILE_FLAGS
|
||||
-Wno-double-promotion
|
||||
-Wno-cast-align
|
||||
-Wno-address-of-packed-member # TODO: fix in c_library_v2
|
||||
INCLUDES
|
||||
${CMAKE_BINARY_DIR}/mavlink
|
||||
${CMAKE_BINARY_DIR}/mavlink/development
|
||||
SRCS
|
||||
simulator.cpp
|
||||
simulator_mavlink.cpp
|
||||
DEPENDS
|
||||
mavlink_c_generate
|
||||
conversion
|
||||
geo
|
||||
drivers_accelerometer
|
||||
drivers_gyroscope
|
||||
drivers_magnetometer
|
||||
)
|
||||
target_include_directories(modules__simulator INTERFACE ${CMAKE_BINARY_DIR}/mavlink)
|
||||
|
||||
add_subdirectory(battery_simulator)
|
||||
add_subdirectory(sensor_baro_sim)
|
||||
add_subdirectory(sensor_gps_sim)
|
||||
add_subdirectory(sensor_mag_sim)
|
||||
@@ -1,12 +0,0 @@
|
||||
menuconfig MODULES_SIMULATOR
|
||||
bool "simulator"
|
||||
default n
|
||||
---help---
|
||||
Enable support for simulator
|
||||
|
||||
menuconfig USER_SIMULATOR
|
||||
bool "simulator running as userspace module"
|
||||
default y
|
||||
depends on BOARD_PROTECTED && MODULES_SIMULATOR
|
||||
---help---
|
||||
Put simulator in userspace memory
|
||||
@@ -1,179 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 Mark Charlebois. All rights reserved.
|
||||
* Copyright (c) 2016-2019 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file simulator.cpp
|
||||
*
|
||||
* This module interfaces via MAVLink to a software in the loop simulator (SITL)
|
||||
* such as jMAVSim or Gazebo.
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/tasks.h>
|
||||
#include <px4_platform_common/time.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_board_led.h>
|
||||
|
||||
#include "simulator.h"
|
||||
|
||||
static px4_task_t g_sim_task = -1;
|
||||
|
||||
Simulator *Simulator::_instance = nullptr;
|
||||
|
||||
void Simulator::parameters_update(bool force)
|
||||
{
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated() || force) {
|
||||
// clear update
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
|
||||
// update parameters from storage
|
||||
updateParams();
|
||||
}
|
||||
}
|
||||
|
||||
int Simulator::start(int argc, char *argv[])
|
||||
{
|
||||
_instance = new Simulator();
|
||||
|
||||
if (_instance) {
|
||||
|
||||
if (argc == 5 && strcmp(argv[3], "-u") == 0) {
|
||||
_instance->set_ip(InternetProtocol::UDP);
|
||||
_instance->set_port(atoi(argv[4]));
|
||||
}
|
||||
|
||||
if (argc == 5 && strcmp(argv[3], "-c") == 0) {
|
||||
_instance->set_ip(InternetProtocol::TCP);
|
||||
_instance->set_port(atoi(argv[4]));
|
||||
}
|
||||
|
||||
if (argc == 6 && strcmp(argv[3], "-t") == 0) {
|
||||
PX4_INFO("Simulator using TCP on remote host %s port %s", argv[4], argv[5]);
|
||||
PX4_WARN("Please ensure port %s is not blocked by a firewall.", argv[5]);
|
||||
_instance->set_ip(InternetProtocol::TCP);
|
||||
_instance->set_tcp_remote_ipaddr(argv[4]);
|
||||
_instance->set_port(atoi(argv[5]));
|
||||
}
|
||||
|
||||
if (argc == 6 && strcmp(argv[3], "-h") == 0) {
|
||||
PX4_INFO("Simulator using TCP on remote host %s port %s", argv[4], argv[5]);
|
||||
PX4_WARN("Please ensure port %s is not blocked by a firewall.", argv[5]);
|
||||
_instance->set_ip(InternetProtocol::TCP);
|
||||
_instance->set_hostname(argv[4]);
|
||||
_instance->set_port(atoi(argv[5]));
|
||||
}
|
||||
|
||||
_instance->run();
|
||||
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
PX4_WARN("Simulator creation failed");
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
static void usage()
|
||||
{
|
||||
PX4_INFO("Usage: simulator {start -[spt] [-u udp_port / -c tcp_port] |stop|status}");
|
||||
PX4_INFO("Start simulator: simulator start");
|
||||
PX4_INFO("Connect using UDP: simulator start -u udp_port");
|
||||
PX4_INFO("Connect using TCP: simulator start -c tcp_port");
|
||||
PX4_INFO("Connect to a remote server using TCP: simulator start -t ip_addr tcp_port");
|
||||
PX4_INFO("Connect to a remote server via hostname using TCP: simulator start -h hostname tcp_port");
|
||||
}
|
||||
|
||||
__BEGIN_DECLS
|
||||
extern int simulator_main(int argc, char *argv[]);
|
||||
__END_DECLS
|
||||
|
||||
|
||||
int simulator_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc > 1 && strcmp(argv[1], "start") == 0) {
|
||||
|
||||
if (g_sim_task >= 0) {
|
||||
PX4_WARN("Simulator already started");
|
||||
return 0;
|
||||
}
|
||||
|
||||
g_sim_task = px4_task_spawn_cmd("simulator",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX,
|
||||
1500,
|
||||
Simulator::start,
|
||||
argv);
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
|
||||
// We want to prevent the rest of the startup script from running until time
|
||||
// is initialized by the HIL_SENSOR messages from the simulator.
|
||||
while (true) {
|
||||
if (Simulator::getInstance() && Simulator::getInstance()->has_initialized()) {
|
||||
break;
|
||||
}
|
||||
|
||||
system_usleep(100);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
} else if (argc == 2 && strcmp(argv[1], "stop") == 0) {
|
||||
if (g_sim_task < 0) {
|
||||
PX4_WARN("Simulator not running");
|
||||
return 1;
|
||||
|
||||
} else {
|
||||
px4_task_delete(g_sim_task);
|
||||
g_sim_task = -1;
|
||||
}
|
||||
|
||||
} else if (argc == 2 && strcmp(argv[1], "status") == 0) {
|
||||
if (g_sim_task < 0) {
|
||||
PX4_WARN("Simulator not running");
|
||||
return 1;
|
||||
|
||||
} else {
|
||||
PX4_INFO("running");
|
||||
}
|
||||
|
||||
} else {
|
||||
usage();
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,39 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016 Anton Matosov. 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
|
||||
|
||||
#cmakedefine ENABLE_UART_RC_INPUT
|
||||
|
||||
#cmakedefine PIXHAWK_DEVICE "@PIXHAWK_DEVICE@"
|
||||
#cmakedefine PIXHAWK_DEVICE_BAUD @PIXHAWK_DEVICE_BAUD@
|
||||
@@ -1,40 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2015 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file simulator_params.c
|
||||
*
|
||||
* Parameters of software in the loop
|
||||
*
|
||||
* @author Mohamed Abdelkader <mohamedashraf123@gmail.com>
|
||||
*/
|
||||
Reference in New Issue
Block a user