Sending and recieving heartbeats to BlueROV from surface computer using pymavlink

Blue Robotics Software
1

Hi,

We are trying to use the BlueROV as a testbed for various autonomous underwater control algorithms, so I’m trying to connect to and fire thrusters on the BlueROV through a surface computer running Python.

Thus far, I’ve been able to connect to the ROV via a UDP port as mentioned here, and send commands to fire individual thrusters using MAV_CMD_DO_MOTOR_TEST as mentioned here.

However, when I try to send motor commands or read sensor data for an extended period of time, QGroundControl plays a sound saying “Communication lost” and it seems that I am no longer getting updated data.

I’ve read that you need to send and recieve heartbeat commands at at least 1Hz, so I attempted to execute the following code (inspired from here) in a loop while reading sensor data, but I’m still getting the “communication lost message.”

master.mav.heartbeat_send(
            6, #MAVTYPE = MAV_TYPE_GCS
            8, #MAVAUTOPILOT = MAV_AUTOPILOT_INVALID
            128, # MAV_MODE = MAV_MODE_FLAG_SAFETY_ARMED, have also tried 0 here
            0,0)

Does anyone know why I might still be struggling to connect with this setup? Do I need explicit code to receive heartbeats from the BlueROV as well? If so, is it anything more than simply master.wait_heartbeat()?

Thanks for the help in advance - I’m still new to this framework so I’m very much in the learning process.

2

Hi @joshholder, welcome to the forum :slight_smile:

Great to see you’ve looked into this before asking about it. This has been asked about a few times recently, and today I had a thought for a hacky workaround that would make it possible, which I’ve described (and implemented) here.

Are you using port 14550 for pymavlink, or have you set up a separate endpoint for it? Two programs aren’t supposed to be able to access the same port at the same time, and 14550 is what QGC uses by default.

Receiving heartbeats isn’t a requirement (since the other end doesn’t know or care who is listening to its heartbeats). master.wait_heartbeat() is the correct code to wait for one though, when it’s relevant to do so.

3

Thanks @EliotBR ! I’ll try using that workaround - it seems way more robust than using DO_MOTOR_TEST. I also changed my port from the one QGC uses, which fixed the issue of QGC saying disconnected.

However, even after doing the above and adding consistent heartbeats I’m still having issues reading sensor data. I’m grabbing attitude data using the following command:

att = self.master.recv_match(type=‘ATTITUDE’,blocking=True).to_dict()

Using this syntax, when I physically move the ROV the sensor readings I’m getting don’t seem to update, or if they do, they update by drifting to the correct values rather than actually tracking the movement. I’m not sure if this is even a heartbeat issue at this point, but is there any reason you can think of why my sensor readings would be off? I’ve also tried grabbing data from the AHRS ports, with the same result.

4

Not sure why your readings are delayed. I’ve just tried the following (both with and without QGC open) and didn’t have noticeable delay when moving the ROV around:

  1. Add a mavlink server at port 14660 in the mavproxy settings (http://192.168.2.2:2770/mavproxy):
    --out udpin:0.0.0.0:14660
  2. Press “Restart MAVProxy”
  3. Connect using pymavlink:
    from rc_override_example import Autopilot

# OR: with Autopilot.connect_to_server(ip='192.168.2.2', port=14660) as sub:
with Autopilot('udpout:192.168.2.2:14660', client=False) as sub:
    # press CTRL+C to quit
    while "testing attitude updates":
        # get the attitude
        msg = sub.recv_match(type='ATTITUDE', blocking=True)
        # extract parameters of interest
        roll, pitch, yaw = msg.roll, msg.pitch, msg.yaw
        # print to terminal/console
        print(f'{roll = :.3f}, {pitch = :.3f}, {yaw = :.3f}')

It also worked fine with a mavlink client instead of a server:

--out udpout:192.168.2.1:14660

connecting with

# OR: with Autopilot.connect_to_client(port=14660) as sub:
with Autopilot('udpin:0.0.0.0:14660', client=True) as sub:
    ...

If that doesn’t work for you/help with your issue, which versions of Companion and ArduSub are you using?

If it does help but you’d rather use your own code structure, feel free to post the code you’re using and I’ll see if I can spot any issues, or try to replicate the issues you’re having here :slight_smile:

5

@EliotBR My code was extremely similar to yours, just using a different port. However, even after switching to 14660, I’m still having issues with sensors not updating. Just in case though, here’s a version of my code that doesn’t seem to work:

from pymavlink import mavutil
import time

if __name__ == "__main__":
    # --out udpout:192.168.2.1:14660 in http://192.168.2.2:2770/mavproxy list
    master = mavutil.mavlink_connection('udpin:0.0.0.0:14660')
    master.wait_heartbeat()

    for i in range(800):
        #in real code this is sending in a separate thread
        master.mav.heartbeat_send(
                6, #MAVTYPE = 
                8, #MAVAUTOPILOT
                128, # MAV_MODE = 
                0,0) #MAVSTATE =

        att = master.recv_match(type='ATTITUDE',blocking=True).to_dict()
        print(att['roll'],att['pitch'],att['yaw'])
        time.sleep(.2)

According to http://192.168.2.2:2770/system, my companion computer software is version 0.0.17, and my ArduSub version is 3.5.4. I see here that the current version of the companion computer and ArduSub are 0.0.29 and 4.0.3, but I don’t see a button on http://192.168.2.2:2770/system which tells me to update the software, and given that I can’t seem to connect to wifi, I looked at this tutorial which says that ROVs purchased after June 2017 shouldn’t need to update. Given that, I’m not sure if you still think that software version could be an issue. If so, should I manually rewrite the SD card as described in the link?

6

Generally our first support response when someone has issues with software that’s older than the latest stable version is to ask them to update so we have an understood baseline to work from.

That post (which was also made in 2017) is specifically referring to updating to ArduSub 3.5. ROVs bought later than June 2017 would already be on ArduSub 3.5 or later, so they don’t need to update to ArduSub 3.5. That’s unrelated to the new releases we’ve made since then, including various bug fixes, improvements, and new features :slight_smile:

I’m not sure why you’re unable to access wifi, but there have been a few different issues with that for previous versions of Companion. Yes, I’d recommend you manually flash 0.0.29 onto your Companion SD card using our Installing Companion instructions :slight_smile:

Once that’s complete you’ll then need to:

  1. Connect Companion to wifi (should hopefully now be possible)
  2. Update Companion to 0.0.30 from the System page (should be quite a quick update; we haven’t built an image for 0.0.30 because 0.0.31 is planned for release very soon, but it fixes an external issue that stops ArduSub firmwares from downloading so is required)
  3. Update your ArduSub firmware to the latest Stable (4.0.3)

Your Actual Issue

While I would still recommend you upgrade your software, your actual issue here is the time.sleep(.2) call. Pymavlink is just receiving the mavlink messages that mavproxy sends (i.e. it’s not requesting anything), and attitude messages are generally sent faster than the 5Hz that you’re getting+printing messages at. That means your computer is collecting messages faster than you’re processing them, and each time you request the next one it gives you the oldest one it’s got (which leads to the large (and growing) delay between when you move the ROV vs when those moves show up in the printed values).

There are multiple ways of dealing with that, but the main principle is making sure you read the values faster than they’re arriving, so your program doesn’t fall behind. Likely the most intended way (including how the pymavlink developers would probably want heartbeats to be handled) is with periodic events that only trigger at their desired frequency:

from pymavlink import mavutil
import time

# --out udpout:192.168.2.1:14660 in http://192.168.2.2:2770/mavproxy list
master = mavutil.mavlink_connection('udpin:0.0.0.0:14660')
# ensure valid connection
master.wait_heartbeat()

heartbeat = mavutil.periodic_event(1) # 1 Hz heartbeat
display = mavutil.periodic_event(5) # 5 Hz output (every 0.2 seconds)

start = time.time()
duration = 160 # seconds

while time.time() - start < duration:
    if heartbeat.trigger():
        master.mav.heartbeat_send(
            mavutil.mavlink.MAV_TYPE_GCS,
            mavutil.mavlink.MAV_AUTOPILOT_INVALID,
            0,0,0)
    
    # read messages every iteration (to not fall behind)
    att = master.recv_match(type='ATTITUDE', blocking=True)
    
    if display.trigger():
        print(att.roll, att.pitch, att.yaw)

I would note that while having the heartbeat in a separate thread can definitely be convenient, it’s not recommended as the default approach in the mavlink docs, and is also the main issue that’s been raised with my mavactive.py proposal. The reasoning behind that is almost certainly that threads make debugging harder, and could in theory allow regular heartbeats to continue when the vehicle/controller is not in fact still ok/healthy.

I personally think that should be an individual choice (e.g. if you’re comfortable with threads and understand the potential pitfalls you could choose to use them when it’s appropriate), but I agree that threads aren’t perfect for heartbeats “in general”, and should be used carefully (if at all).

7

Thank you so much! You managed to identify the critical gap in understanding that I had - sensor readings as well as thruster firings are working as of now.

So to clarify, within the mavlink_connection() object, it stores a list of received messages. When calling master.recv_match(type=‘ATTITUDE’), it grabs the first received ATTITUDE message on the list, returns it, and then removes it from the list. By calling recv_match constantly, we keep the first entry on the list updated to the piece of data that was received most recently.

Follow-up question in this case: given that the list of received messages is always growing, is there some upper limit when messages start being culled from the list? Otherwise, wouldn’t the amount of data grow without bound?

Also, do you have recommendations for documentation sources for mavlink itself? I completely missed this subtlety in my research, and even now I’m not able to find great documentation on periodic_event() or other mavlink functions.

8

Great to hear! :slight_smile:

It’s not a list - it’s a stream of packets (the main difference being it’s only one-way - once you’ve received a message you can’t un-receive it, or go back through previous ones unless you’ve saved them yourself in some other data structure). Some pointers/notes:

  • Calling recv_match with a type check parses that stream:
    1. converts the earliest data into a message
    2. stores the message in the master.messages dictionary (keeps track of the latest message of each type)
    3. returns the message if it’s one of the requested types, otherwise continues parsing
    4. If it gets through all the data that it’s received (no more messages available to read) without finding the requested type then it returns None by default.
  • If you set blocking=True then instead of returning None when it runs out of data it does a blocking wait and just waits for more data until eventually the requested message type arrives (if it ever does).
  • If you set a timeout then it will read messages either until it runs out of them or until the timeout has been reached.
  • If both blocking and timeout are set then the timeout acts as an early-stop, that prevents endlessly blocking if the message isn’t arriving.
  • For completeness, calling recv_match with no type check just tries to get and return any message, if one is available, otherwise returns None (timeout and blocking apply as normal).

Your operating system has a default UDP buffer length, but it’s not a circular buffer (i.e. if it gets full then old packets aren’t overwritten - new ones are dropped). From what I can tell the pymavlink UDP option doesn’t attempt to override/specify a buffer length, so it will depend on your operating system.

There seems to be another slight misconception here - Pymavlink is a Python interface to the MAVLink protocol, they’re not the same thing (i.e. there are several other interfaces to the MAVLink protocol, in different programming languages). The MAVLink documentation is a bit tricky to navigate, but has reasonably good coverage of the mavlink protocol, available messages (common and ardupilotmega for ArduSub), and relevant micro-services.

Pymavlink provides several utilities to make working with MAVLink easier, but has quite minimal documentation, and the documentation that does exist is written as though you already know how MAVLink works and happen to want to use a Python implementation of it (which is the case for the main developers of Pymavlink, but makes it not particularly beginner friendly).

The idea is seemingly to “lead by example”, in the sense that some examples are provided in the pymavlink repository, and if you want more information about particular functionality then you should go look in the code for how it’s implemented. We (Blue Robotics) have our own set of ArduSub-specific examples for small snippets of functionality, but the rc_override_example gist I made a few weeks ago is the first ‘complete program’ example we have.

periodic_event is a Python class which I found (quite recently) while looking through the mavutil implementation file, although it’s also used in some of the Pymavlink repo examples. It’s not related to MAVLink at all (it’s just Python code that checks whether a specified amount of time has passed) - it’s in mavutil because it’s a useful utility for working with MAVLink connections.

Because of Python’s popularity, Pymavlink is a reasonably common starting point for people wanting to start interfacing with MAVLink devices. The lack of beginner friendly docs is becoming enough of a support issue on our end that it will likely end up being one of our/my priorities early next year to write some decent introductory documentation that makes it easier for people to get started. I’m not yet sure if that would be best done generally and contributed to the official Pymavlink documentation, or if it makes more sense to target it at ArduSub and put it in our own docs. At minimum, we very much agree that the current Pymavlink docs don’t make it super easy to get started with development, and that’s important enough to us and the users of ArduSub that we’re planning to help change/fix it :slight_smile:

1 Like
9

Hi @EliotBR,

I’ve found your reply here while searching about RC override via pymavlink for arduplane.
I’m trying to send RC overrides using a python script with pymavlink in RealFlight SITL. I noticed that RC input overrides are not matching RC outputs while flying in AUTO mode.

Here’s a brief walkthrough of what I did to investigate the issue:

  • I inputted a square PWM in AUTO mode preceded by the wait command: vehicle.wait_heartbeat() . RCOUT matches RCIN in this case. This was in SITL where the plane was stationary, and I was just testing RC overrides.

image

  • The, I tried the same setup while flying, but RC output did not match RC input.
10

image
image692×397 21.3 KB

Could there be any considerations or configurations that I’m missing?

I’d really appreciate your help.