From 35e93c24e85ad87b9e8897f5dd3f424e9ad10a6a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 19 Sep 2016 20:08:35 +0200
Subject: [PATCH] fix sitl_multiple_run.sh: adapt to path updates

---
 Tools/sitl_multiple_run.sh                 | 54 +++++++++++-----------
 posix-configs/SITL/init/ekf2/multiple_iris | 45 ++++++++++--------
 2 files changed, 55 insertions(+), 44 deletions(-)

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_