From e6b9806ee1b5140a032dc82f882740c7c54bb62a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sat, 11 Aug 2018 14:09:20 +0200 Subject: [PATCH] SITL: add multi-vehicle support to rcS This also removes the HIGHRES_IMU mavlink stream to the GCS. I don't see why it's needed and it adds noticeable CPU load. --- .../init.d-posix/6011_typhoon_h480.post | 6 +- ROMFS/px4fmu_common/init.d-posix/rcS | 33 ++++--- Tools/sitl_multiple_run.sh | 42 +++------ launch/multi_uav_mavros_sitl.launch | 4 +- posix-configs/SITL/init/ekf2/iris_1 | 85 ------------------- posix-configs/SITL/init/ekf2/iris_2 | 85 ------------------- src/modules/simulator/simulator.cpp | 2 +- src/modules/simulator/simulator_mavlink.cpp | 6 -- src/modules/simulator/simulator_params.c | 7 -- 9 files changed, 37 insertions(+), 233 deletions(-) delete mode 100644 posix-configs/SITL/init/ekf2/iris_1 delete mode 100644 posix-configs/SITL/init/ekf2/iris_2 diff --git a/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480.post b/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480.post index 5c133dc83f..78da0d2ff3 100644 --- a/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480.post +++ b/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480.post @@ -3,6 +3,6 @@ mixer append /dev/pwm_output0 etc/mixers/mount_legs.aux.mix mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 -mavlink stream -r 10 -s MOUNT_ORIENTATION -u 14556 -mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14557 -mavlink stream -r 10 -s MOUNT_ORIENTATION -u 14557 +mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_gcs_port_local +mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_offboard_port_local +mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_offboard_port_local diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index c51a9a8ba9..ca77ae6748 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -71,6 +71,12 @@ else set AUTOCNF yes fi +# multi-instance setup +param set MAV_SYS_ID $((1+$px4_instance)) +simulator_udp_port=$((14560+$px4_instance)) +udp_offboard_port_local=$((14557+$px4_instance)) +udp_offboard_port_remote=$((14540+$px4_instance)) +udp_gcs_port_local=$((14556+$px4_instance)) if [ $AUTOCNF == yes ] then @@ -137,7 +143,7 @@ sh "$autostart_file" dataman start replay tryapplyparams -simulator start -s +simulator start -s -u $simulator_udp_port tone_alarm start gyrosim start accelsim start @@ -166,19 +172,20 @@ fi # sh etc/init.d/rc.vehicle_setup +# GCS link +mavlink start -x -u $udp_gcs_port_local -r 4000000 +mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u $udp_gcs_port_local +mavlink stream -r 50 -s LOCAL_POSITION_NED -u $udp_gcs_port_local +mavlink stream -r 50 -s GLOBAL_POSITION_INT -u $udp_gcs_port_local +mavlink stream -r 50 -s ATTITUDE -u $udp_gcs_port_local +mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_gcs_port_local +mavlink stream -r 50 -s ATTITUDE_TARGET -u $udp_gcs_port_local +mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u $udp_gcs_port_local +mavlink stream -r 20 -s RC_CHANNELS -u $udp_gcs_port_local +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local -mavlink start -x -u 14556 -r 4000000 -mavlink start -x -u 14557 -r 4000000 -m onboard -o 14540 -mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556 -mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556 -mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14556 -mavlink stream -r 50 -s ATTITUDE -u 14556 -mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14556 -mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556 -mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556 -mavlink stream -r 20 -s RC_CHANNELS -u 14556 -mavlink stream -r 250 -s HIGHRES_IMU -u 14556 -mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +# API/Offboard link +mavlink start -x -u $udp_offboard_port_local -r 4000000 -m onboard -o $udp_offboard_port_remote # execute autostart post script if any [ -e "$autostart_file".post ] && sh "$autostart_file".post diff --git a/Tools/sitl_multiple_run.sh b/Tools/sitl_multiple_run.sh index 826931f3f1..55d58a00a7 100755 --- a/Tools/sitl_multiple_run.sh +++ b/Tools/sitl_multiple_run.sh @@ -2,21 +2,16 @@ # run multiple instances of the 'px4' binary, but w/o starting the simulator. # It assumes px4 is already built, with 'make posix_sitl_default' +# The simulator is expected to send to UDP port 14560+i for i in [0, N-1] +# For example jmavsim can be run like this: +#./Tools/jmavsim_run.sh -p 14561 + sitl_num=2 - -sim_port=15019 -mav_port=15010 -mav_port2=15011 - -mav_oport=15015 -mav_oport2=15016 - -port_step=10 +[ -n "$1" ] && sitl_num="$1" SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" src_path="$SCRIPT_DIR/.." -rc_script="posix-configs/SITL/init/ekf2/multiple_iris" build_path=${src_path}/build/posix_sitl_default echo "killing running instances" @@ -24,32 +19,17 @@ pkill -x px4 || true sleep 1 -cd $build_path +export PX4_SIM_MODEL=iris -user=`whoami` -n=1 -while [ $n -le $sitl_num ]; do - working_dir="instance_$n" - if [ ! -d $working_dir ]; then - mkdir -p "$working_dir" - pushd "$working_dir" &>/dev/null - - # replace template config with configured ports of current instance - cat ${src_path}/${rc_script} | sed s/_SIMPORT_/${sim_port}/ | \ - sed s/_MAVPORT_/${mav_port}/g | sed s/_MAVOPORT_/${mav_oport}/ | \ - sed s/_MAVPORT2_/${mav_port2}/ | sed s/_MAVOPORT2_/${mav_oport2}/ > rcS - popd &>/dev/null - fi +n=0 +while [ $n -lt $sitl_num ]; do + working_dir="$build_path/instance_$n" + [ ! -d "$working_dir" ] && mkdir -p "$working_dir" pushd "$working_dir" &>/dev/null echo "starting instance $n in $(pwd)" - sudo -b -u $user ../bin/px4 -i $n -d "$src_path" -s rcS >out.log 2>err.log + ../bin/px4 -i $n -d "$src_path/ROMFS/px4fmu_common" -s etc/init.d-posix/rcS >out.log 2>err.log & popd &>/dev/null n=$(($n + 1)) - sim_port=$(($sim_port + $port_step)) - mav_port=$(($mav_port + $port_step)) - mav_port2=$(($mav_port2 + $port_step)) - mav_oport=$(($mav_oport + $port_step)) - mav_oport2=$(($mav_oport2 + $port_step)) done diff --git a/launch/multi_uav_mavros_sitl.launch b/launch/multi_uav_mavros_sitl.launch index 5bea67c670..9de487a76a 100644 --- a/launch/multi_uav_mavros_sitl.launch +++ b/launch/multi_uav_mavros_sitl.launch @@ -34,7 +34,7 @@ - + @@ -49,7 +49,7 @@ - + diff --git a/posix-configs/SITL/init/ekf2/iris_1 b/posix-configs/SITL/init/ekf2/iris_1 deleted file mode 100644 index 7f39f4e0c4..0000000000 --- a/posix-configs/SITL/init/ekf2/iris_1 +++ /dev/null @@ -1,85 +0,0 @@ -#!/usr/bin/bash -# PX4 commands need the 'px4-' prefix in bash. -# (px4-alias.sh is expected to be in the PATH) -source px4-alias.sh - -uorb start -param load -dataman start -param set MAV_SYS_ID 1 -param set BAT_N_CELLS 3 -param set CAL_ACC0_ID 1376264 -param set CAL_ACC0_XOFF 0.01 -param set CAL_ACC0_XSCALE 1.01 -param set CAL_ACC0_YOFF -0.01 -param set CAL_ACC0_YSCALE 1.01 -param set CAL_ACC0_ZOFF 0.01 -param set CAL_ACC0_ZSCALE 1.01 -param set CAL_ACC1_ID 1310728 -param set CAL_ACC1_XOFF 0.01 -param set CAL_GYRO0_ID 2293768 -param set CAL_GYRO0_XOFF 0.01 -param set CAL_MAG0_ID 196616 -param set CAL_MAG0_XOFF 0.01 -param set COM_DISARM_LAND 3 -param set COM_OBL_ACT 2 -param set COM_OBL_RC_ACT 0 -param set COM_OF_LOSS_T 5 -param set COM_RC_IN_MODE 1 -param set EKF2_AID_MASK 1 -param set EKF2_ANGERR_INIT 0.01 -param set EKF2_GBIAS_INIT 0.01 -param set EKF2_HGT_MODE 0 -param set EKF2_MAG_TYPE 1 -param set MAV_TYPE 2 -param set MC_PITCH_P 6 -param set MC_PITCHRATE_P 0.2 -param set MC_ROLL_P 6 -param set MC_ROLLRATE_P 0.2 -param set MIS_TAKEOFF_ALT 2.5 -param set MPC_HOLD_MAX_Z 2.0 -param set MPC_Z_VEL_I 0.15 -param set MPC_Z_VEL_P 0.6 -param set NAV_ACC_RAD 2.0 -param set NAV_DLL_ACT 2 -param set RTL_DESCEND_ALT 5.0 -param set RTL_LAND_DELAY 5 -param set RTL_RETURN_ALT 30.0 -param set SDLOG_DIRS_MAX 7 -param set SENS_BOARD_ROT 0 -param set SENS_BOARD_X_OFF 0.000001 -param set SITL_UDP_PRT 14560 -param set SYS_AUTOSTART 4010 -param set SYS_MC_EST_GROUP 2 -param set SYS_RESTART_TYPE 2 -replay tryapplyparams -simulator start -s -tone_alarm start -gyrosim start -accelsim start -barosim start -gpssim start -pwm_out_sim start -sensors start -commander start -land_detector start multicopter -navigator start -ekf2 start -mc_pos_control start -mc_att_control start -mixer load /dev/pwm_output0 etc/mixers/quad_w.main.mix -mavlink start -x -u 14556 -r 4000000 -mavlink start -x -u 14557 -r 4000000 -m onboard -o 14540 -mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556 -mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556 -mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14556 -mavlink stream -r 50 -s ATTITUDE -u 14556 -mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14556 -mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556 -mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556 -mavlink stream -r 20 -s RC_CHANNELS -u 14556 -mavlink stream -r 250 -s HIGHRES_IMU -u 14556 -mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 -logger start -e -t -mavlink boot_complete -replay trystart diff --git a/posix-configs/SITL/init/ekf2/iris_2 b/posix-configs/SITL/init/ekf2/iris_2 deleted file mode 100644 index ab6955058e..0000000000 --- a/posix-configs/SITL/init/ekf2/iris_2 +++ /dev/null @@ -1,85 +0,0 @@ -#!/usr/bin/bash -# PX4 commands need the 'px4-' prefix in bash. -# (px4-alias.sh is expected to be in the PATH) -source px4-alias.sh - -uorb start -param load -dataman start -param set MAV_SYS_ID 2 -param set BAT_N_CELLS 3 -param set CAL_ACC0_ID 1376264 -param set CAL_ACC0_XOFF 0.01 -param set CAL_ACC0_XSCALE 1.01 -param set CAL_ACC0_YOFF -0.01 -param set CAL_ACC0_YSCALE 1.01 -param set CAL_ACC0_ZOFF 0.01 -param set CAL_ACC0_ZSCALE 1.01 -param set CAL_ACC1_ID 1310728 -param set CAL_ACC1_XOFF 0.01 -param set CAL_GYRO0_ID 2293768 -param set CAL_GYRO0_XOFF 0.01 -param set CAL_MAG0_ID 196616 -param set CAL_MAG0_XOFF 0.01 -param set COM_DISARM_LAND 3 -param set COM_OBL_ACT 2 -param set COM_OBL_RC_ACT 0 -param set COM_OF_LOSS_T 5 -param set COM_RC_IN_MODE 1 -param set EKF2_AID_MASK 1 -param set EKF2_ANGERR_INIT 0.01 -param set EKF2_GBIAS_INIT 0.01 -param set EKF2_HGT_MODE 0 -param set EKF2_MAG_TYPE 1 -param set MAV_TYPE 2 -param set MC_PITCH_P 6 -param set MC_PITCHRATE_P 0.2 -param set MC_ROLL_P 6 -param set MC_ROLLRATE_P 0.2 -param set MIS_TAKEOFF_ALT 2.5 -param set MPC_HOLD_MAX_Z 2.0 -param set MPC_Z_VEL_I 0.15 -param set MPC_Z_VEL_P 0.6 -param set NAV_ACC_RAD 2.0 -param set NAV_DLL_ACT 2 -param set RTL_DESCEND_ALT 5.0 -param set RTL_LAND_DELAY 5 -param set RTL_RETURN_ALT 30.0 -param set SDLOG_DIRS_MAX 7 -param set SENS_BOARD_ROT 0 -param set SENS_BOARD_X_OFF 0.000001 -param set SITL_UDP_PRT 14562 -param set SYS_AUTOSTART 4010 -param set SYS_MC_EST_GROUP 2 -param set SYS_RESTART_TYPE 2 -replay tryapplyparams -simulator start -s -tone_alarm start -gyrosim start -accelsim start -barosim start -gpssim start -pwm_out_sim start -sensors start -commander start -land_detector start multicopter -navigator start -ekf2 start -mc_pos_control start -mc_att_control start -mixer load /dev/pwm_output0 etc/mixers/quad_w.main.mix -mavlink start -x -u 14558 -r 4000000 -mavlink start -x -u 14559 -r 4000000 -m onboard -o 14541 -mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14558 -mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14558 -mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14558 -mavlink stream -r 50 -s ATTITUDE -u 14558 -mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14558 -mavlink stream -r 50 -s ATTITUDE_TARGET -u 14558 -mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14558 -mavlink stream -r 20 -s RC_CHANNELS -u 14558 -mavlink stream -r 250 -s HIGHRES_IMU -u 14558 -mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14558 -logger start -e -t -mavlink boot_complete -replay trystart diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 9beea9e2ff..9188830901 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -142,7 +142,7 @@ void Simulator::parameters_update(bool force) int Simulator::start(int argc, char *argv[]) { int ret = 0; - int udp_port = 0; + int udp_port = 14560; _instance = new Simulator(); if (_instance) { diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 105109842e..e2ad359e5b 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -669,12 +669,6 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port) // udp socket data struct sockaddr_in _myaddr; - if (udp_port < 1) { - int32_t prt; - param_get(param_find("SITL_UDP_PRT"), &prt); - udp_port = prt; - } - // try to setup udp socket for communcation with simulator memset((char *)&_myaddr, 0, sizeof(_myaddr)); _myaddr.sin_family = AF_INET; diff --git a/src/modules/simulator/simulator_params.c b/src/modules/simulator/simulator_params.c index 1df3ec7e63..65bf9aa89f 100644 --- a/src/modules/simulator/simulator_params.c +++ b/src/modules/simulator/simulator_params.c @@ -40,13 +40,6 @@ */ #include -/** - * Simulator UDP port - * - * @group SITL - */ -PARAM_DEFINE_INT32(SITL_UDP_PRT, 14560); - /** * Simulator Battery drain interval *