Mavros: Keep dropping Message and said:TX queue overflow, when publish topic to mavros/setpoint_raw/local

Created on 18 Jul 2018  ·  3Comments  ·  Source: mavlink/mavros

Dear team,
Could you guys give some help or advice about this.

I have no idea where to debug this, I am using same code, but this error just appear in second day, seems strange.

Issue details

I am using OFFBOARD mode to control the Drone to fly indoor, I have depth sensor and visual tag as feedback and using speed closeloop control. But when I publish msg to /mavros/setpoint_raw/local at 100hz for several second, the error appeared, and seems the drone can not receive any msgs, I test several publish rate and different pixhawk(2 and 4) the error still exist.
I am complete lost. please give a direction.

My launch procedure:
terminal1: roslaunch mavros px4.launch
terminal2: rosrun offboard drone_control.py

My px4.launch

<launch>
    <!-- vim: set ft=xml noet : -->
    <!-- example launch script for PX4 based FCU's -->

    <arg name="fcu_url" default="/dev/ttyACM0:921600" />
    <arg name="gcs_url" default="" />
    <arg name="tgt_system" default="1" />
    <arg name="tgt_component" default="1" />
    <arg name="log_output" default="screen" />
    <arg name="fcu_protocol" default="v2.0" />
    <arg name="respawn_mavros" default="false" />

    <include file="$(find mavros)/launch/node.launch">
        <arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
        <arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />

        <arg name="fcu_url" value="$(arg fcu_url)" />
        <arg name="gcs_url" value="$(arg gcs_url)" />
        <arg name="tgt_system" value="$(arg tgt_system)" />
        <arg name="tgt_component" value="$(arg tgt_component)" />
        <arg name="log_output" value="$(arg log_output)" />
        <arg name="fcu_protocol" value="$(arg fcu_protocol)" />
        <arg name="respawn_mavros" default="$(arg respawn_mavros)" />
    </include>
</launch>

the main loop of my control code

rospy.init_node('drone_control')
PIDinit()

rc_in = rospy.Subscriber('mavros/rc/in', RCIn, callback_rc, queue_size=1)
sub_state = rospy.Subscriber('mavros/state', State, callback_state, queue_size=1)
subodom = rospy.Subscriber('mavros/local_position/odom', Odometry, callback_odom)
subdepth = rospy.Subscriber('depth', Odometry, callback_depth)
position_pub = rospy.Publisher('mavros/setpoint_raw/local', PositionTarget, queue_size=1)
#rospy.spin()


rate = rospy.Rate(100)  
while not rospy.is_shutdown():


    vel_control = TwistStamped()
    pos_control = PoseStamped()

    scale_x = 0.4
    scale_y = 0.4
    scale_z = 1
    scale_yaw = 1

    pid_x.update(feedback_x)
    output_x = pid_x.output * scale_x
    feedback_x = pos_fuse_x
    pid_x.SetPoint = 0

    pid_y.update(feedback_y)
    output_y = pid_y.output * scale_y
    feedback_y = pos_fuse_y
    pid_y.SetPoint = 0

    pid_z.update(feedback_z)
    output_z = pid_z.output * scale_z
    feedback_z = pos_fuse_z

    if SWITCH == 1:
        pid_z.SetPoint = jump_height
    else:
        pid_z.SetPoint = 0

    feedback_yaw = odom_yaw
    if feedback_yaw - last_feedback_yaw != 0: 
        pid_yaw.update(feedback_yaw)
        output_yaw = pid_yaw.output * scale_yaw
        pid_yaw.SetPoint = tag_yaw



    #for stable
    if output_x > 0.02:
        output_x = 0.02
    elif output_x < -0.02:
        output_x = -0.02

    if output_y > 0.02:
        output_y = 0.02
    elif output_y < -0.02:
        output_y = -0.02

    if output_z > 0.1:
        output_z = 0.1
    elif output_z < -0.05:
        output_z = -0.05

    #output_z = 0
    if Z_FEEDBACK_AVALIABLE == 0:
        output_z = 0
    if XY_FEEDBACK_AVALIABLE == 0:
        output_x = 0
        output_y = 0

    setpoint = Vector3()
    setpoint.x = output_x
    setpoint.y = output_y
    setpoint.z = output_z
    stamp = rospy.get_rostime()

    msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
                             type_mask=
                                       PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ +
                                       PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ +
                                       PositionTarget.IGNORE_YAW + PositionTarget.IGNORE_YAW_RATE,
                             velocity=setpoint, 

                             yaw = output_yaw)
    msg.header.stamp = stamp
    position_pub.publish(msg)

    print  "z_set:", pid_z.SetPoint, "z_out:", output_z, "z_Feb:", feedback_z,'jump',jump_height 

rate.sleep()

MAVROS version and platform

Mavros: 0.18.4
ROS: Kinetic
Ubuntu: 16.04

Autopilot type and version

PX4, the latest version .
My hardware setup :pixhawk4 -(PX4 1.8.0) and pixhawk2, both have the same issue.

Node logs

eleboss@eleboss:~$ roslaunch mavros px4.launch 
... logging to /home/eleboss/.ros/log/7996cdf2-8a74-11e8-8157-00215caf42cc/roslaunch-eleboss-2176.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://eleboss:34437/

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/ttyACM0:921600
 * /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.000349065850399
 * /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/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/mocap/use_pose: True
 * /mavros/mocap/use_tf: False
 * /mavros/odometry/frame_tf/body_frame_orientation: frd
 * /mavros/odometry/frame_tf/local_frame: vision_ned
 * /mavros/plugin_blacklist: ['safety_area', '...
 * /mavros/plugin_whitelist: []
 * /mavros/px4flow/frame_id: px4flow
 * /mavros/px4flow/ranger_fov: 0.118682389136
 * /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_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: 1
 * /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: map
 * /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
 * /rosdistro: kinetic
 * /rosversion: 1.12.13

NODES
  /
    mavros (mavros/mavros_node)

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

setting /run_id to 7996cdf2-8a74-11e8-8157-00215caf42cc
process[rosout-1]: started with pid [2212]
started core service [/rosout]
process[mavros-2]: started with pid [2234]
[ INFO] [1531909350.225120069]: FCU URL: /dev/ttyACM0:921600
[ INFO] [1531909350.226491699]: serial0: device: /dev/ttyACM0 @ 921600 bps
[ INFO] [1531909350.226820176]: GCS bridge disabled
[ INFO] [1531909350.237634293]: Plugin 3dr_radio loaded
[ INFO] [1531909350.239873558]: Plugin 3dr_radio initialized
[ INFO] [1531909350.239991995]: Plugin actuator_control loaded
[ INFO] [1531909350.244402711]: Plugin actuator_control initialized
[ INFO] [1531909350.247686605]: Plugin adsb loaded
[ INFO] [1531909350.252164477]: Plugin adsb initialized
[ INFO] [1531909350.252287819]: Plugin altitude loaded
[ INFO] [1531909350.253538253]: Plugin altitude initialized
[ INFO] [1531909350.253642790]: Plugin cam_imu_sync loaded
[ INFO] [1531909350.254385422]: Plugin cam_imu_sync initialized
[ INFO] [1531909350.254507458]: Plugin command loaded
[ INFO] [1531909350.260767607]: Plugin command initialized
[ INFO] [1531909350.260882761]: Plugin debug_value loaded
[ INFO] [1531909350.266724238]: Plugin debug_value initialized
[ INFO] [1531909350.266761950]: Plugin distance_sensor blacklisted
[ INFO] [1531909350.266880713]: Plugin fake_gps loaded
[ INFO] [1531909350.285235202]: Plugin fake_gps initialized
[ INFO] [1531909350.285378124]: Plugin ftp loaded
[ INFO] [1531909350.292893777]: Plugin ftp initialized
[ INFO] [1531909350.293056406]: Plugin global_position loaded
[ INFO] [1531909350.316684204]: Plugin global_position initialized
[ INFO] [1531909350.316814783]: Plugin hil loaded
[ INFO] [1531909350.332854567]: Plugin hil initialized
[ INFO] [1531909350.332996072]: Plugin home_position loaded
[ INFO] [1531909350.337255788]: Plugin home_position initialized
[ INFO] [1531909350.337395106]: Plugin imu loaded
[ INFO] [1531909350.349334493]: Plugin imu initialized
[ INFO] [1531909350.349473928]: Plugin local_position loaded
[ INFO] [1531909350.356977561]: Plugin local_position initialized
[ INFO] [1531909350.357114405]: Plugin manual_control loaded
[ INFO] [1531909350.360976164]: Plugin manual_control initialized
[ INFO] [1531909350.361126079]: Plugin mocap_pose_estimate loaded
[ INFO] [1531909350.365655865]: Plugin mocap_pose_estimate initialized
[ INFO] [1531909350.365762619]: Plugin obstacle_distance loaded
[ INFO] [1531909350.368553759]: Plugin obstacle_distance initialized
[ INFO] [1531909350.368653103]: Plugin odom loaded
[ INFO] [1531909350.375619144]: Plugin odom initialized
[ INFO] [1531909350.375787260]: Plugin param loaded
[ INFO] [1531909350.379399807]: Plugin param initialized
[ INFO] [1531909350.379516510]: Plugin px4flow loaded
[ INFO] [1531909350.388167015]: Plugin px4flow initialized
[ INFO] [1531909350.388223705]: Plugin rangefinder blacklisted
[ INFO] [1531909350.388343616]: Plugin rc_io loaded
[ INFO] [1531909350.393300762]: Plugin rc_io initialized
[ INFO] [1531909350.393341683]: Plugin safety_area blacklisted
[ INFO] [1531909350.393454501]: Plugin setpoint_accel loaded
[ INFO] [1531909350.397677023]: Plugin setpoint_accel initialized
[ INFO] [1531909350.397807837]: Plugin setpoint_attitude loaded
[ INFO] [1531909350.412717136]: Plugin setpoint_attitude initialized
[ INFO] [1531909350.412859535]: Plugin setpoint_position loaded
[ INFO] [1531909350.429525693]: Plugin setpoint_position initialized
[ INFO] [1531909350.429714780]: Plugin setpoint_raw loaded
[ INFO] [1531909350.443358052]: Plugin setpoint_raw initialized
[ INFO] [1531909350.443515688]: Plugin setpoint_velocity loaded
[ INFO] [1531909350.454251102]: Plugin setpoint_velocity initialized
[ INFO] [1531909350.454424023]: Plugin sys_status loaded
[ INFO] [1531909350.467968466]: Plugin sys_status initialized
[ INFO] [1531909350.468137527]: Plugin sys_time loaded
[ INFO] [1531909350.481905565]: TM: Timesync mode: MAVLINK
[ INFO] [1531909350.483516225]: Plugin sys_time initialized
[ INFO] [1531909350.483657797]: Plugin trajectory loaded
[ INFO] [1531909350.493581639]: Plugin trajectory initialized
[ INFO] [1531909350.494069993]: Plugin vfr_hud loaded
[ INFO] [1531909350.496142178]: Plugin vfr_hud initialized
[ INFO] [1531909350.496190904]: Plugin vibration blacklisted
[ INFO] [1531909350.496335388]: Plugin vision_pose_estimate loaded
[ INFO] [1531909350.515468449]: Plugin vision_pose_estimate initialized
[ INFO] [1531909350.515643746]: Plugin vision_speed_estimate loaded
[ INFO] [1531909350.521799504]: Plugin vision_speed_estimate initialized
[ INFO] [1531909350.521968502]: Plugin waypoint loaded
[ INFO] [1531909350.538189416]: Plugin waypoint initialized
[ INFO] [1531909350.538386255]: Plugin wind_estimation loaded
[ INFO] [1531909350.539889616]: Plugin wind_estimation initialized
[ INFO] [1531909350.539964946]: Autostarting mavlink via USB on PX4
[ INFO] [1531909350.540088826]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1531909350.540114338]: Built-in MAVLink package version: 2018.6.6
[ INFO] [1531909350.540150085]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad icarous matrixpilot paparazzi slugs standard uAvionix ualberta
[ INFO] [1531909350.540186079]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ INFO] [1531909350.622977431]: IMU: High resolution IMU detected!
[ INFO] [1531909350.770808248]: IMU: Attitude quaternion IMU detected!
[ INFO] [1531909351.510894360]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot
[ INFO] [1531909351.524575461]: IMU: High resolution IMU detected!
[ INFO] [1531909351.536849572]: IMU: Attitude quaternion IMU detected!
[ INFO] [1531909352.521216751]: VER: 1.1: Capabilities         0x000000000000e4ef
[ INFO] [1531909352.521527249]: VER: 1.1: Flight software:     010800ff (00000000F3E3ADAE)
[ INFO] [1531909352.521695764]: VER: 1.1: Middleware software: 010800ff (00000000F3E3ADAE)
[ INFO] [1531909352.521851432]: VER: 1.1: OS software:         071600ff (0000000000000000)
[ INFO] [1531909352.522007743]: VER: 1.1: Board hardware:      00000011
[ INFO] [1531909352.522150719]: VER: 1.1: VID/PID:             26ac:0011
[ INFO] [1531909352.522287591]: VER: 1.1: UID:                 3036510831353833
[ WARN] [1531909352.522524746]: CMD: Unexpected command 520, result 0
[ERROR] [1531909354.991362039]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909354.991492639]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909354.992848484]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909354.995085754]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909354.995192916]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909354.995250845]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909354.998997752]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909354.999679892]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909354.999754820]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.000055547]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.002572007]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.002665214]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.002730521]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.002792850]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.002890051]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.002958791]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.003023608]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.003079069]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.003137396]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.003192739]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.003248728]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.003375496]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.003647546]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.004041463]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.004533199]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.005017337]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.005340368]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.005722578]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.006120254]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.006537709]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.006987191]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.007467659]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.007845275]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.008362372]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.008871683]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.009518278]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.010015781]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow
[ERROR] [1531909355.010528035]: 0: DROPPED Message SET_POSITION_TARGET_LOCAL_NED: MAVConnSerial::send_message: TX queue overflow

Diagnostics

eleboss@eleboss:~$ rostopic echo /diagnostics 
header: 
  seq: 14
  stamp: 
    secs: 1531909925
    nsecs: 482805514
  frame_id: ''
status: 
  - 
    level: 0
    name: "mavros: FCU connection"
    message: "connected"
    hardware_id: "/dev/ttyACM0:921600"
    values: 
      - 
        key: "Received packets:"
        value: "4799"
      - 
        key: "Dropped packets:"
        value: "0"
      - 
        key: "Buffer overruns:"
        value: "0"
      - 
        key: "Parse errors:"
        value: "0"
      - 
        key: "Rx sequence number:"
        value: "176"
      - 
        key: "Tx sequence number:"
        value: "166"
      - 
        key: "Rx total bytes:"
        value: "222864"
      - 
        key: "Tx total bytes:"
        value: "42088"
      - 
        key: "Rx speed:"
        value: "18536.000000"
      - 
        key: "Tx speed:"
        value: "4120.000000"
  - 
    level: 2
    name: "mavros: GPS"
    message: "No satellites"
    hardware_id: "/dev/ttyACM0:921600"
    values: 
      - 
        key: "Satellites visible"
        value: "0"
      - 
        key: "Fix type"
        value: "0"
      - 
        key: "EPH (m)"
        value: "Unknown"
      - 
        key: "EPV (m)"
        value: "Unknown"
  - 
    level: 0
    name: "mavros: Heartbeat"
    message: "Normal"
    hardware_id: "/dev/ttyACM0:921600"
    values: 
      - 
        key: "Heartbeats since startup"
        value: "12"
      - 
        key: "Frequency (Hz)"
        value: "0.955432"
      - 
        key: "Vehicle type"
        value: "Hexarotor"
      - 
        key: "Autopilot type"
        value: "PX4 Autopilot"
      - 
        key: "Mode"
        value: "MANUAL"
      - 
        key: "System status"
        value: "Standby"
  - 
    level: 0
    name: "mavros: System"
    message: "Normal"
    hardware_id: "/dev/ttyACM0:921600"
    values: 
      - 
        key: "Sensor present"
        value: "0x00000000"
      - 
        key: "Sensor enabled"
        value: "0x00000000"
      - 
        key: "Sensor helth"
        value: "0x00000000"
      - 
        key: "CPU Load (%)"
        value: "55.5"
      - 
        key: "Drop rate (%)"
        value: "0.0"
      - 
        key: "Errors comm"
        value: "0"
      - 
        key: "Errors count #1"
        value: "0"
      - 
        key: "Errors count #2"
        value: "0"
      - 
        key: "Errors count #3"
        value: "0"
      - 
        key: "Errors count #4"
        value: "0"
  - 
    level: 0
    name: "mavros: Battery"
    message: "Normal"
    hardware_id: "/dev/ttyACM0:921600"
    values: 
      - 
        key: "Voltage"
        value: "65.54"
      - 
        key: "Current"
        value: "-0.0"
      - 
        key: "Remaining"
        value: "-1.0"
  - 
    level: 0
    name: "mavros: Time Sync"
    message: "Normal"
    hardware_id: "/dev/ttyACM0:921600"
    values: 
      - 
        key: "Timesyncs since startup"
        value: "44"
      - 
        key: "Frequency (Hz)"
        value: "3.507350"
      - 
        key: "Last RTT (ms)"
        value: "6450.054686"
      - 
        key: "Mean RTT (ms)"
        value: "281.613853"
      - 
        key: "Last remote time (s)"
        value: "702.575705000"
      - 
        key: "Estimated time offset (s)"
        value: "1531909221.221841335"
---

question

Most helpful comment

YES!!!!!!!!!!!!
You are right about this!
I checked the publish rate of the topic, and found it become 1000HZ, not 100 HZ, and then I found there is a indent error in my code that failed the rate.sleep(), I modified it ,and it work as usual !

@vooon Thank you very much!

All 3 comments

Look at diag entry "mavros: FCU connection", when you got overflow.
Check rate of your setpoints, on sub 1M baud 100 Hz should be ok.
But you somehow generate much more messages than port may handle.
TX queue have 1k limit, which in normal case newer hit (usually 10 entries max).

Other possible thing, that your UART adaptor cannot handle 921600 and effective bandwidth limited by something else.

YES!!!!!!!!!!!!
You are right about this!
I checked the publish rate of the topic, and found it become 1000HZ, not 100 HZ, and then I found there is a indent error in my code that failed the rate.sleep(), I modified it ,and it work as usual !

@vooon Thank you very much!

Hi guys,
I guess that I have same problem like you, eleboss... I changed FCU on 921600 but I would like to know where did you find publish rate!
Please, can you help me? THX

Was this page helpful?
0 / 5 - 0 ratings

Related issues

RR2-IP2 picture RR2-IP2  ·  4Comments

jannsta1 picture jannsta1  ·  7Comments

Phadadev picture Phadadev  ·  4Comments

TSC21 picture TSC21  ·  12Comments

y22ma picture y22ma  ·  7Comments