support of multi uav simulation in SITL

launch/multi_uav_mavros_sitl.launch

@@ -0,0 +1,97 @@
+<launch>
+
+    <!-- MAVROS posix SITL environment launch script -->
+
+    <arg name="debug" default="false"/>
+    <arg name="verbose" default="false"/>
+    <arg name="paused" default="false"/>
+
+    <arg name="est" default="ekf2"/>
+    <arg name="vehicle" default="iris"/>
+    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
+
+    <arg name="headless" default="false"/>
+    <arg name="gui" default="true"/>
+    <arg name="ns" default="/"/>
+
+    <arg name="pluginlists_yaml" default="$(find mavros)/launch/px4_pluginlists.yaml" />
+    <arg name="config_yaml" default="$(find mavros)/launch/px4_config.yaml" />
+
+    <!-- Load world -->
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="headless" value="$(arg headless)"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="world_name" value="$(arg world)" />
+        <arg name="debug" value="$(arg debug)" />
+        <arg name="verbose" value="$(arg verbose)" />
+        <arg name="paused" value="$(arg paused)" />
+    </include>
+
+    <!-- UAV1 iris_1-->
+    <group ns="uav1">
+        <arg name="fcu_url" default="udp://:14540@localhost:14557"/>
+        <arg name="gcs_url" value=""/>
+        <arg name="tgt_system" value="1"/> 
+        <arg name="tgt_component" value="1"/>
+        <arg name="rcS1" default="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_1"/>
+        <arg name="ID" value="1"/>
+
+        <include file="$(find px4)/launch/single_vehcile_spawn.launch">
+            <arg name="x" value="0"/>
+            <arg name="y" value="0"/>
+            <arg name="z" value="0"/>
+            <arg name="R" value="0"/>
+            <arg name="P" value="0"/>
+            <arg name="Y" value="0"/>
+            <arg name="vehicle" value="$(arg vehicle)"/>
+            <arg name="rcS" value="$(arg rcS1)"/>
+            <arg name="mavlink_udp_port" value="14560"/>
+            <arg name="ID" value="$(arg ID)"/>
+        </include>
+
+        <include file="$(find mavros)/launch/node.launch">
+            <arg name="pluginlists_yaml" value="$(arg pluginlists_yaml)" />
+            <arg name="config_yaml" value="$(arg config_yaml)" />
+
+            <arg name="fcu_url" value="$(arg fcu_url)" />
+            <arg name="gcs_url" value="$(arg gcs_url)" />
+            <arg name="tgt_system" value="$(arg tgt_system)" />
+            <arg name="tgt_component" value="$(arg tgt_component)" />
+        </include>
+    </group>
+
+    <!-- UAV2 iris_2 -->
+    <group ns="uav2">
+        <arg name="fcu_url" default="udp://:14541@localhost:14559"/>
+        <arg name="gcs_url" value=""/>
+        <arg name="tgt_system" value="2"/> 
+        <arg name="tgt_component" value="1"/>
+        <arg name="rcS2" default="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_2"/>
+        <arg name="ID" value="2"/>
+
+        <include file="$(find px4)/launch/single_vehcile_spawn.launch">
+            <arg name="x" value="1"/>
+            <arg name="y" value="0"/>
+            <arg name="z" value="0"/>
+            <arg name="R" value="0"/>
+            <arg name="P" value="0"/>
+            <arg name="Y" value="0"/>
+            <arg name="vehicle" value="$(arg vehicle)"/>
+            <arg name="rcS" value="$(arg rcS2)"/>
+            <arg name="mavlink_udp_port" value="14562"/>
+            <arg name="ID" value="$(arg ID)"/>
+        </include>
+
+        <include file="$(find mavros)/launch/node.launch">
+            <arg name="pluginlists_yaml" value="$(arg pluginlists_yaml)" />
+            <arg name="config_yaml" value="$(arg config_yaml)" />
+
+            <arg name="fcu_url" value="$(arg fcu_url)" />
+            <arg name="gcs_url" value="$(arg gcs_url)" />
+            <arg name="tgt_system" value="$(arg tgt_system)" />
+            <arg name="tgt_component" value="$(arg tgt_component)" />
+        </include>
+    </group>
+
+</launch>
+<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->

launch/single_vehcile_spawn.launch

@@ -0,0 +1,33 @@
+<launch>
+
+    <!-- Posix SITL environment launch script -->
+    <arg name="x" default="0"/>
+    <arg name="y" default="0"/>
+    <arg name="z" default="0"/>
+    <arg name="R" default="0"/>
+    <arg name="P" default="0"/>
+    <arg name="Y" default="0"/>
+    <arg name="est" default="lpe"/>
+    <arg name="vehicle" default="iris"/>
+    <arg name="ID" default="1"/>
+    <arg name="rcS" default="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_$(arg ID)"/>
+    <arg name="mavlink_udp_port" default="14560" />
+
+
+
+    <arg name="cmd" default="$(find xacro)/xacro.py $(find px4)/Tools/sitl_gazebo/models/rotors_description/urdf/$(arg vehicle)_base.xacro rotors_description_dir:=$(find px4)/Tools/sitl_gazebo/models/rotors_description mavlink_udp_port:=$(arg mavlink_udp_port) > $(arg vehicle)_$(arg ID).urdf ; 'gz sdf -p $(arg vehicle)_$(arg ID).urdf'" />
+
+
+    <param command="$(arg cmd)" name="$(arg vehicle)_$(arg ID)_sdf" />
+
+    <node name="sitl_$(arg ID)" pkg="px4" type="px4" output="screen"
+        args="$(find px4) $(arg rcS)">
+    </node>
+
+    <node name="$(arg vehicle)_$(arg ID)_spawn" output="screen" pkg="gazebo_ros" type="spawn_model"
+        args="-sdf -param $(arg vehicle)_$(arg ID)_sdf -model $(arg vehicle)_$(arg ID) -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)" respawn="false"/>
+
+
+</launch>
+
+<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->

posix-configs/SITL/init/ekf2/iris_1

@@ -0,0 +1,80 @@
+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 SENS_BOARD_ROT 0
+param set SENS_BOARD_X_OFF 0.000001
+param set SYS_AUTOSTART 4010
+param set SYS_MC_EST_GROUP 2
+param set SYS_RESTART_TYPE 2
+param set SITL_UDP_PRT 14560
+replay tryapplyparams
+simulator start -s
+tone_alarm start
+gyrosim start
+accelsim start
+barosim start
+adcsim start
+gpssim start
+pwm_out_sim mode_pwm
+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 ROMFS/px4fmu_common/mixers/quad_dc.main.mix
+mavlink start -u 14556 -r 4000000
+mavlink start -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

@@ -0,0 +1,80 @@
+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 SENS_BOARD_ROT 0
+param set SENS_BOARD_X_OFF 0.000001
+param set SYS_AUTOSTART 4010
+param set SYS_MC_EST_GROUP 2
+param set SYS_RESTART_TYPE 2
+param set SITL_UDP_PRT 14562
+replay tryapplyparams
+simulator start -s
+tone_alarm start
+gyrosim start
+accelsim start
+barosim start
+adcsim start
+gpssim start
+pwm_out_sim mode_pwm
+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 ROMFS/px4fmu_common/mixers/quad_dc.main.mix
+mavlink start -u 14558 -r 4000000
+mavlink start -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

posix-configs/SITL/init/lpe/iris_1

@@ -0,0 +1,81 @@
+uorb start
+param load
+param set MAV_SYS_ID 1
+param set MAV_TYPE 2
+param set SYS_AUTOSTART 4010
+param set SYS_RESTART_TYPE 2
+param set SYS_MC_EST_GROUP 1
+dataman start
+param set BAT_N_CELLS 3
+param set CAL_GYRO0_ID 2293768
+param set CAL_ACC0_ID 1376264
+param set CAL_ACC1_ID 1310728
+param set CAL_MAG0_ID 196616
+param set CAL_GYRO0_XOFF 0.01
+param set CAL_ACC0_XOFF 0.01
+param set CAL_ACC0_YOFF -0.01
+param set CAL_ACC0_ZOFF 0.01
+param set CAL_ACC0_XSCALE 1.01
+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_HOLD_MAX_Z 2.0
+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 SITL_UDP_PRT 14560
+# GPS only mode
+param set LPE_FUSION 145
+
+replay tryapplyparams
+simulator start -s
+tone_alarm start
+gyrosim start
+accelsim start
+barosim start
+adcsim start
+gpssim start
+pwm_out_sim mode_pwm
+sensors start
+commander start
+land_detector start multicopter
+navigator start
+attitude_estimator_q start
+local_position_estimator start
+mc_pos_control start
+mc_att_control start
+mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_dc.main.mix
+mavlink start -u 14556 -r 4000000
+mavlink start -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/lpe/iris_2

@@ -0,0 +1,81 @@
+uorb start
+param load
+param set MAV_SYS_ID 2
+param set MAV_TYPE 2
+param set SYS_AUTOSTART 4010
+param set SYS_RESTART_TYPE 2
+param set SYS_MC_EST_GROUP 1
+dataman start
+param set BAT_N_CELLS 3
+param set CAL_GYRO0_ID 2293768
+param set CAL_ACC0_ID 1376264
+param set CAL_ACC1_ID 1310728
+param set CAL_MAG0_ID 196616
+param set CAL_GYRO0_XOFF 0.01
+param set CAL_ACC0_XOFF 0.01
+param set CAL_ACC0_YOFF -0.01
+param set CAL_ACC0_ZOFF 0.01
+param set CAL_ACC0_XSCALE 1.01
+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_HOLD_MAX_Z 2.0
+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 SITL_UDP_PRT 14562
+# GPS only mode
+param set LPE_FUSION 145
+
+replay tryapplyparams
+simulator start -s
+tone_alarm start
+gyrosim start
+accelsim start
+barosim start
+adcsim start
+gpssim start
+pwm_out_sim mode_pwm
+sensors start
+commander start
+land_detector start multicopter
+navigator start
+attitude_estimator_q start
+local_position_estimator start
+mc_pos_control start
+mc_att_control start
+mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_dc.main.mix
+mavlink start -u 14558 -r 4000000
+mavlink start -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_mavlink.cpp

@@ -619,7 +619,9 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
 	struct sockaddr_in _myaddr;
 
 	if (udp_port < 1) {
-		udp_port = UDP_PORT;
+		int prt;
+		param_get(param_find("SITL_UDP_PRT"), &prt);
+		udp_port = prt;
 	}
 
 	// try to setup udp socket for communcation with simulator

src/modules/simulator/simulator_params.c

@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file simulator_params.c
+ *
+ * Parameters of software in the loop
+ *
+ * @author Mohamed Abdelkader <mohamedashraf123@gmail.com>
+ */
+#include <systemlib/param/param.h>
+
+/**
+ * Simulator UDP port
+ *
+ * @group SITL
+ */
+PARAM_DEFINE_INT32(SITL_UDP_PRT, 14560);