diff --git a/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink_gcs b/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink_gcs
new file mode 100644
index 0000000..2ca2b25
--- /dev/null
+++ b/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink_gcs
@@ -0,0 +1,16 @@
+#!/bin/sh
+# shellcheck disable=SC2154
+
+# GCS link
+mavlink start -x -u $MAVLINK_GCS_UDP_PORT -r 4000000 -f
+mavlink stream -r 100 -s ATTITUDE -u $MAVLINK_GCS_UDP_PORT
+mavlink stream -r 100 -s ATTITUDE_QUATERNION -u $MAVLINK_GCS_UDP_PORT # /mavros/imu/data
+mavlink stream -r 100 -s ATTITUDE_TARGET -u $MAVLINK_GCS_UDP_PORT
+mavlink stream -r 100 -s HIGHRES_IMU -u $MAVLINK_GCS_UDP_PORT # /mavros/imu/data_raw
+mavlink stream -r 100 -s LOCAL_POSITION_NED -u $MAVLINK_GCS_UDP_PORT
+mavlink stream -r 100 -s ODOMETRY -u $MAVLINK_GCS_UDP_PORT
+mavlink stream -r 100 -s GLOBAL_POSITION_INT -u $MAVLINK_GCS_UDP_PORT
+mavlink stream -r 10 -s RC_CHANNELS -u $MAVLINK_GCS_UDP_PORT
+mavlink stream -r 10 -s SYS_STATUS -u $MAVLINK_GCS_UDP_PORT
+mavlink stream -r 100 -s HEARTBEAT -u $MAVLINK_GCS_UDP_PORT
+mavlink stream -r 100 -s DISTANCE_SENSOR -u $MAVLINK_GCS_UDP_PORT
diff --git a/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/rcS b/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/rcS
index 55f1c75..2e819d3 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/rcS
+++ b/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/rcS
@@ -271,6 +271,11 @@ fi
#user defined mavlink streams for instances can be in PATH
. px4-rc.mavlink
+if [ -n "$MAVLINK_GCS_UDP_PORT" ]
+then
+ . px4-rc.mavlink_gcs
+fi
+
# execute autostart post script if any
[ -e "$autostart_file".post ] && . "$autostart_file".post
diff --git a/ros_packages/mrs_uav_gazebo_simulation/launch/run_simulation_firmware.launch b/ros_packages/mrs_uav_gazebo_simulation/launch/run_simulation_firmware.launch
index 9661dd6..45b00ff 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/launch/run_simulation_firmware.launch
+++ b/ros_packages/mrs_uav_gazebo_simulation/launch/run_simulation_firmware.launch
@@ -4,8 +4,9 @@
+
-
+
@@ -18,9 +19,10 @@
+
-
+
diff --git a/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/mrs_drone_spawner.py b/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/mrs_drone_spawner.py
index 0fd409b..68dae53 100755
--- a/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/mrs_drone_spawner.py
+++ b/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/mrs_drone_spawner.py
@@ -471,7 +471,7 @@ def parse_user_input(self, input_str):
'model': None,
'ids': [],
'names': [],
- 'spawn_poses': {},
+ 'spawn_poses': {}
}
# parse out the keywords starting with '--'
@@ -999,6 +999,10 @@ def get_jinja_params_for_one_robot(self, params_dict, index, ID):
robot_params['mavlink_config'] = self.get_mavlink_config_for_robot(ID)
+ if 'mavlink_gcs_udp_port' in params_dict.keys():
+ if len(params_dict['mavlink_gcs_udp_port']) > index:
+ robot_params['mavlink_gcs_udp_port'] = params_dict['mavlink_gcs_udp_port'][index]
+
return robot_params
# #}
@@ -1051,6 +1055,14 @@ def launch_px4_firmware(self, robot_params):
'PX4_SIM_MODEL:=' + str(robot_params['model']),
'ROMFS_PATH:=' + str(romfs_path)
]
+
+ # do we want to send raw mavlink data to a specific port? (this will also auto connect to QGroundControl)
+ if 'mavlink_gcs_udp_port' in robot_params.keys():
+ if isinstance(robot_params['mavlink_gcs_udp_port'], int):
+ roslaunch_args.append('MAVLINK_GCS_UDP_PORT:=' + str(robot_params["mavlink_gcs_udp_port"]))
+ else:
+ raise CouldNotLaunch(f'\'{robot_params["mavlink_gcs_udp_port"]}\' is not a valid port number')
+
roslaunch_sequence = [(self.px4_fimrware_launch_path, roslaunch_args)]
firmware_process = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_sequence)