zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

support of multi uav simulation in SITL

sbg
Mohamed Abdelkader Zahana 8 years ago committed by Lorenz Meier
parent
669e580f0a
commit
cd1e04d43c
8 changed files with 503 additions and 1 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 97
      launch/multi_uav_mavros_sitl.launch
  2. 33
      launch/single_vehcile_spawn.launch
  3. 80
      posix-configs/SITL/init/ekf2/iris_1
  4. 80
      posix-configs/SITL/init/ekf2/iris_2
  5. 81
      posix-configs/SITL/init/lpe/iris_1
  6. 81
      posix-configs/SITL/init/lpe/iris_2
  7. 4
      src/modules/simulator/simulator_mavlink.cpp
  8. 48
      src/modules/simulator/simulator_params.c

97
launch/multi_uav_mavros_sitl.launch
Unescape View File

@ -0,0 +1,97 @@ @@ -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 : -->

33
launch/single_vehcile_spawn.launch
Unescape View File

@ -0,0 +1,33 @@ @@ -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 : -->

80
posix-configs/SITL/init/ekf2/iris_1
Unescape View File

@ -0,0 +1,80 @@ @@ -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

80
posix-configs/SITL/init/ekf2/iris_2
Unescape View File

@ -0,0 +1,80 @@ @@ -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

81
posix-configs/SITL/init/lpe/iris_1
Unescape View File

@ -0,0 +1,81 @@ @@ -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

81
posix-configs/SITL/init/lpe/iris_2
Unescape View File

@ -0,0 +1,81 @@ @@ -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

4
src/modules/simulator/simulator_mavlink.cpp
Unescape View File

@ -619,7 +619,9 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port) @@ -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

48
src/modules/simulator/simulator_params.c
Unescape View File

@ -0,0 +1,48 @@ @@ -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);
Write Preview
Loading…
Cancel
Save