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.

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 10 -s MOUNT_ORIENTATION -u $udp_gcs_port_local
-mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14557
+mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_offboard_port_local
-mavlink stream -r 10 -s MOUNT_ORIENTATION -u 14557
+mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_offboard_port_local

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
+# API/Offboard link
-mavlink start -x -u 14557 -r 4000000 -m onboard -o 14540
+mavlink start -x -u $udp_offboard_port_local -r 4000000 -m onboard -o $udp_offboard_port_remote
-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
 
 # execute autostart post script if any
 [ -e "$autostart_file".post ] && sh "$autostart_file".post

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
-
+[ -n "$1" ] && sitl_num="$1"
-sim_port=15019
-mav_port=15010
-mav_port2=15011
-
-mav_oport=15015
-mav_oport2=15016
-
-port_step=10
 
 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=0
-n=1
+while [ $n -lt $sitl_num ]; do
-while [ $n -le $sitl_num ]; do
+	working_dir="$build_path/instance_$n"
-	working_dir="instance_$n"
+	[ ! -d "$working_dir" ] && mkdir -p "$working_dir"
-	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
 
 	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

launch/multi_uav_mavros_sitl.launch

@@ -34,7 +34,7 @@
             <arg name="Y" value="0"/>
             <arg name="vehicle" value="$(arg vehicle)"/>
             <arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_$(arg ID)"/>
-            <arg name="mavlink_udp_port" value="14560"/>
+            <arg name="mavlink_udp_port" value="14561"/>
             <arg name="ID" value="$(arg ID)"/>
         </include>
         <!-- MAVROS -->
@@ -49,7 +49,7 @@
     <group ns="uav2">
         <!-- MAVROS and vehicle configs -->
         <arg name="ID" value="2"/>
-        <arg name="fcu_url" default="udp://:14541@localhost:14559"/>
+        <arg name="fcu_url" default="udp://:14541@localhost:14558"/>
         <!-- PX4 SITL and vehicle spawn -->
         <include file="$(find px4)/launch/single_vehicle_spawn.launch">
             <arg name="x" value="1"/>

posix-configs/SITL/init/ekf2/iris_1

@@ -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

posix-configs/SITL/init/ekf2/iris_2

@@ -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

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) {

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;

src/modules/simulator/simulator_params.c

@@ -40,13 +40,6 @@
  */
 #include <parameters/param.h>
 
-/**
- * Simulator UDP port
- *
- * @group SITL
- */
-PARAM_DEFINE_INT32(SITL_UDP_PRT, 14560);
-
 /**
  * Simulator Battery drain interval
  *