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

I loaded the software and scripts as per instructions (minors issues - typos that will be corrected in the text above), and the system worked as expected.

First outdoor test was conducted in the shade, facing side of building and results where generally OK. Will continue testing and tuning to get my setup rock solid in loiter before switching to guided. Confidence level was steady @ 100% throughout the test.

This is my test ‘‘mule’’ : Flamewheel 450 + CUBE + Jetson NANO :


Connected to the NANO - GPIO + FET Driver

LED turn on when level is HIGH, giving visual feedback without having to look at the GCS. Basically it is writing to GPIO using the User-Space Mapping : sys/class/gpio


Thanks to Thien ,I followed your blog and tested it,It works very well. I tested fly at giude and auto and smart return . But my copter loiter accuracy is still not very good. It drifts a little. Especially it can hardly be used outdoors. So I tried to turn t265 toward the ground and modify the matrix. When I start it, I let t265 point forward. But the location information of the copter is wrong. Are there any problems? Does the downward effect of t265 really improve?
for downward I noly modified
H_aeroRef_T265Ref = np.array([[0,1, 0,0],[1,0,0,0],[0,0,-1,0],[0,0,0,1]])

Hi @zleo, if the T265 is facing down with the USB port pointing to the right, the matrices should be like as follows:

H_aeroRef_T265Ref = np.array([[0,1, 0,0],[1,0,0,0],[0,0,-1,0],[0,0,0,1]]), 
H_T265body_aeroBody = np.linalg.inv(np.array([[0,0,-1,0],[1,0,0,0],[0,-1,0,0],[0,0,0,1]]))

I have added this configuration to the script’s example, you can also use the latest one.

Additionally, note that yaw angle can be off by a bit if the camera is completely flat when the script starts/ros node launches, so tilt the head up a bit at the beginning (in my experience tilting the nose up will provide the most consistent yaw).

@LuckyBird Thanks a lot,According to your instructions, I modified matrices to like this
H_aeroRef_T265Ref = np.array([[0,1, 0,0],[1,0,0,0],[0,0,-1,0],[0,0,0,1]])
H_T265body_aeroBody = np.linalg.inv(np.array([[0,0,-1,0],[1,0,0,0],[0,-1,0,0],[0,0,0,1]]))
or like this
H_aeroRef_T265Ref = np.array([[0,1, 0,0],[1,0,0,0],[0,0,-1,0],[0,0,0,1]])
H_T265body_aeroBody =np.linalg.inv(H_aeroRef_T265Ref)
and tilt forward copter before start script,but the tracking path in missionplanner is still incorrect ,
Is there anything else to pay attention to?

Just for confirmation, is your T265 facing down, with the USB port to the right of the vehicle (with respect to moving forward direction)? If it is, then the first set of matrices should be correct.

I usually tilt the copter’s nose up like below, to have a consistent yaw:

When the quad icon appears on the Mission Planner map, does it face to the north or to some random direction? Could you provide more details on how is the path incorrect?

T265 is facing down and quad icon to random direction

in two restarts tracking trajectory is to take the drone forward for 5 meters and then come back
I tilt the copter nose up 45 degrees until a confidence prompt appears.

That sounds like the problem with down-facing cases, as described in this issue. I would suggest you experiment with this a bit:

  • Test with various inclination angles (from small to large, like 10, 20, 30 then 45 degrees) at launch and see if there are any differences. For me, small angles like 10 or 20 degrees are enough.

  • You can also add a debug line into the code, just like this one and see whether the output data, specifically yaw angle, are the same between runs with the same starting angle.

  • If you can run ROS (see part 2 in particular), you will have more tools to debug the system, like RViz. realsense-ros uses librealsense so this issue still applies.

thanks I find Yaw rotates clockwise after run , tracking path problem should be related to this,I’ll add some debugging information and report any problems to you.

Hey Thien,

As I’m up to this stage, i thought i’d reply to your post here.

Again, thanks for the quick reply - everything seems to be working after copying the .so files over

Ill keep going and let you know how i go!

Thanks mate,


Just a quick update here
As part of next Lab ( 1. Lab 6 : Calibration and camera orientation for vision positioning with Intel T265.), @LuckyBird is presently working on both the Python Scripting and on the code (EKF , Time Synchronisation and others libraries). As we wrote above , this next step could take few weeks but in the mean time , we would like to provide everyone with a minimal support for Beta tests.

There is a couple of known issues with the T265, regarding the orientation, initialization and other intrinsic parameters that are issued on Intel’s github. It would be preferable to get these fixed in the Firmware instead of building ‘‘work around’’ ;-)… but we are aware that they might not get resolved during the GSoC period.

So I asked Thien if he could make a ''quick fix" to get the T265 used as a standalone positioning system (without use of Optical Flow) aided with Compass fusion. This requires a method to align internal references to Magnetic North so that we dont get random initial vectors at startup and the associated ‘‘toilel bowl effect’’. We will keep you updated on this.


Yesterday, I finally got the whole system working and tested it on a ground rover.

Firstly, congratulations; the results were nothing short of spectacular. In 6 years of testing non-gps localisation systems, I’ve never seen anything that performed as well as your system did.

Today i’m going to strap the system to a quadrotor and see how it performs in the air.

Ill let you know how it goes!

Thanks for your reliable support,



Glad it works well for you! Really looking forward to hear your results and experience with both ground and air performance.

Hello , i try it work, i receive VISION_POSITION_ESTIMATE message in Mision Planner , but the quadcopter is not appears on the map , i think the Ardupilot doesn’t send GPS Glitch and GPS Glitch Clear message ,what is the reason for that ? i folow the same arducopter parameters (only i use serial_2 baud 115 ) ? is it about flight modes ? you use guided mode ?

The GPS Glitch and GPS Glitch Clear messages sometimes are not sent by the FCU, even though the EKF still works fine.

You can always set EKF home directly from Mission Planner’s map (right click > Set Home Here > Set EKF Origin Here).

If that also does not work, you need to reboot the flight controller, either through Mission Planner or power cycle the vehicle.

I removed the IF condition (not check Glitch and GPS Glitch Clear message )to send SET_GPS_GLOBAL_ORIGIN message,but noting change .
I set EKF home directly from Mission Planner , also doesn’t work ;no drone icon
what strange I see some times the message “Need 3d fix” while I disabled gps!

Can you check if param AHRS_GPS_USE = 0 or 1? I believe this param also needs to be 0 for GPS to be disabled completely, which might be your issue.

tnx , i changed the Ardupilot version to a new (3.6.10) , before i used a 3.4 , now i can see the quadcopter icon .

Hey Thien,

I finally had time to test the 265 on a copter, the results were stunning - see the video.

I am however having issues creating a boot service. works fine if executed alone, but once executed through t265.service, returns errors regarding permissions.

any help would be much appreciated



@hugh Congrats on your result! That looks very stable indeed.

Regarding the permission issues, maybe you can run everything as root and see if the problem still persists.