From 458420f9cbaa55b84243d2f13f2e69d9236c37d8 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Thu, 17 Sep 2020 20:32:42 +0200 Subject: [PATCH] Add jsbsim bridge to enable jsbsim for px4 SITL on jsbsim (#15748) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add jsbsim bridge to enable jsbsim for px4 SITL/HIL on jsbsim This is a PX4 HIL/SITL integration into JSBSim. JSBSim is an open source flight dynamics model (http://jsbsim.sourceforge.net/) Currently there are three models available which is the rascal, quadrotor_x, hexarotor_x integrated into the bridge. The simulation can be run with the firmware with the following command for example ``` make px4_sitl jsbsim_rascal ``` The visualization is done flightgear and is done by the bridge sending UDP packets to flightgear. To disable the visualization `HEADLESS=1` when running the make command. The simulation can be configured through the configuration files under the `config` directory through a xml file. Senor configurations, The xml file name should match the name of the model. * Update Tools/sitl_run.sh Co-authored-by: Beat Küng Co-authored-by: Beat Küng --- .gitmodules | 3 + .../init.d-posix/10017_quadrotor_x | 12 +++ .../init.d-posix/10018_hexarotor_x | 35 ++++++++ Tools/jsbsim_bridge | 1 + Tools/setup_jsbsim.bash | 40 +++++++++ Tools/sitl_run.sh | 24 ++++- platforms/posix/cmake/sitl_target.cmake | 88 +++++++++++++++++++ 7 files changed, 202 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/10017_quadrotor_x create mode 100644 ROMFS/px4fmu_common/init.d-posix/10018_hexarotor_x create mode 160000 Tools/jsbsim_bridge create mode 100755 Tools/setup_jsbsim.bash diff --git a/.gitmodules b/.gitmodules index 8fb183eab4..73a955cfb5 100644 --- a/.gitmodules +++ b/.gitmodules @@ -48,3 +48,6 @@ [submodule "Tools/flightgear_bridge"] path = Tools/flightgear_bridge url = https://github.com/PX4/PX4-FlightGear-Bridge.git +[submodule "Tools/jsbsim_bridge"] + path = Tools/jsbsim_bridge + url = https://github.com/PX4/px4-jsbsim-bridge.git diff --git a/ROMFS/px4fmu_common/init.d-posix/10017_quadrotor_x b/ROMFS/px4fmu_common/init.d-posix/10017_quadrotor_x new file mode 100644 index 0000000000..c82eae8794 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/10017_quadrotor_x @@ -0,0 +1,12 @@ +#!/bin/sh +# +# @name Quadrotor SITL model for JSBSim +# +# @type Quadrotor Wide +# +# @maintainer Jaeyoung Lim +# + +sh /etc/init.d/rc.mc_defaults + +set MIXER quad_w diff --git a/ROMFS/px4fmu_common/init.d-posix/10018_hexarotor_x b/ROMFS/px4fmu_common/init.d-posix/10018_hexarotor_x new file mode 100644 index 0000000000..3ee8751da3 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/10018_hexarotor_x @@ -0,0 +1,35 @@ +#!/bin/sh +# +# @name Hexacopter SITL model for JSBSim +# +# @type Hexarotor x +# +# @maintainer Jaeyoung Lim +# + +sh /etc/init.d/rc.mc_defaults + +if [ $AUTOCNF = yes ] +then + param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_I 0.05 + param set MC_PITCH_P 6.0 + param set MC_ROLLRATE_P 0.15 + param set MC_ROLLRATE_I 0.1 + param set MC_ROLL_P 6.0 + param set MPC_XY_VEL_I_ACC 4 + param set MPC_XY_VEL_P_ACC 3 + + param set RTL_DESCEND_ALT 10 + param set RTL_LAND_DELAY 0 + + param set TRIG_INTERFACE 3 + param set TRIG_MODE 4 + param set MNT_MODE_IN 4 + param set MNT_DO_STAB 2 + param set MAV_PROTO_VER 2 +fi + +set MAV_TYPE 13 + +set MIXER hexa_x diff --git a/Tools/jsbsim_bridge b/Tools/jsbsim_bridge new file mode 160000 index 0000000000..efe19dbec7 --- /dev/null +++ b/Tools/jsbsim_bridge @@ -0,0 +1 @@ +Subproject commit efe19dbec7e82784a48c6b6e7710f54468c8a8d4 diff --git a/Tools/setup_jsbsim.bash b/Tools/setup_jsbsim.bash new file mode 100755 index 0000000000..e780477d36 --- /dev/null +++ b/Tools/setup_jsbsim.bash @@ -0,0 +1,40 @@ +#!/bin/bash +# +# Setup environment to make JSBSim visible to PX4. +# +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository + +if [ "$#" != 3 ]; then + echo -e "usage: source setup_jsbsim.bash src_dir build_dir model\n" + return 1 +fi + +SRC_DIR="$1" +BUILD_DIR="$2" +MODEL="$3" + +export FG_AIRCRAFT="${SRC_DIR}/Tools/jsbsim_bridge/models" + +# This is needed for aircraft namespace mapping +# Need more architectural discussions to make this more scalable +case "$MODEL" in + rascal) + AIRCRAFT_DIR="Rascal" + MODEL_NAME="Rascal110-JSBSim" + ;; + quadrotor_x) + AIRCRAFT_DIR="quadrotor_x" + MODEL_NAME="quadrotor_x" + ;; + hexarotor_x) + AIRCRAFT_DIR="hexarotor_x" + MODEL_NAME="hexarotor_x" + ;; + *) + echo "Unknown Model" + exit 1 + +esac + +export JSBSIM_AIRCRAFT_DIR="$AIRCRAFT_DIR" +export JSBSIM_AIRCRAFT_MODEL="$MODEL_NAME" diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 2ab9cbd1cc..0a5820fd29 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -58,7 +58,11 @@ fi if [ "$model" == "" ] || [ "$model" == "none" ]; then echo "empty model, setting iris as default" - model="iris" + if [ "$program" == "jsbsim" ]; then + model="rascal" + else + model="iris" + fi fi # kill process names that might stil @@ -146,6 +150,21 @@ elif [ "$program" == "flightgear" ] && [ -z "$no_sim" ]; then "${src_path}/Tools/flightgear_bridge/FG_run.py" "models/"${model}".json" 0 "${build_path}/build_flightgear_bridge/flightgear_bridge" 0 `./get_FGbridge_params.py "models/"${model}".json"` & FG_BRIDGE_PID=$! +elif [ "$program" == "jsbsim" ] && [ -z "$no_sim" ]; then + source "$src_path/Tools/setup_jsbsim.bash" "${src_path}" "${build_path}" ${model} + if [[ -n "$HEADLESS" ]]; then + echo "not running flightgear gui" + else + fgfs --fdm=null \ + --native-fdm=socket,in,60,,5550,udp \ + --aircraft=$JSBSIM_AIRCRAFT_MODEL \ + --airport=${world} \ + --disable-hud \ + --disable-ai-models &> /dev/null & + FGFS_PID=$! + fi + "${build_path}/build_jsbsim_bridge/jsbsim_bridge" "models/${JSBSIM_AIRCRAFT_DIR}" $JSBSIM_AIRCRAFT_MODEL ${model} "${src_path}/Tools/jsbsim_bridge/scene/${world}.xml" $HEADLESS 2> /dev/null & + JSBSIM_PID=$! fi pushd "$rootfs" >/dev/null @@ -199,4 +218,7 @@ elif [ "$program" == "gazebo" ]; then elif [ "$program" == "flightgear" ]; then kill $FG_BRIDGE_PID kill -9 `cat /tmp/px4fgfspid_0` +elif [ "$program" == "jsbsim" ]; then + kill $JSBSIM_PID + kill $FGFS_PID fi diff --git a/platforms/posix/cmake/sitl_target.cmake b/platforms/posix/cmake/sitl_target.cmake index f3ddfeb254..bf83a3ebbf 100644 --- a/platforms/posix/cmake/sitl_target.cmake +++ b/platforms/posix/cmake/sitl_target.cmake @@ -26,6 +26,7 @@ add_custom_target(run_config px4_add_git_submodule(TARGET git_gazebo PATH "${PX4_SOURCE_DIR}/Tools/sitl_gazebo") px4_add_git_submodule(TARGET git_jmavsim PATH "${PX4_SOURCE_DIR}/Tools/jMAVSim") px4_add_git_submodule(TARGET git_flightgear_bridge PATH "${PX4_SOURCE_DIR}/Tools/flightgear_bridge") +px4_add_git_submodule(TARGET git_jsbsim_bridge PATH "${PX4_SOURCE_DIR}/Tools/jsbsim_bridge") # Add support for external project building include(ExternalProject) @@ -73,6 +74,20 @@ ExternalProject_Add(flightgear_bridge BUILD_ALWAYS 1 ) +ExternalProject_Add(jsbsim_bridge + SOURCE_DIR ${PX4_SOURCE_DIR}/Tools/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 +) + # create targets for each viewer/model/debugger combination set(viewers none jmavsim gazebo) set(debuggers none ide gdb lldb ddd valgrind callgrind) @@ -161,6 +176,79 @@ foreach(viewer ${viewers}) endforeach() endforeach() +# create targets for jsbsim +set(models_jsbsim none rascal quadrotor_x hexarotor_x) +set(worlds_jsbsim none LSZH) +foreach(debugger ${debuggers}) + foreach(model ${models_jsbsim}) + foreach(world ${worlds_jsbsim}) + if (world STREQUAL "none") + if (debugger STREQUAL "none") + if (model STREQUAL "none") + set(_targ_name "jsbsim") + else() + set(_targ_name "jsbsim_${model}") + endif() + else() + if (model STREQUAL "none") + set(_targ_name "jsbsim__${debugger}_${world}") + else() + set(_targ_name "jsbsim_${model}_${debugger}_${world}") + endif() + endif() + + add_custom_target(${_targ_name} + COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh + $ + ${debugger} + jsbsim + ${model} + "LSZH" + ${PX4_SOURCE_DIR} + ${PX4_BINARY_DIR} + WORKING_DIRECTORY ${SITL_WORKING_DIR} + USES_TERMINAL + DEPENDS + logs_symlink + ) + list(APPEND all_posix_vmd_make_targets ${_targ_name}) + add_dependencies(${_targ_name} px4 jsbsim_bridge) + else() + if (debugger STREQUAL "none") + if (model STREQUAL "none") + set(_targ_name "jsbsim___${world}") + else() + set(_targ_name "jsbsim_${model}__${world}") + endif() + else() + if (model STREQUAL "none") + set(_targ_name "jsbsim___${debugger}_${world}") + else() + set(_targ_name "jsbsim_${model}_${debugger}_${world}") + endif() + endif() + + add_custom_target(${_targ_name} + COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh + $ + ${debugger} + jsbsim + ${model} + ${world} + ${PX4_SOURCE_DIR} + ${PX4_BINARY_DIR} + WORKING_DIRECTORY ${SITL_WORKING_DIR} + USES_TERMINAL + DEPENDS + logs_symlink + ) + list(APPEND all_posix_vmd_make_targets ${_targ_name}) + add_dependencies(${_targ_name} px4 jsbsim_bridge) + endif() + endforeach() + endforeach() +endforeach() + #add flighgear targets if( ENABLE_LOCKSTEP_SCHEDULER STREQUAL "no") set(models