Skip to content

Commit

Permalink
Merge pull request #10 from ctu-mrs/mavlink_to_gcs
Browse files Browse the repository at this point in the history
add --mavlink_gcs_udp_port param to the drone spawner
  • Loading branch information
klaxalk authored Jul 9, 2024
2 parents fa3f4a8 + f1a0640 commit 29ba363
Show file tree
Hide file tree
Showing 4 changed files with 38 additions and 3 deletions.
Original file line number Diff line number Diff line change
@@ -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
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,8 +4,9 @@
<arg name="ID"/>
<arg name="PX4_SIM_MODEL"/>
<arg name="PX4_ESTIMATOR" default="ekf2"/>
<arg name="MAVLINK_GCS_UDP_PORT" default=""/> <!-- mavlink port for QGC -->
<arg name="ROMFS_PATH" default="$(find mrs_uav_gazebo_simulation)/ROMFS"/>

<!-- PX4 configs -->
<arg name="interactive" default="false"/>
<!-- PX4 SITL -->
Expand All @@ -18,9 +19,10 @@
<!-- PX4 params -->
<env name="PX4_SIM_MODEL" value="$(arg PX4_SIM_MODEL)" />
<env name="PX4_ESTIMATOR" value="$(arg PX4_ESTIMATOR)" />
<env if="$(eval MAVLINK_GCS_UDP_PORT != '')" name="MAVLINK_GCS_UDP_PORT" value="$(arg MAVLINK_GCS_UDP_PORT)" />

<!-- PX4 SITL -->
<node name="sitl_uav$(arg ID)" pkg="px4" type="px4" output="screen" args="$(arg ROMFS_PATH)/px4fmu_common -s etc/init.d-posix/rcS $(arg px4_interactive_mode) -i $(arg ID) -w sitl_uav$(arg ID)"/>

</group>
</launch>
Original file line number Diff line number Diff line change
Expand Up @@ -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 '--'
Expand Down Expand Up @@ -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
# #}

Expand Down Expand Up @@ -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)
Expand Down

0 comments on commit 29ba363

Please sign in to comment.