Integration of ArduPilot and VIO tracking camera (Part 4): non-ROS bridge to MAVLink in Python

Yes, the setup is the same as described in the page: T265 + RPI3b connected to the FCU. I’m using Arducopter v4.0.7 running on a Pixhawk4. I have set the parameters as described in the wiki (EK2_GPS_TYPE = 3, GPS_TYPE = 0) to avoid gps usage. The GPS hardware is connected to the FCU as in pixhawk 4 this module includes the safety switch to arm the quadcopter.

You should use EKF3 for this configuration and disable GPS as well with EK3_GPS_TYPE=3

The matrices (1) and (3) are H_aeroRef_T265Ref and H_T265body_aeroBody , respectively, and are defined in the earlier part of the code . You can figure out the meaning of their names in a similar way to (2) . Their values depend on the initial orientation of the camera and do not change throughout the operation.

Is that true order about variable name (1) H_aeroRef_T265Ref, (3) H_T265body_aeroBody? or vice versa

What the effect if we are not sending vision_speed_estimate to FCU or vice versa ?

Hello, I’m stuck when running the code t265_to_mavlink.py. I’m using an intel NUC and it is already running Mavlink router, which means that I can connect to it with MP in the GCS. Once I run the aforementioned python script it gives me the following error: serialException: device reports readiness to read but returned no data (device disconnected or multiple access on port?)
I tried to stop Mavlink router and the error doesn’t arise anymore: the problem now is that I can’t anymore connect to the drone with MP, it says “connection failed”.
Do you have any idea?

Looks like a USB ressource conflict… How do you connect Mavlink ?

My px4 mini 3dr is connected through micro usb to the USB 3.0 of the intel NUC
Then to activate it I run the following command: sudo systemctl start mavlink-router

And when you start the router the camera get the error?

What about realsense viewer , is it crashing too?

realsense viewer start without any problem and it works even with MavlinkRouter running.
The problem arises when both the MavlinkRouter and the script t265_to_mavlink.py run together. I get the following error:

~/Documents/vision_to_mavros/scripts$ python3 t265_to_mavlink.py --connect /dev/ttyACM0 --baudrate 57600
INFO: Using connection_string /dev/ttyACM0
INFO: Using connection_baudrate 57600.0
INFO: Using default vision_position_estimate_msg_hz 30.0
INFO: Using default vision_position_delta_msg_hz 30.0
INFO: Using default vision_speed_estimate_msg_hz 30.0
INFO: Using camera position offset: Disabled
INFO: Using compass: Disabled
INFO: Using default scale factor 1.0
INFO: Using default camera orientation 0
INFO: Starting Vehicle communications
INFO: Connecting to camera...
INFO: Camera connected.
INFO: Sending vision messages to FCU
INFO: Tracking confidence: Medium
Exception in thread Thread-1:
Traceback (most recent call last):
  File "/usr/lib/python3.8/threading.py", line 932, in _bootstrap_inner
    self.run()
  File "/usr/lib/python3.8/threading.py", line 870, in run
    self._target(*self._args, **self._kwargs)
  File "t265_to_mavlink.py", line 264, in mavlink_loop
    m = conn.recv_match(type=interesting_messages, timeout=1, blocking=True)
  File "/home/wines-nuc10/.local/lib/python3.8/site-packages/pymavlink/mavutil.py", line 457, in recv_match
    m = self.recv_msg()
  File "/home/wines-nuc10/.local/lib/python3.8/site-packages/pymavlink/mavutil.py", line 420, in recv_msg
    s = self.recv(n)
  File "/home/wines-nuc10/.local/lib/python3.8/site-packages/pymavlink/mavutil.py", line 965, in recv
    ret = self.port.read(n)
  File "/home/wines-nuc10/.local/lib/python3.8/site-packages/serial/serialposix.py", line 595, in read
    raise SerialException(
serial.serialutil.SerialException: device reports readiness to read but returned no data (device disconnected or multiple access on port?)
^CClosing the script...
INFO: Realsense pipeline and vehicle object closed.

If I don’t execute MavlinkRouter there is no error but I’m not able to connect MP to the drone, and so I’m not able to view the VISION_POSITION_ESTIMATE data in the MAVlink inspector. It seems that both the python script and mavlink router want to use the same USB resource.

To recap: I’m trying to connect to the NUC to MP in my GCS (in my Windows PC) using TCP or UDP connection through the WIFI (looking the guide of this project I suppose you did the same). Is my setup correct (MavlinkRouter + t265_to_mavlink.py)? or do you use something else to connect the RPi (in my case the intel NUC) to MP in the GCS?

Ahhh OK, you are not using the t265_test_streams.py as you show at the beginning

This is quite normal that you are conficting with the t265_to_mavlink.py as it tries to initiate Mavlink from the script == You obviously have to disable one of them :wink:

Oh yes sorry, I just noticed my error. Thankyou for your answer, so I stop the MavlinkRouter with sudo systemctl stop mavlink-router and run the t265_to_mavlink.py code with the same Device (/dev/ttyACM0) and Baud (921600) found in /etc/mavlink-router/main.conf file:
python3 t265_to_mavlink.py --connect /dev/ttyACM0 --baudrate 921600
Now I’m not getting any error from the NUC

/Documents/vision_to_mavros/scripts$ python3 t265_to_mavlink.py --connect /dev/ttyACM0 --baudrate 921600
INFO: Using connection_string /dev/ttyACM0
INFO: Using connection_baudrate 921600.0
INFO: Using default vision_position_estimate_msg_hz 30.0
INFO: Using default vision_position_delta_msg_hz 30.0
INFO: Using default vision_speed_estimate_msg_hz 30.0
INFO: Using camera position offset: Disabled
INFO: Using compass: Disabled
INFO: Using default scale factor 1.0
INFO: Using default camera orientation 0
INFO: Starting Vehicle communications
INFO: Connecting to camera...
INFO: Received first ATTITUDE message with heading yaw -0.36 degrees
INFO: Camera connected.
INFO: Sending vision messages to FCU
INFO: Tracking confidence: Medium
........

but in MP if I try to connect to my NUC using TCP or UDP I still get “Connection Failed” as the NUC is unreachable.

Yes this is normal as you dont route the signal to UDP/TCP , what I was suggesting (maybe it was not clear enough) it to disable Mavlink from the t265_to_mavlink.py

Ok thank you, it makes sense. Any idea how to disable it from the script? What I think is that (please tell me if I’m going on the wrong way) in t265_to_mavlink.py there is the following piece of code

conn = mavutil.mavlink_connection(
    connection_string,
    autoreconnect = True,
    source_system = 1,
    source_component = 93,
    baud=connection_baudrate,
    force_connected=True,
)

that creates the MavlinkConnection and saves it in the “conn” variable. The variable is used multiple times in the code to send messages. If I comment this line, of course the variable is no anymore available. How can I avoid this problem?

Yeahhhh… looking at it, you will have to recreate the message externally … too complicated
You need to change the parameter of your mavlink-router to read from internal network udp 127.0.0.1 (where the MavLink signal should already exist and out to whatever UDP/TCP stack and address you have configured for Mission Planner

ok thanks, so I went to the /etc/mavlink-router/main.conf file and I removed the connection with the ttyACM0. Looking online I found some values to try, like:

[TcpEndpoint delta]
Address = 127.0.0.1
Port = 25790

[UdpEndpoint local]
 Mode=normal
 Address=127.0.0.1
 Port=14550

[UdpEndpoint charlie]
Mode = Normal
Address = 127.0.0.1
Port = 11000

but I’m still not able to connect with MP (I tested UDP conn with all UDP ports mentioned above).
Do you have any idea on which values to write?

If the script t265_to_mavlink.py doesn’t route the signal to UDP/TCP, in the original project how were you connecting to the RPi with MP?

Most of the time we were using rfd900 serial radio, but you can easily connect through the NUC using the Mission Planner Staion UDP address, something like 192.168.xx.yy

I finally managed to connect Mavlink with MavlinkRouter, thanks for the help.
Does this setup also allow autonomous navigation with Waypoints? (I ask because I didn’t see any example in the guide) If yes, have you tested with some particular library?

Yes it can, I suggest you start in guided mode , setting waypoint using the right mouse on the Map Screen

Beware of the T265 getting lost and be ready to regain control in manual mode