How to Send Position Estimate to Multiple Drones

ArduCopter Copter 4.0
1

Hello, new here.

I am doing external positioning (no GPS) for multiple drones (with different SYSID_THISMAV parameter values), and currently I am using a companion computer (on each drone) to forward positioning results to drone FCU via serial as VISION_POSITION_ESTIMATE messages.

The problem is, each drone is receiving all other positioning results meant for other drones, which is counterproductive.

From Overview · MAVLink Developer Guide I see that only some messages are sent point-to-point, while others are broadcasts. And from VISION_POSITION_ESTIMATE is a broadcast message, it seems like VISION_POSITION_ESTIMATE belongs to the latter.

Is it possible to send VISION_POSITION_ESTIMATE to a specific drone? Or are there other methods (that I’m oblivious of) of sending external positioning data via MAVLink?.

Please let me know if you need more information, thank you.

2

As you noted, VISION_POSITION_ESTIMATE (and all other non-GPS positioning messages) are broadcast messages.

You’ll need likely need to have separate communication links to each drone, or develop a custom application to filter the appropriate packets.

How is your drone communications network set up?

3

Thanks for the response.

Right now we are just ssh-ing (over wifi) onto the companion computer, connected to FCU via serial2, to run pymavlink to communicate with the FCU.

It turns out that, the radio module on my drone, connected to FCU via serial1, is broadcasting mavlink messages. I wasn’t aware of that till now. For now, we are setting SERIAL1_PROTOCOL to None to disable this broadcasting. I think I need to recheck the wirings to see if there’s any other things that I missed regarding hardware.

Will doing this affect other functions of the drone? I’m not very familiar with radio. From what we are seeing, RC seems to works fine, but I’m not absolutely sure.

4

By default, Ardupilot will bridge MAVLink data between the serial ports.

It sounds like you’ll want to disable this feature. You can do this via setting SERIAL1_OPTIONS and SERIAL2_OPTIONS to both 1024 (see Complete Parameter List — Copter documentation).

5

Thanks for the pointer! However, I did a test on two drones: after setting SERIAL1_OPTIONS and SERIAL2_OPTIONS to 1024 on both, VISION_POSITION_ESTIMATE from one can still be read from the other.

I’m checking this by emitting messages on the RPi one drone, and connecting to the FCU of other drone over serial0 (USB cable) via MissionPlanner, and checking MAVLink Inspector. Do you have a clue on what might be the cause?

6

Hmm … that’s odd. I’ve just tested here. I put a MAVLink peripheral on TELEM2, set SERIAL2_OPTIONS to 1024 and (after a reboot) I could no longer see the messages from the peripheral being forwarded to serial0 (usb).

Could you post a diagram/description of your mavlink network?

7

Here’s a diagram:

photo_2024-04-29_15-44-11_edit
photo_2024-04-29_15-44-11_edit1097×768 130 KB

On drone 2, the companion computer sends VISION_POSITION_ESTIMATE via serial2 to FCU. After setting SERIAL1_OPTIONS and SERIAL2_OPTIONS to 1024 on both FCUs, MissionPlanner can still see messages from CC of drone 2. After setting SERIAL1_PROTOCOL to None on either FCU I stop seeing messages.

8

That setup looks about right. Are you able to share your code (or a relevant snippet) for sending the VISION_POSITION_ESTIMATE messages?

9

Sorry, this is as much as I can share, but I think these are all of the relevant parts:

import rospy
from pymavlink import mavutil
from geometry_msgs.msg import PoseStamped

def connect_fcu(serial_port, baud):
    rospy.loginfo('[MAV] Waiting FCU for connection...')
    conn = mavutil.mavlink_connection(serial_port, baud=baud)
    conn.wait_heartbeat()
    rospy.loginfo("[MAV] Heartbeat from system (system %u component %u)" %
        (conn.target_system, conn.target_component))
    return conn

def position_estimate_CB(conn, msg):
    x, y, z = enu_to_ned(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z)
    roll, pitch, yaw = 0, 0, 0

    conn.mav.vision_position_estimate_send(
        int(msg.header.stamp.to_nsec() / 1000), # usec
        x, y, z, roll, pitch, yaw)

def main():
    rospy.init_node(NODE_NAME, anonymous=True)
    conn = connect_fcu(serial_port='/dev/ttyS0', baud=115200)
    sub = rospy.Subscriber(EST_TOPIC, PoseStamped, (lambda msg: position_estimate_CB(conn, msg)) )
    rospy.spin()

We are using ROS1 for passing the positioning data. Each CC runs its own rosmaster.

10

To confirm, are you using the latest pymavlink and ArduCopter versions?

11

We are on ArduCopter 4.0.3 (not latest) and pymavlink 2.4.41 (latest).

12

There’s your problem. That (very old) version of Ardupilot doesn’t support not-bridging of the MAVLink ports (SERIALn_OPTIONS = 1024). You’ll need to upgrade to a newer version.

EDIT: It appears that support was added in ArduCopter 4.1.0

13

We had hardware issues and had to stick with an old version of Ardupilot, though we’ve since moved on to something else (our current setup). Guess it’s a good time to (finally) upgrade.

Thanks again for your time on this issue.