Unexpected latency in ATTITUDE messages

Blue Robotics Software ArduSub
1

I want to ask something about obtaining telemetry data via pymavlink.

Currently, I am trying to retrieve the orientation of the vehicle via “ATTITUDE” message, and set the message interval to around 20hz.It works, and does return the orientation, but the strange issue is that it takes a while for the attitude message to reflect the actual orientation of the vehicle. For example, if I manually pitch it up, it takes a good 2-3s for it to register that the pitch is up, whereas in QGC, it is reflected instantly.

Do you have any insights as to why this might be the case?

2

Hi @Luc001,

I have some previous advice in these two posts which is likely a decent starting point - there’s probably a delay in your code (either explicitly in a sleep/wait, or just while it’s doing other things) that’s causing the extra latency.

If those examples don’t help we can potentially take a look at the actual code you’re running, assuming that’s something you’re able and willing to share.

3

Thank you for the two fantastic guides! I can certainly provide the snippet of my code, which I think might be a different issue from these two guides:

In the init function:

self.request_message_interval(mavutil.mavlink.MAVLINK_MSG_ID_ATTITUDE, frequency_hz=20)
self.request_message_interval(mavutil.mavlink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT, frequency_hz=10)
self.request_message_interval(mavutil.mavlink.MAVLINK_MSG_ID_SCALED_PRESSURE, frequency_hz=1)

    def update_vehicle_status(self):
        try:
            update = self.master.recv_match().to_dict()
        except:
            pass
        else:
            if update['mavpackettype'] == "GLOBAL_POSITION_INT":
                self.depth = update['relative_alt'] / 1000     # Original unit is mm
                self.is_depth_updated = True
            elif update['mavpackettype'] == "ATTITUDE":
                attitude = [update['roll'], update['pitch'], update['yaw']]
                self.heading = update['yaw'] * 180 / 3.14159
                self.angular_velocity = [update['rollspeed'], update['pitchspeed'], update['yawspeed']]
                self.orientation = list(QuaternionBase(attitude))
                self.is_orientation_updated = True
            elif update["mavpackettype"] == "SCALED_PRESSURE":
                # self.pressure = update['press_abs'] / 1000
                self.temperature = update['temperature'] / 100
                self.is_temperature_updated = True

    def run(self):
        rate = rospy.Rate(50) # hz
        while not rospy.is_shutdown():
            self.update_vehicle_status()

            if self.is_depth_updated:
                self.depth_pub.publish(Float32(data=self.depth))
                self.is_depth_updated = False

            if self.is_orientation_updated:
                self.publish_orientation(self.orientation)
                self.publish_pose([0, 0, 0], self.orientation)
                self.heading_pub.publish(Float32(data=self.heading))
                self.is_orientation_updated = False

            rate.sleep()

In fact, I request a few more types of message, but the general lines of code are like this. Using this code, I can receive attitude data at ~7hz (which is way lower than 20hz that I requested but I suppose it’s because I am requesting many types of messages); however, when I visualize the orientation, it usually lags behind the actual orientation of the vehicle. While I still receive new data, the data is not reflecting the current state.

4

The issue here is that you’re calling update_vehicle_status at 50 Hz, which only receives and processes a single message at a time, so you’re receiving telemetry messages at at most 50 Hz, and that’s split between the 31 messages you’ve explicitly requested per second, plus any that the autopilot is sending automatically.

The key piece of understanding here is that if you’re not receiving enough the MAVLink messages will build up, and (conversely), if you’re spending too long receiving then you’ll undershoot your desired rate when you add on the time required for your ROS code to run.

If rate.sleep() is purely a slow-down with a variable sleep call to keep the rate consistent then you could replace that with timeouts in recv_match calls, with message hooks like in the last option presented in the first post I linked to.

If ROS needs to receive and process things during that time though then you’ll need to split the “wait” time between ROS and pymavlink, in which case you can do the same calculation and just choose a proportion of the available remaining time, or go for a slightly simpler approach and wait for a fixed proportion of your desired rate as the MAVLink time (e.g. timeout=0.3/50 for 30% of 50Hz), at the risk of then not quite meeting your rate if the ROS portion takes longer than the allocated time.

1 Like
5

I didn’t expect that 50Hz is not fast enough, I thought it is already quite too fast.

rate.sleep() is also for ros to process subscriber/service callbacks. In that sense, I am not sure if using timeout in recv_match will be a good idea, as that would mean there’s an additional blocking function in the while loop.

I will try just requesting one type of message and see if there are any improments.

6

The point being that there are more than 50 messages being sent per second (because the autopilot sends several message types automatically, by default), so it’s better to receive several “at once” (per iteration) to keep up with the inflow, instead of just receiving the latest message in the buffer.

This likely won’t help unless you also disable the other messages the autopilot normally sends automatically, which could work for the stream groups, but I’m not sure is possible for the outgoing messages.

If you use a service like MAVLink2REST then it can filter messages for you, so you only need to request/receive specifically the types you’re after, but if you just connect to a standard MAVLink endpoint then it’s necessary to at least receive and parse all the headers of the messages the autopilot is sending, so you can choose to ignore the ones you don’t care about.

1 Like