Servers by jDrones

TFmini plus ,avoidance and arduino

HI there,

I’m new to this community even if I have already spent many hours reading :slight_smile:

I’m trying to feed my copter with 4 distance messages mesured from 4 TFMini plus sensors configured to work with I2C.
I didn’t find any way to configure multiple I2C lidar sensors (each one in a different direction) neither a driver compliant with arducopter, so I imagined the workaround: An arduino (or teensy) connected to a pixhawk telemetry port.

=> The arduino collects the TFmini plus values on the I2C bus ( DONE ! I can plot the 4 distances on the I2C bus. I can give the code if someone is interested)

=>The arduino sends msg_distance_sensor to the FC
(, “any sensor capable of providing distances using the MAVLink [DISTANCE_SENSOR] message”)

=> MissionPlanner should display the distances in the RADAR type window

I have integrated the mavlink 2 library to the arduino project:
- “” provides the data structure to be filled
- the “mavlink_msg_distance_sensor_pack” function and “mavlink_msg_to_send_buffer” should allow to send what I need to send.

I have validated I can receive telemetry from the FC to the arduino, but is there a way to log if my messages are correctly received by the FC? (RADAR window don’t display anything :frowning: )

Thanks for your help!

Have you looked at this


Yes, the choose of the sensor has been inspired by your project, but with the new generation of TFMini plus which can be configured to work in I2C. The next step should be to (understand how to and) develop a driver directly to be run by ardupilot and the pixhawk I2C port.

Did you find a way to get a trace of the messages you send?
Do the RADAR window shows the proximity values in stabilize mode not armed?


Yes, you need to CTRL-F and choose proximity in mission planner
You can look at proximity message as well in MavLink inspector (on the same CTRL-F screen) but is is not easy to read as the different vectors values keep on changing

When I look proximity in mission planner, it gives me the utrasonic rangefinder value in the proximity field. I found good values with QgroundControl.
For the TFmini plus, Benewake has provided a corrective update for the sensor’s firmware for I2C protocol (I had issues). Now I only have to merge the sensor script and the mavlink script.
For the next step, I have no idea how I could integrate & compile directly a new driver into the pixhawk firmware.

As I wrote above, you use Arduino to transmit the MavLink proximity message to the PixHawk , as I demonstrated (and documented ) with the POC.

@lucasdemarchi, I just find in github ardupilot sources, you did a driver for TFmini plus I2C in the AP_RangeFinder lib, do you know if it’s usable for multiple sensors? (2 TFMini plus in front, 2 on the sides, 1 back, each one configured and tested to work in I2C and all with the new firmware 02.00.03)

@Slyv06 as long as each of them is given a different address, yes.

I am currently in the process of building a new quadcopter using the cube orange on the standard carrier board running AC Dev 4.0 from (9/29/19). I have a TFmini plus mounted to the bottom of the copter facing down, it is plugged into GPS 2 (serial 4). I have set the following parameters to this:
SERIAL4_PROTOCOL to 9 (rangefinder).
SERIAL4_BAUD to 115 (115200).
RNGFNDA_TYPE to 25 (Benewake TFMiniPlus).
RNGFNDA_MAX_CM to 600 cm for outdoor recommended settings.
RNGFND_GNDCLEAR to what it is between copter OTG and distance to TFmini, approx 19.5cm.
RNGFND_MIN_CM to 30 cm.
I have been unable to get any values to show up in MP. I did hook it up to an FTDI cable on my PC and opened th GUI to make sure it was reading and working properly, which it was. I also notice that there was an option to set it up for pixhawk by checking the pix mode in GUI which I also did. My question here is do you need to set it up in pix mode for it to operate properly? See pages 12 and 13 for pix mode here.
I also followed this page here for setup. I will double check tonight that rx–>tx and tx–>rx just to make certain that it isn’t something that simple. I will also state that I am powering the unit directly from the gps 2 port. Any clarification on weather or not you need to set it to pix mode or leave it as it came from factory and if powering from GPS2 is advised, would be greatly appreciated. I am also unable to mess with the rangefinder settings under optional hardware despite the previous settings being set, in the MP HUD it gives me bad lidar health.

Thanks for your time!

TFMiniPlus is supported in I2C mode, not uart. Switch the sensor to I2C and connect it in the right port of your flight controller


thanks for the reply. why does it clearly show using a serial port in this setup page that is maintained by the ardu community? A lot of questions that are asked on here, the first thing anyone says is have you looked at the wiki well I looked at the wiki.
I choose to follow the instructions given to me by the community and they failed even though it clearly says and shows serial?

Community means that everyone is involved in testing, maintaining and updating the wiki… including YOU :wink: , so once you make it work correctly, it would be appreciated that you update the wiki with the TFMiniPlus, that is a new breed of the TFMini.

how do you edit the wiki to show such, sorry some of us in the community are not so great with github and don’t understand it even after reading the how to edit the wiki, wki! Again I fully expected this response that anyone in the community is responsible for editing the wiki but who validates the changes? If I may quote this page, " All changes to the wiki are reviewed by the wiki “maintainers” to help reduce the chance of misleading or incorrect information being posted." Who are these wiki maintainers as I am seeing an issue with stuff being published that is currently not correct yet it is published. It seems that there is often times frustration on some folks part when the wiki is published with misinformation and they have tried and tried and searched the forums to solve the problem to eventually find out that the page folks tell people to go look at for help is spreading misinformation. Will definitely report back if I am able to get the TFmini plus working on serial.

Much appreciate the help!!

At the bottom of each wiki there is a link for issues or suggestions
Questions, issues, and suggestions about this page can be raised on the forums. Issues and suggestions may be posted on the forums or the Github Issue Tracker.

Simply use it and a wiki maintainer will take care of it

Duly noted and issue has already been submitted, again appreciate your help on this topic.

Hi there! I have not gone forward with the use of TFmini plus but I noticed one thing: message for data request is different in I²c than UART:
In AP_RangeFinder_Benewake_TFMiniPlus.cpp from our C++ master @lucasdemarchi , I find " uint8_t CMD_READ_MEASUREMENT[] = { 0x5A, 0x05, 0x00, 0x07, 0x66 }; " which is correct for UART mode, however the correct message for reqesting data on I2C (in centimeter) would be :

CMD_RequestData[]={0x5A, 0x05, 0x00, 0x01,0x60};
(page 18 of the product manual, after having assigned an I2C address, saved, and switched to I2C mode)

After having confirmed the I2C way of running with an arduino nano (in attachement I2C.txt (9.4 KB)), I have tried connecting many tfm+ in I2C on my pixhawk but nothing happened in Mission Planner : No new device or option for assigning a sensor to an avoidance direction.

That’s all I investigated for now… (my baby and my studies are taking all the time :tired_face:)

I hope this will help!

Did you connect to tfmin p to the gui and change it over to pix mode when trying I2C. I cannot get this to work on i2c or uart. wondering if it doesn’t need to be in pix mode.

I have used a TTL to USB adapter to connect each TFM+, you have to choose your sensor and COM port and, if the GUI doesn’t show you any data, that’s surely because your device must be resetted ( “5A 04 02 60” in the “send” area),
Now it should plot measurement data. (you can play with different frequencies…)

After, you have to select an I2C address and calculate the checksum ( I did it in a worksheet exc l) and send it, then save it ( it seems to work also without saving).
Then switch to I2C mode with the Communication interface setup command. (Be warned that after that, any command must be sent in I2C mode! )

Then, with the “Obtain Data Frame” command, you can read the 9 bytes, verify checksum and parse the data.

No need to select PIx mode in I2C mode, if you send the “Obtain Data Frame” command for centimeter , it will send data in centimeter which is the correct format for the Pixhawk

That command is correct. As far as I remember there would be an updated manual showing this command for I2C.

Sorry but I don’t find this command in the manual(just DOwnloaded to be sure)

Servers by jDrones