Avoidance Experiments with the POC and Benewake TFMINI

That is ok , then just make sure you set both serial speed correctly and get tx-rx connection. Then you should configure to read proximity message from MavLink
Proximity Type 2

Revisiting the POC with a Q330 and 5 TFMini covering full 180 Deg

Actually it is 6 TFMini there is one looking down acting as altimeter as well:


Avoidance set to 2 Meter

Here is the video of test (Sorry for poor quality and messy background…)


very cool as always
Keep up the cool and interesting work

1 Like

That’s very interesting ! Thank you for the update. :revolving_hearts:

I think TFmini Plus (12m, FOV 3.6°, 1-1000Hz, IP65) also works ?

Thank you
Yes this model would work better as we could speed up the sensor scanning to 1 khz

Great, is it possible to cover 360 with only 4?

YIf the sensor has large FoV, you certainly use it. But if you use TFMini that just covers 3-4 degs it makes the detection quite spotty.

We are currently working on POC using four TFmini Plus for the Ranger Q700. Still having some I2C issues with multiple sensors. The controller, sensor interface and MAVLINK interface are via Teensy 3.2. I’ll post the debugged system here when its tested properly.


@Arbutus Hi, for I2C issues, may be you could check the Github info and see whether it helps a little ?

@Siya, @Arbutus , Benewake support has direclty provided a new firmware 02.00.03 for TFmini plus and I have tested it with 5 TFM+ in I2C with arduino nano (post here) and it works very well. Maybe they will publish this new firmware soon?

Now, I think that using an arduino or teensy is useless with I2C sensors, so I hope someone mastering C++ could help to continue the development of the driver directly in ardupilot firmware.
@lucasdemarchi has created the files here for a direct connection to an I2C port on the FC, but I don’t know if it’s a good implementation (the .c file seem to run in UART mode, and the good request is an “on-demand” request/response (I²C sensors can’t speak on the bus when they want!), and no need to parameter the frequency of measurements (UART mode only).
Then, we would need to “map” I²C sensor(s) address(es) to an avoidance direction/quaternion in mission planner.

The new Benewake firmware is a great improvement. Most polling issues are resolved when sampling multiple I2C TFmini Plus with the Teensy providing buffering. @Slyv06 agreed - much better if multiple sensor array timing and sensor mapping was internal to Ardupilot.

@Slyv06 @Arbutus the implementation you mentioned is already part of ardupilot for some months now. You don’t need any teensy: you can connect them directly to a I2C port on your flight controller if you are running an updated version of the software.


Thank you Lucas for your answer. From what I experimented with arduino , the measurement request for I2C is { 0x5A, 0x05, 0x00, 0x01, 0x60 }; (in centimeter) , which is different from what I read in the cpp CMD_READ_MEASUREMENT function ( { 0x5A, 0x05, 0x00, 0x07, 0x66 }; is the output activation in UART mode for a single sensor). I’ll give a try with the code you did, and try to assignate a direction for each sensor.

Thank you for your project, thanks to him I taught the copter to see obstacles.
But there was a need to set the angle of the sensors is not 45 degrees.
In the messages DISTANCE_SENSOR description (https://mavlink.io/en/messages/common.html#DISTANCE_SENSOR) there is a quaternion field that specifies an arbitrary position of the sensor. You need mavlink 2 to use it. And then there are difficulties.
How do I send a message from quaternion over mavlink 2?
Have you had such an experience?


Thanks for this helpful tutorial. I am trying to do the same thing but reading from CAN interface sensor instead.
I have some questions:

  1. Could I feed the MAVLINK to serial 4? At the moment, I am successfully feeding the GPS to serial 4 using MAVLINK also, is it the problem is my code includes 2 messages (GPS_INPUT and DISTANCE_SENSOR)?

  2. Could you please show me what parameters I have to set in the Mission Planner? I am using Arducopter 3.6.11 in Cube

Many thanks!

Can you describe your setup ?
Picture schematic
What kind of can sensors
How are they connected
Can you read the distance values and how

Bear in mind that my setup is reading sensor and publish mavlink on serial

As for the setup, you can refer to the avoidance wiki.

I have an Arduino as intermediate stage for reading data from Radar sensor and sending MAVLINK messages to Arducopter.

At this time, I just want to feed a “dumpy” value of distance to Ardupilot by MAVLINK but I dont know how to set the right parameters in Mission Planner (such as PRX_TYPE, RGN_TYPE…).

It is in the wiki : http://ardupilot.org/copter/docs/copter-object-avoidance.html

When I connect Uart of Arduino to Serial 2 of CubeBlack (correct baudrate and MAVLINK chosen for Serial 2). In the wiki, it is just mentioned about the AVOID_ENABLE, AVOID_… How about the others?

It is very kind that you send me a parameters file for reference. Thanks.

1 Like