From cd1e04d43cb9c2a2f9110ac8a1808d58fcc24124 Mon Sep 17 00:00:00 2001 From: Mohamed Abdelkader Zahana Date: Sat, 13 May 2017 22:51:46 +0300 Subject: [PATCH] support of multi uav simulation in SITL --- launch/multi_uav_mavros_sitl.launch | 97 +++++++++++++++++++++ launch/single_vehcile_spawn.launch | 33 +++++++ posix-configs/SITL/init/ekf2/iris_1 | 80 +++++++++++++++++ posix-configs/SITL/init/ekf2/iris_2 | 80 +++++++++++++++++ posix-configs/SITL/init/lpe/iris_1 | 81 +++++++++++++++++ posix-configs/SITL/init/lpe/iris_2 | 81 +++++++++++++++++ src/modules/simulator/simulator_mavlink.cpp | 4 +- src/modules/simulator/simulator_params.c | 48 ++++++++++ 8 files changed, 503 insertions(+), 1 deletion(-) create mode 100644 launch/multi_uav_mavros_sitl.launch create mode 100644 launch/single_vehcile_spawn.launch create mode 100644 posix-configs/SITL/init/ekf2/iris_1 create mode 100644 posix-configs/SITL/init/ekf2/iris_2 create mode 100644 posix-configs/SITL/init/lpe/iris_1 create mode 100644 posix-configs/SITL/init/lpe/iris_2 create mode 100644 src/modules/simulator/simulator_params.c diff --git a/launch/multi_uav_mavros_sitl.launch b/launch/multi_uav_mavros_sitl.launch new file mode 100644 index 0000000000..ebedfc3c3e --- /dev/null +++ b/launch/multi_uav_mavros_sitl.launch @@ -0,0 +1,97 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/single_vehcile_spawn.launch b/launch/single_vehcile_spawn.launch new file mode 100644 index 0000000000..fcf7f07ff8 --- /dev/null +++ b/launch/single_vehcile_spawn.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/posix-configs/SITL/init/ekf2/iris_1 b/posix-configs/SITL/init/ekf2/iris_1 new file mode 100644 index 0000000000..95a165b41d --- /dev/null +++ b/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 diff --git a/posix-configs/SITL/init/ekf2/iris_2 b/posix-configs/SITL/init/ekf2/iris_2 new file mode 100644 index 0000000000..48d6a9a3de --- /dev/null +++ b/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 diff --git a/posix-configs/SITL/init/lpe/iris_1 b/posix-configs/SITL/init/lpe/iris_1 new file mode 100644 index 0000000000..4421f31e97 --- /dev/null +++ b/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 diff --git a/posix-configs/SITL/init/lpe/iris_2 b/posix-configs/SITL/init/lpe/iris_2 new file mode 100644 index 0000000000..ae5afe9e41 --- /dev/null +++ b/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 diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 3a25087aa3..4b3aae08e6 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/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 diff --git a/src/modules/simulator/simulator_params.c b/src/modules/simulator/simulator_params.c new file mode 100644 index 0000000000..818233fe3b --- /dev/null +++ b/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 + */ +#include + +/** + * Simulator UDP port + * + * @group SITL + */ +PARAM_DEFINE_INT32(SITL_UDP_PRT, 14560);