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:
Daniel Agar
2022-08-22 11:00:03 -04:00
parent 6b2509cbba
commit 4040e4cdf2
200 changed files with 1778 additions and 2806 deletions
-5
View File
@@ -1,5 +0,0 @@
menuconfig MODULES_IGNITION_SIMULATOR
bool "ignition_simulator"
default n
---help---
Enable support for ignition_simulator
-12
View File
@@ -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
+15
View File
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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);
}
@@ -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
@@ -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:
@@ -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
@@ -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;
}
@@ -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()
@@ -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);
}
-76
View File
@@ -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)
-12
View File
@@ -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
-179
View File
@@ -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@
-40
View File
@@ -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>
*/