dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
Beat Küng 2018-08-11 14:09:20 +02:00 committed by Daniel Agar
parent a00f7ebf47
commit e6b9806ee1
9 changed files with 37 additions and 233 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480.post
View File

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

33
ROMFS/px4fmu_common/init.d-posix/rcS
View File

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

42
Tools/sitl_multiple_run.sh
View File

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

4
launch/multi_uav_mavros_sitl.launch
View File

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

85
posix-configs/SITL/init/ekf2/iris_1
View File

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

85
posix-configs/SITL/init/ekf2/iris_2
View File

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

2
src/modules/simulator/simulator.cpp
View File

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

6
src/modules/simulator/simulator_mavlink.cpp
View File

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

7
src/modules/simulator/simulator_params.c
View File

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