diff --git a/Tools/sitl_multiple_run.sh b/Tools/sitl_multiple_run.sh index f306679edd..0626ed5ea4 100755 --- a/Tools/sitl_multiple_run.sh +++ b/Tools/sitl_multiple_run.sh @@ -1,4 +1,6 @@ #!/bin/bash +# run multiple instances of the 'px4' binary, but w/o starting the simulator. +# It assumes px4 is already built, with 'make posix_sitl_default' sitl_num=2 @@ -11,42 +13,42 @@ mav_oport2=15016 port_step=10 -src_path=`pwd` +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +src_path="$SCRIPT_DIR/.." -rc_script="posix-configs/SITL/init/rcS_multiple" +rc_script="posix-configs/SITL/init/ekf2/multiple_iris" build_path=${src_path}/build_posix_sitl_default +echo "killing running instances" pkill px4 -sleep 2 +sleep 1 -cd $build_path/src/firmware/posix +cd $build_path user=`whoami` n=1 while [ $n -le $sitl_num ]; do - if [ ! -d $n ]; then - mkdir -p $n - cd $n + working_dir="instance_$n" + if [ ! -d $working_dir ]; then + mkdir -p "$working_dir" + pushd "$working_dir" &>/dev/null - mkdir -p rootfs/fs/microsd - mkdir -p rootfs/eeprom - touch rootfs/eeprom/parameters + # 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 - cp ${src_path}/ROMFS/px4fmu_common/mixers/quad_w.main.mix ./ - cat ${src_path}/${rc_script}_gazebo_iris | 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 - cd ../ - fi + pushd "$working_dir" &>/dev/null + echo "starting instance $n in $(pwd)" + sudo -b -u $user ../src/firmware/posix/px4 -d "$src_path" rcS >out.log 2>err.log + popd &>/dev/null - cd $n - - sudo -b -u $user ../px4 -d rcS >out.log 2>err.log - - cd ../ - - 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)) + 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/posix-configs/SITL/init/ekf2/multiple_iris b/posix-configs/SITL/init/ekf2/multiple_iris index f405688c5b..17b7674b64 100644 --- a/posix-configs/SITL/init/ekf2/multiple_iris +++ b/posix-configs/SITL/init/ekf2/multiple_iris @@ -1,5 +1,4 @@ uorb start -simulator start -s -u _SIMPORT_ param load param set MAV_TYPE 2 param set SYS_AUTOSTART 4010 @@ -19,23 +18,32 @@ param set CAL_ACC0_YSCALE 1.01 param set CAL_ACC0_ZSCALE 1.01 param set CAL_ACC1_XOFF 0.01 param set CAL_MAG0_XOFF 0.01 +param set SENS_BOARD_ROT 0 +param set SENS_BOARD_X_OFF 0.000001 +param set COM_RC_IN_MODE 1 +param set NAV_DLL_ACT 2 +param set COM_DISARM_LAND 3 +param set NAV_ACC_RAD 2.0 +param set COM_OF_LOSS_T 5 +param set COM_OBL_ACT 2 +param set COM_OBL_RC_ACT 0 +param set RTL_RETURN_ALT 30.0 +param set RTL_DESCEND_ALT 5.0 +param set RTL_LAND_DELAY 5 +param set MIS_TAKEOFF_ALT 2.5 param set MC_ROLLRATE_P 0.2 param set MC_PITCHRATE_P 0.2 param set MC_PITCH_P 6 param set MC_ROLL_P 6 -param set MPC_XY_P 0.15 -param set MPC_XY_VEL_P 0.05 -param set MPC_XY_VEL_D 0.005 -param set MPC_XY_FF 0.1 -param set SENS_BOARD_ROT 0 -param set COM_RC_IN_MODE 1 -param set NAV_ACC_RAD 2.0 -param set RTL_RETURN_ALT 30.0 -param set RTL_DESCEND_ALT 10.0 +param set MPC_HOLD_MAX_Z 2.0 +param set MPC_HOLD_XY_DZ 0.1 +param set MPC_Z_VEL_P 0.6 +param set MPC_Z_VEL_I 0.15 param set EKF2_GBIAS_INIT 0.01 param set EKF2_ANGERR_INIT 0.01 param set EKF2_MAG_TYPE 1 replay tryapplyparams +simulator start -s -u _SIMPORT_ rgbledsim start tone_alarm start gyrosim start @@ -52,14 +60,15 @@ navigator start ekf2 start mc_pos_control start mc_att_control start -mixer load /dev/pwm_output0 quad_w.main.mix -mavlink start -u _MAVPORT_ -r 2000000 -o _MAVOPORT_ -mavlink start -u _MAVPORT2_ -r 2000000 -m onboard -o _MAVOPORT2_ -mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u _MAVPORT_ -mavlink stream -r 80 -s LOCAL_POSITION_NED -u _MAVPORT_ -mavlink stream -r 80 -s GLOBAL_POSITION_INT -u _MAVPORT_ -mavlink stream -r 80 -s ATTITUDE -u _MAVPORT_ -mavlink stream -r 80 -s ATTITUDE_TARGET -u _MAVPORT_ +mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix +mavlink start -u _MAVPORT_ -r 4000000 -o _MAVOPORT_ +mavlink start -u _MAVPORT2_ -r 4000000 -m onboard -o _MAVOPORT2_ +mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u _MAVPORT_ +mavlink stream -r 50 -s LOCAL_POSITION_NED -u _MAVPORT_ +mavlink stream -r 50 -s GLOBAL_POSITION_INT -u _MAVPORT_ +mavlink stream -r 50 -s ATTITUDE -u _MAVPORT_ +mavlink stream -r 50 -s ATTITUDE_QUATERNION -u _MAVPORT_ +mavlink stream -r 50 -s ATTITUDE_TARGET -u _MAVPORT_ mavlink stream -r 20 -s RC_CHANNELS -u _MAVPORT_ mavlink stream -r 250 -s HIGHRES_IMU -u _MAVPORT_ mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u _MAVPORT_