MAVROS integration & FCU URL

umlr

Hello,

I could find any information or example code for running MAVROS on the RB5 (like it exists for the VOXL here: https://docs.modalai.com/mavros/). Is there any documentation around?

I tried running MAVROS on the RB5 (from a docker container running ROS noetic and MAVROS) but it is unable to connect to the FCU. I'm not sure of the address of the FCU and what port to use, and using the default address (localhost) and port did not work:

roslaunch mavros px4.launch fcu_url:=udp://127.0.0.1:14551@:14551

umlr

An update:

I managed to get some kind of connection working on ttyUSB0 but I still got a lot of issues.

I added a mavlink serial stream in /etc/modalai/full-m0052.config by adding the following line (tried with different baud rates).

mavlink start -d /dev/ttyUSB0 -b 57600 -r 20000 -m onboard

Launching mavros using:

roslaunch mavros px4.launch fcu_url:=/dev/ttyUSB0:57600 tgt_system:=140

Will give the following errors:

started roslaunch server http://127.0.0.1:35521/

SUMMARY
========

CLEAR PARAMETERS
 * /mavros/

PARAMETERS
 * /mavros/cmd/use_comp_id_system_control: False
 * /mavros/conn/heartbeat_rate: 1.0
 * /mavros/conn/system_time_rate: 1.0
 * /mavros/conn/timeout: 10.0
 * /mavros/conn/timesync_rate: 10.0
 * /mavros/distance_sensor/hrlv_ez4_pub/field_of_view: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/frame_id: hrlv_ez4_sonar
 * /mavros/distance_sensor/hrlv_ez4_pub/id: 0
 * /mavros/distance_sensor/hrlv_ez4_pub/orientation: PITCH_270
 * /mavros/distance_sensor/hrlv_ez4_pub/send_tf: True
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/laser_1_sub/id: 3
 * /mavros/distance_sensor/laser_1_sub/orientation: PITCH_270
 * /mavros/distance_sensor/laser_1_sub/subscriber: True
 * /mavros/distance_sensor/lidarlite_pub/field_of_view: 0.0
 * /mavros/distance_sensor/lidarlite_pub/frame_id: lidarlite_laser
 * /mavros/distance_sensor/lidarlite_pub/id: 1
 * /mavros/distance_sensor/lidarlite_pub/orientation: PITCH_270
 * /mavros/distance_sensor/lidarlite_pub/send_tf: True
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/sonar_1_sub/id: 2
 * /mavros/distance_sensor/sonar_1_sub/orientation: PITCH_270
 * /mavros/distance_sensor/sonar_1_sub/subscriber: True
 * /mavros/fake_gps/eph: 2.0
 * /mavros/fake_gps/epv: 2.0
 * /mavros/fake_gps/fix_type: 3
 * /mavros/fake_gps/geo_origin/alt: 408.0
 * /mavros/fake_gps/geo_origin/lat: 47.3667
 * /mavros/fake_gps/geo_origin/lon: 8.55
 * /mavros/fake_gps/gps_rate: 5.0
 * /mavros/fake_gps/mocap_transform: True
 * /mavros/fake_gps/satellites_visible: 5
 * /mavros/fake_gps/tf/child_frame_id: fix
 * /mavros/fake_gps/tf/frame_id: map
 * /mavros/fake_gps/tf/listen: False
 * /mavros/fake_gps/tf/rate_limit: 10.0
 * /mavros/fake_gps/tf/send: False
 * /mavros/fake_gps/use_mocap: True
 * /mavros/fake_gps/use_vision: False
 * /mavros/fcu_protocol: v2.0
 * /mavros/fcu_url: /dev/ttyUSB0:57600
 * /mavros/gcs_url: 
 * /mavros/global_position/child_frame_id: base_link
 * /mavros/global_position/frame_id: map
 * /mavros/global_position/gps_uere: 1.0
 * /mavros/global_position/rot_covariance: 99999.0
 * /mavros/global_position/tf/child_frame_id: base_link
 * /mavros/global_position/tf/frame_id: map
 * /mavros/global_position/tf/global_frame_id: earth
 * /mavros/global_position/tf/send: False
 * /mavros/global_position/use_relative_alt: True
 * /mavros/image/frame_id: px4flow
 * /mavros/imu/angular_velocity_stdev: 0.0003490659 // 0...
 * /mavros/imu/frame_id: base_link
 * /mavros/imu/linear_acceleration_stdev: 0.0003
 * /mavros/imu/magnetic_stdev: 0.0
 * /mavros/imu/orientation_stdev: 1.0
 * /mavros/landing_target/camera/fov_x: 2.0071286398
 * /mavros/landing_target/camera/fov_y: 2.0071286398
 * /mavros/landing_target/image/height: 480
 * /mavros/landing_target/image/width: 640
 * /mavros/landing_target/land_target_type: VISION_FIDUCIAL
 * /mavros/landing_target/listen_lt: False
 * /mavros/landing_target/mav_frame: LOCAL_NED
 * /mavros/landing_target/target_size/x: 0.3
 * /mavros/landing_target/target_size/y: 0.3
 * /mavros/landing_target/tf/child_frame_id: camera_center
 * /mavros/landing_target/tf/frame_id: landing_target
 * /mavros/landing_target/tf/listen: False
 * /mavros/landing_target/tf/rate_limit: 10.0
 * /mavros/landing_target/tf/send: True
 * /mavros/local_position/frame_id: map
 * /mavros/local_position/tf/child_frame_id: base_link
 * /mavros/local_position/tf/frame_id: map
 * /mavros/local_position/tf/send: False
 * /mavros/local_position/tf/send_fcu: False
 * /mavros/mission/pull_after_gcs: True
 * /mavros/mission/use_mission_item_int: True
 * /mavros/mocap/use_pose: True
 * /mavros/mocap/use_tf: False
 * /mavros/odometry/fcu/odom_child_id_des: base_link
 * /mavros/odometry/fcu/odom_parent_id_des: map
 * /mavros/plugin_blacklist: ['safety_area', '...
 * /mavros/plugin_whitelist: []
 * /mavros/px4flow/frame_id: px4flow
 * /mavros/px4flow/ranger_fov: 0.118682
 * /mavros/px4flow/ranger_max_range: 5.0
 * /mavros/px4flow/ranger_min_range: 0.3
 * /mavros/safety_area/p1/x: 1.0
 * /mavros/safety_area/p1/y: 1.0
 * /mavros/safety_area/p1/z: 1.0
 * /mavros/safety_area/p2/x: -1.0
 * /mavros/safety_area/p2/y: -1.0
 * /mavros/safety_area/p2/z: -1.0
 * /mavros/setpoint_accel/send_force: False
 * /mavros/setpoint_attitude/reverse_thrust: False
 * /mavros/setpoint_attitude/tf/child_frame_id: target_attitude
 * /mavros/setpoint_attitude/tf/frame_id: map
 * /mavros/setpoint_attitude/tf/listen: False
 * /mavros/setpoint_attitude/tf/rate_limit: 50.0
 * /mavros/setpoint_attitude/use_quaternion: False
 * /mavros/setpoint_position/mav_frame: LOCAL_NED
 * /mavros/setpoint_position/tf/child_frame_id: target_position
 * /mavros/setpoint_position/tf/frame_id: map
 * /mavros/setpoint_position/tf/listen: False
 * /mavros/setpoint_position/tf/rate_limit: 50.0
 * /mavros/setpoint_raw/thrust_scaling: 1.0
 * /mavros/setpoint_velocity/mav_frame: LOCAL_NED
 * /mavros/startup_px4_usb_quirk: True
 * /mavros/sys/disable_diag: False
 * /mavros/sys/min_voltage: 10.0
 * /mavros/target_component_id: 1
 * /mavros/target_system_id: 140
 * /mavros/tdr_radio/low_rssi: 40
 * /mavros/time/time_ref_source: fcu
 * /mavros/time/timesync_avg_alpha: 0.6
 * /mavros/time/timesync_mode: MAVLINK
 * /mavros/vibration/frame_id: base_link
 * /mavros/vision_pose/tf/child_frame_id: vision_estimate
 * /mavros/vision_pose/tf/frame_id: odom
 * /mavros/vision_pose/tf/listen: False
 * /mavros/vision_pose/tf/rate_limit: 10.0
 * /mavros/vision_speed/listen_twist: True
 * /mavros/vision_speed/twist_cov: True
 * /mavros/wheel_odometry/child_frame_id: base_link
 * /mavros/wheel_odometry/count: 2
 * /mavros/wheel_odometry/frame_id: odom
 * /mavros/wheel_odometry/send_raw: True
 * /mavros/wheel_odometry/send_twist: False
 * /mavros/wheel_odometry/tf/child_frame_id: base_link
 * /mavros/wheel_odometry/tf/frame_id: odom
 * /mavros/wheel_odometry/tf/send: False
 * /mavros/wheel_odometry/use_rpm: False
 * /mavros/wheel_odometry/vel_error: 0.1
 * /mavros/wheel_odometry/wheel0/radius: 0.05
 * /mavros/wheel_odometry/wheel0/x: 0.0
 * /mavros/wheel_odometry/wheel0/y: -0.15
 * /mavros/wheel_odometry/wheel1/radius: 0.05
 * /mavros/wheel_odometry/wheel1/x: 0.0
 * /mavros/wheel_odometry/wheel1/y: 0.15
 * /rosdistro: noetic
 * /rosversion: 1.15.14

NODES
  /
    mavros (mavros/mavros_node)

auto-starting new master
process[master]: started with pid [128]
ROS_MASTER_URI=http://127.0.0.1:11311

setting /run_id to b5ae965e-8a6f-11ec-b8b7-00037f12fd78
process[rosout-1]: started with pid [138]
started core service [/rosout]
process[mavros-2]: started with pid [146]
[ INFO] [1644497294.714175814]: FCU URL: /dev/ttyUSB0:57600
[ INFO] [1644497294.716806765]: serial0: device: /dev/ttyUSB0 @ 57600 bps
[ INFO] [1644497294.718143109]: GCS bridge disabled
[ INFO] [1644497294.736680394]: Plugin 3dr_radio loaded
[ INFO] [1644497294.744110305]: Plugin 3dr_radio initialized
[ INFO] [1644497294.744244695]: Plugin actuator_control loaded
[ INFO] [1644497294.747218942]: Plugin actuator_control initialized
[ INFO] [1644497294.752170375]: Plugin adsb loaded
[ INFO] [1644497294.755233416]: Plugin adsb initialized
[ INFO] [1644497294.755420958]: Plugin altitude loaded
[ INFO] [1644497294.756526875]: Plugin altitude initialized
[ INFO] [1644497294.756649228]: Plugin cam_imu_sync loaded
[ INFO] [1644497294.757191634]: Plugin cam_imu_sync initialized
[ INFO] [1644497294.757365471]: Plugin command loaded
[ INFO] [1644497294.762948053]: Plugin command initialized
[ INFO] [1644497294.763087654]: Plugin companion_process_status loaded
[ INFO] [1644497294.764991939]: Plugin companion_process_status initialized
[ INFO] [1644497294.767453065]: Plugin debug_value loaded
[ INFO] [1644497294.771286022]: Plugin debug_value initialized
[ INFO] [1644497294.771344697]: Plugin distance_sensor blacklisted
[ INFO] [1644497294.771509884]: Plugin fake_gps loaded
[ INFO] [1644497294.781571527]: Plugin fake_gps initialized
[ INFO] [1644497294.781784967]: Plugin ftp loaded
[ INFO] [1644497294.788256117]: Plugin ftp initialized
[ INFO] [1644497294.788456009]: Plugin global_position loaded
[ INFO] [1644497294.799789810]: Plugin global_position initialized
[ INFO] [1644497294.799951245]: Plugin gps_rtk loaded
[ INFO] [1644497294.801789820]: Plugin gps_rtk initialized
[ INFO] [1644497294.801950317]: Plugin hil loaded
[ INFO] [1644497294.811865532]: Plugin hil initialized
[ INFO] [1644497294.812071677]: Plugin home_position loaded
[ INFO] [1644497294.815328096]: Plugin home_position initialized
[ INFO] [1644497294.815564308]: Plugin imu loaded
[ INFO] [1644497294.820583066]: Plugin imu initialized
[ INFO] [1644497294.820723136]: Plugin landing_target loaded
[ INFO] [1644497294.831767353]: Plugin landing_target initialized
[ INFO] [1644497294.831906068]: Plugin local_position loaded
[ INFO] [1644497294.837216378]: Plugin local_position initialized
[ INFO] [1644497294.837338418]: Plugin log_transfer loaded
[ INFO] [1644497294.842142955]: Plugin log_transfer initialized
[ INFO] [1644497294.842299491]: Plugin manual_control loaded
[ INFO] [1644497294.848333705]: Plugin manual_control initialized
[ INFO] [1644497294.848478309]: Plugin mocap_pose_estimate loaded
[ INFO] [1644497294.850915620]: Plugin mocap_pose_estimate initialized
[ INFO] [1644497294.851052512]: Plugin mount_control loaded
[ INFO] [1644497294.855169569]: Plugin mount_control initialized
[ INFO] [1644497294.855311619]: Plugin obstacle_distance loaded
[ INFO] [1644497294.859459734]: Plugin obstacle_distance initialized
[ INFO] [1644497294.859597303]: Plugin odom loaded
[ INFO] [1644497294.863852346]: Plugin odom initialized
[ INFO] [1644497294.863983297]: Plugin onboard_computer_status loaded
[ INFO] [1644497294.865691286]: Plugin onboard_computer_status initialized
[ INFO] [1644497294.865915564]: Plugin param loaded
[ INFO] [1644497294.869083450]: Plugin param initialized
[ INFO] [1644497294.869244155]: Plugin px4flow loaded
[ INFO] [1644497294.876249427]: Plugin px4flow initialized
[ INFO] [1644497294.876290124]: Plugin rangefinder blacklisted
[ INFO] [1644497294.876493351]: Plugin rc_io loaded
[ INFO] [1644497294.879109606]: Plugin rc_io initialized
[ INFO] [1644497294.879156192]: Plugin safety_area blacklisted
[ INFO] [1644497294.879273438]: Plugin setpoint_accel loaded
[ INFO] [1644497294.882430849]: Plugin setpoint_accel initialized
[ INFO] [1644497294.882702495]: Plugin setpoint_attitude loaded
[ INFO] [1644497294.893776207]: Plugin setpoint_attitude initialized
[ INFO] [1644497294.894115751]: Plugin setpoint_position loaded
[ INFO] [1644497294.903744834]: Plugin setpoint_position initialized
[ INFO] [1644497294.904512510]: Plugin setpoint_raw loaded
[ INFO] [1644497294.913539886]: Plugin setpoint_raw initialized
[ INFO] [1644497294.913738735]: Plugin setpoint_trajectory loaded
[ INFO] [1644497294.929639963]: Plugin setpoint_trajectory initialized
[ INFO] [1644497294.929829849]: Plugin setpoint_velocity loaded
[ INFO] [1644497294.934190935]: Plugin setpoint_velocity initialized
[ INFO] [1644497294.934582068]: Plugin sys_status loaded
[ INFO] [1644497294.946098500]: Plugin sys_status initialized
[ INFO] [1644497294.946281561]: Plugin sys_time loaded
[ INFO] [1644497294.953763737]: TM: Timesync mode: MAVLINK
[ INFO] [1644497294.954854282]: Plugin sys_time initialized
[ INFO] [1644497294.955021814]: Plugin trajectory loaded
[ INFO] [1644497294.959519999]: Plugin trajectory initialized
[ INFO] [1644497294.959697953]: Plugin vfr_hud loaded
[ INFO] [1644497294.962785381]: Plugin vfr_hud initialized
[ INFO] [1644497294.962844473]: Plugin vibration blacklisted
[ INFO] [1644497294.962985273]: Plugin vision_pose_estimate loaded
[ INFO] [1644497294.969410359]: Plugin vision_pose_estimate initialized
[ INFO] [1644497294.969568354]: Plugin vision_speed_estimate loaded
[ INFO] [1644497294.972662818]: Plugin vision_speed_estimate initialized
[ INFO] [1644497294.972857081]: Plugin waypoint loaded
[ INFO] [1644497294.979514836]: Plugin waypoint initialized
[ INFO] [1644497294.979571739]: Plugin wheel_odometry blacklisted
[ INFO] [1644497294.979705504]: Plugin wind_estimation loaded
[ INFO] [1644497294.980244940]: Plugin wind_estimation initialized
[ INFO] [1644497294.980320811]: Autostarting mavlink via USB on PX4
[ INFO] [1644497294.980513824]: Built-in SIMD instructions: ARM NEON
[ INFO] [1644497294.980561296]: Built-in MAVLink package version: 2020.6.6
[ INFO] [1644497294.980583912]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad icarous matrixpilot paparazzi slugs standard uAvionix ualberta
[ INFO] [1644497294.980598919]: MAVROS started. MY ID 1.240, TARGET ID 140.1
[ INFO] [1644497295.440938800]: IMU: Attitude quaternion IMU detected!
[ INFO] [1644497301.933338907]: IMU: High resolution IMU detected!
[ WARN] [1644497327.169825716]: TM : RTT too high for timesync: 1644496290819.24 ms.
[ WARN] [1644497327.172690387]: CMD: Unexpected command 200, result 4
[ WARN] [1644497337.591434803]: TM : RTT too high for timesync: 1644496290541.59 ms.
[ WARN] [1644497344.210049731]: CMD: Unexpected command 200, result 4
[ WARN] [1644497344.808649428]: TM : RTT too high for timesync: 253.45 ms.
[ WARN] [1644497354.378344512]: TM : RTT too high for timesync: 118.36 ms.
[ WARN] [1644497365.156384473]: TM : RTT too high for timesync: 1644496290605.98 ms.
[ INFO] [1644497373.358340297]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot
[ INFO] [1644497373.403917305]: IMU: Attitude quaternion IMU detected!
[ WARN] [1644497375.373903108]: VER: broadcast request timeout, retries left 4
[ WARN] [1644497376.374285764]: VER: broadcast request timeout, retries left 3
[ WARN] [1644497382.378677965]: VER: unicast request timeout, retries left 2
[ INFO] [1644497383.362510985]: HP: requesting home position
[ WARN] [1644497383.364569045]: CON: Lost connection, HEARTBEAT timed out.
[ INFO] [1644497383.549022387]: IMU: Attitude quaternion IMU detected!
[ WARN] [1644497387.398762128]: CMD: Command 520 -- wait ack timeout
[ WARN] [1644497387.404753274]: PR: request param #0 timeout, retries left 2, and 609 params still missing
[ WARN] [1644497388.373647222]: CMD: Command 410 -- wait ack timeout
[ WARN] [1644497388.406444188]: PR: request param #0 timeout, retries left 1, and 609 params still missing
[ WARN] [1644497388.859446485]: TM : RTT too high for timesync: 156.31 ms.
[ WARN] [1644497389.408527120]: PR: request param #0 timeout, retries left 0, and 609 params still missing
[ERROR] [1644497390.409564636]: PR: request param #0 completely missing.
[ WARN] [1644497390.411134535]: PR: 608 params still missing, trying to request next: #1
[ WARN] [1644497391.411360578]: PR: request param #1 timeout, retries left 2, and 608 params still missing
[ WARN] [1644497392.403711901]: PR: request param #1 timeout, retries left 1, and 608 params still missing
[ WARN] [1644497393.405928494]: PR: request param #1 timeout, retries left 0, and 608 params still missing
[ERROR] [1644497394.407300447]: PR: request param #1 completely missing.
[ WARN] [1644497394.408432158]: PR: 607 params still missing, trying to request next: #2
[ WARN] [1644497395.408714817]: PR: request param #2 timeout, retries left 2, and 607 params still missing
[ INFO] [1644497395.503135963]: IMU: High resolution IMU detected!
[ WARN] [1644497396.503682815]: PR: request param #3 timeout, retries left 2, and 606 params still missing
[ WARN] [1644497397.505073527]: PR: request param #3 timeout, retries left 1, and 606 params still missing
[ WARN] [1644497398.506513898]: PR: request param #3 timeout, retries left 0, and 606 params still missing
[ WARN] [1644497398.632140494]: TM : RTT too high for timesync: 30.71 ms.
[ERROR] [1644497399.508405695]: PR: request param #3 completely missing.
[ WARN] [1644497399.509034343]: PR: 605 params still missing, trying to request next: #4
[ WARN] [1644497400.509314187]: PR: request param #4 timeout, retries left 2, and 605 params still missing
[ WARN] [1644497401.510247586]: PR: request param #4 timeout, retries left 1, and 605 params still missing
[ WARN] [1644497402.511594944]: PR: request param #4 timeout, retries left 0, and 605 params still missing
[ERROR] [1644497403.512411879]: PR: request param #4 completely missing.
[ WARN] [1644497403.513780219]: PR: 604 params still missing, trying to request next: #5
[ WARN] [1644497403.853859468]: CMD: Unexpected command 200, result 4
[ WARN] [1644497404.514771981]: PR: request param #5 timeout, retries left 2, and 604 params still missing
[ WARN] [1644497405.516676804]: PR: request param #5 timeout, retries left 1, and 604 params still missing
[ WARN] [1644497406.519431322]: PR: request param #5 timeout, retries left 0, and 604 params still missing
[ERROR] [1644497407.520202507]: PR: request param #5 completely missing.
[ WARN] [1644497407.521118381]: PR: 603 params still missing, trying to request next: #6
[ WARN] [1644497408.521594938]: PR: request param #6 timeout, retries left 2, and 603 params still missing
[ WARN] [1644497409.523688501]: PR: request param #6 timeout, retries left 1, and 603 params still missing
[ WARN] [1644497410.525578474]: PR: request param #6 timeout, retries left 0, and 603 params still missing
[ERROR] [1644497411.527071634]: PR: request param #6 completely missing.
[ WARN] [1644497411.528027320]: PR: 602 params still missing, trying to request next: #7
[ WARN] [1644497412.528616746]: PR: request param #7 timeout, retries left 2, and 602 params still missing
[ WARN] [1644497413.529532430]: PR: request param #7 timeout, retries left 1, and 602 params still missing
[ WARN] [1644497414.530423568]: PR: request param #7 timeout, retries left 0, and 602 params still missing
[ERROR] [1644497415.531906046]: PR: request param #7 completely missing.
[ WARN] [1644497415.532438812]: PR: 601 params still missing, trying to request next: #8

Connection is working partially and it's possible to get some message on some topics intermittently.

Eric Katzfey

@umlr I have worked with MAVSDK on RB5 and I believe there are similarities between the way that connects to PX4 and how MAVROS does. The connection is via UDP and doesn't rely on a serial connection at all. As you figured out above you need to start a new instance of mavlink in PX4 for the connection. Here is the one I added for MAVSDK: mavlink start -x -o 14540 -u 14558 -n lo. Hopefully that helps getting the MAVROS connection up and running. Ultimately we will be integrating MAVROS ourselves and will have better instruction after that. Also, here is the link to the mavlink module reference: https://dev.px4.io/master/en/middleware/modules_communication.html#mavlink

umlr

@Eric-Katzfey Thanks for your help! I added the mavlink instance as you suggested and it works.

To recap for anyone with the same issue.

Add mavlink start -x -o 14540 -u 14558 -n lo in /etc/modalai/full-m0052.config after the line mavlink start -u 14556 -t 127.0.0.1 -n lo -m custom
Run MAVROS locally using the following (adapt tgt_system to your target system ID)

roslaunch mavros px4.launch fcu_url:=udp://127.0.0.1:14540@:14558 tgt_system:=1

umlr

Another update that could be useful to someone:

I needed to have a high enough frequency on the local position feedback (ROS topic /mavros/local_position/pose) but I was only receiving at 1hz. Usual solution with most FCU is to set the parameter MAV_1_MODE to Onboard but this parameter is not available on the RB5.

Instead I added the line mavlink stream -u 14558 -s LOCAL_POSITION_NED -r 100 in /etc/modalai/full-m0052.config to set the stream rate. The end of my file looks like that.

# Configure a second mavlink instance for onboard messages (e.g. VIO and VOA)
mavlink start -u 14556 -t 127.0.0.1 -n lo -m custom
mavlink start -x -o 14540 -u 14558 -n lo

sleep 1

mavlink stream -u 14557 -s HIGHRES_IMU -r 5
mavlink stream -u 14557 -s ATTITUDE -r 5
mavlink stream -u 14557 -s RC_CHANNELS -r 20
mavlink stream -u 14558 -s LOCAL_POSITION_NED -r 100

sleep 1

# Start up the IMU server to support VIO
imu_server start

And this way I managed to get a frequency of around 55hz on /mavros/local_position/pose.