How to to provide different missions to multiple UAVS in SITL

Hello All,

I have gone through the “[multi vehicle simulation”] (Multi-Vehicle Simulation · PX4 Developer Guide) in Gazebo documentation and was able to add one more IRIS model to multi_uav_mavros_sitl.launch and it works great. There are 3 IRISes in Gazebo and 3 Vehicles in QGC.

I attempted to provide 3 different missions to the 3 different vehicles via QGC. For example,

Vehicle 1: Takeoff@5m, WP1 at 5 meter and hover for 30 seconds, WP at 5 meters hover for 30 seconds, Land

Vehicle 2: Takeoff@10m, WP1 at 10 meters and hover for 30 seconds, WP at 10 meters hover for 30 seconds, Land.

Vehicle 3: Takeoff@15m, WP1 at 15 meters and hover for 30 seconds, WP at 15 meters hover for 30 seconds, Land.

However, it seems to me like all 3 vehicles follow the mission at 15 meters. Could it be that I am missing some menu selection step in QGC?

What is the best way to provide 3 different missions to 3 different vehicles in SITL? I would like it so that the correct missions are already “stored” in the virtual vehicles and I just start the mission via QGC. What is the best way to accomplish this?

Thanks,
Kiran.

Ok So I created a ROS node to push 2 different sets of waypoints to the simulated iris_1 and iris_2. At the command line, when I do a

rostopic echo /uav1/mavros/mission/waypoints

I get the output below. Notice the z_alt is 50.0


waypoints:

frame: 3
command: 22
is_current: False
autocontinue: True
param1: 0.0
param2: 0.0
param3: 0.0
param4: nan
x_lat: 38.0149993896
y_long: -84.5067138672
z_alt: 50.0
  • frame: 3
    command: 16
    is_current: True
    autocontinue: True
    param1: 60.0
    param2: 0.0
    param3: 0.0
    param4: nan
    x_lat: 38.0149993896
    y_long: -84.5061187744
    z_alt: 50.0
  • frame: 2
    command: 178
    is_current: False
    autocontinue: True
    param1: 1.0
    param2: 2.0
    param3: -1.0
    param4: 0.0
    x_lat: 0.0
    y_long: 0.0
    z_alt: 0.0
  • frame: 3
    command: 16
    is_current: False
    autocontinue: True
    param1: 30.0
    param2: 0.0
    param3: 0.0
    param4: nan
    x_lat: 38.0153160095
    y_long: -84.5061187744
    z_alt: 50.0
  • frame: 2
    command: 178
    is_current: False
    autocontinue: True
    param1: 1.0
    param2: 2.0
    param3: -1.0
    param4: 0.0
    x_lat: 0.0
    y_long: 0.0
    z_alt: 0.0
  • frame: 3
    command: 16
    is_current: False
    autocontinue: True
    param1: 30.0
    param2: 0.0
    param3: 0.0
    param4: nan
    x_lat: 38.0153198242
    y_long: -84.5067596436
    z_alt: 50.0
  • frame: 2
    command: 178
    is_current: False
    autocontinue: True
    param1: 1.0
    param2: 2.0
    param3: -1.0
    param4: 0.0
    x_lat: 0.0
    y_long: 0.0
    z_alt: 0.0
  • frame: 3
    command: 21
    is_current: False
    autocontinue: True
    param1: 0.0
    param2: 0.0
    param3: 0.0
    param4: nan
    x_lat: 38.0149650574
    y_long: -84.5067367554
    z_alt: 0.0

and
when I do a

rostopic echo /uav2/mavros/mission/waypoints

I get the output below. Notice that the z_alt is 10.0

waypoints:

frame: 3
command: 22
is_current: False
autocontinue: True
param1: 0.0
param2: 0.0
param3: 0.0
param4: nan
x_lat: 38.0149993896
y_long: -84.5067138672
z_alt: 10.0
  • frame: 3
    command: 16
    is_current: True
    autocontinue: True
    param1: 60.0
    param2: 0.0
    param3: 0.0
    param4: nan
    x_lat: 38.0149993896
    y_long: -84.5061187744
    z_alt: 10.0
  • frame: 2
    command: 178
    is_current: False
    autocontinue: True
    param1: 1.0
    param2: 2.0
    param3: -1.0
    param4: 0.0
    x_lat: 0.0
    y_long: 0.0
    z_alt: 0.0
  • frame: 3
    command: 16
    is_current: False
    autocontinue: True
    param1: 30.0
    param2: 0.0
    param3: 0.0
    param4: nan
    x_lat: 38.0153160095
    y_long: -84.5061187744
    z_alt: 10.0
  • frame: 2
    command: 178
    is_current: False
    autocontinue: True
    param1: 1.0
    param2: 2.0
    param3: -1.0
    param4: 0.0
    x_lat: 0.0
    y_long: 0.0
    z_alt: 0.0
  • frame: 3
    command: 16
    is_current: False
    autocontinue: True
    param1: 30.0
    param2: 0.0
    param3: 0.0
    param4: nan
    x_lat: 38.0153198242
    y_long: -84.5067596436
    z_alt: 10.0
  • frame: 2
    command: 178
    is_current: False
    autocontinue: True
    param1: 1.0
    param2: 2.0
    param3: -1.0
    param4: 0.0
    x_lat: 0.0
    y_long: 0.0
    z_alt: 0.0
  • frame: 3
    command: 21
    is_current: False
    autocontinue: True
    param1: 0.0
    param2: 0.0
    param3: 0.0
    param4: nan
    x_lat: 38.0149650574
    y_long: -84.5067367554
    z_alt: 0.0

So this indicates that both are getting different sets of waypoints. But when I start the mission both irises try to fly at an altitude of 10.0 m in gazebo.

Can anyone shed any light on why this would be? Is there only one navigator running? Is this a bug or am I doing something wrong?

I figured it out. The reason for both UAVs following the same waypoints at the same altitude is because there was only one dataman file. The default one. So the 2 instances of navigator were being provided the same set of waypoints, which were whatever was written last to the dataman file.

The fix is to provide two different files when dataman is started. This is a bug that should be fixed by the maintainers of PX4. Additionally, the two parameters below must be set to 0, if you are running without QGC and without any manual control. Otherwise, even with QGC, if datalink is lost, the iries will Return to Land in Gazebo.

param set NAV_DLL_ACT 0
param set NAV_RCL_ACT 0

So the big change was to provide a new file to dataman start up in the rCS files like so:

dataman start -f dataman1

and

dataman start -f dataman2

Hope this helps any others who might run in to the same issue.

So iris_1 under Firmware/posix-configs/SITL/init/ekf2

is

uorb start
param load
dataman start -f dataman1
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 0
param set NAV_RCL_ACT 0
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 -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

and iris_2 in under Firmware/posix-configs/SITL/init/ekf2

is

uorb start
param load
dataman start -f dataman2
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 0
param set NAV_RCL_ACT 0
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 -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

1 Like

This is very helpful, thanks.