Avoidance Experiments with the POC and Benewake TFMINI

RAZOR15000 · July 9, 2021, 8:55am

@rishabsingh3003 @ppoirier hello thank you for your kind reply.

i got able to working with the dev 4.1. the problem was, i was using onboard serial port. as soon as i switched to software serial, it worked fine.

how ever i have another question to ask.
in my setup, proximity is working and it properly indicates in the GUI. but im in need to get the data to display on the rangefinder section. is it possible to get the mavlink distance data to show up on the rangefinder (only one is enough, )
thank you
kind regards

rishabsingh3003 · July 9, 2021, 9:27am

@RAZOR15000 I am really not sure when I say this, mostly because I barely ever use MP. But I think those rangefinder sections are used for Downward facing rangefinders only (used for terrain following). This is just a guess though

RAZOR15000 · July 9, 2021, 9:37am

yes exactly , i need one for terrain following .

ppoirier · July 9, 2021, 10:16am

On my experiments I dedicated one of the sensors as a rangefinder and it is connected as a “standard” device directly to the FC and the signal appears as a rangefinder signal. The POC act as proximity and signals appears on the “radar screen”.

RAZOR15000 · July 9, 2021, 10:19am

yes sir, that will be the last choice. i was trying to save up the serial ports.
thank you very much for your reply.

ppoirier · July 9, 2021, 10:25am

Depending on the sensor model you can use I2C, check on my other blog on the TFMINI-s

rishabsingh3003 · July 9, 2021, 10:36am

@RAZOR15000 I don’t think I understand your problem properly.
DISTANCE_SENSOR has two uses depending on the sensor orientation that you have set (see https://mavlink.io/en/messages/common.html#MAV_SENSOR_ORIENTATION)

Use for avoidance as a proximity sensor
Use for terrain-following as a downward-facing range finder

If you want to use for “1” then the orientation field of DISTANCE_SENSOR has to be: between 0 - 7, depending on which direction the sensor is facing.

If you want to use for “2” then the orientation field of DISTANCE_SENSOR has to be: 25, so that the ArduPilot know the message is for downward-facing direction.

To see the DISTANCE_SENSOR fields: https://mavlink.io/en/messages/common.html#DISTANCE_SENSOR

RAZOR15000 · July 9, 2021, 10:46am

sorry for my bad communication.

i want to use the rangefinders for the proximity detection.

i just need to monitor the distance values from the ground station. in order to do that i need to guide the mavlink data from the arduino to this rangefinder sections,

but im struggling to do that.
proximity GUI is working fine when using the PRX_TYPE =2

ppoirier · July 9, 2021, 10:52am

As @rishabsingh3003 wrote, you can use Mavlink signal for both proximity and range finder , the drivers will filter the incoming signals depending on the orientation.

Here’s how the rangefinder is filtering the mavlink distance message

github.com

ArduPilot/ardupilot/blob/master/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp#L27


#include <AP_HAL/AP_HAL.h>


/*
   Set the distance based on a MAVLINK message
*/
void AP_RangeFinder_MAVLink::handle_msg(const mavlink_message_t &msg)
{
    mavlink_distance_sensor_t packet;
    mavlink_msg_distance_sensor_decode(&msg, &packet);


    // only accept distances for downward facing sensors
    if (packet.orientation == MAV_SENSOR_ROTATION_PITCH_270) {
        state.last_reading_ms = AP_HAL::millis();
        distance_cm = packet.current_distance;
        _max_distance_cm = packet.max_distance;
        _min_distance_cm = packet.min_distance;
        sensor_type = (MAV_DISTANCE_SENSOR)packet.type;
    }
}


int16_t AP_RangeFinder_MAVLink::max_distance_cm() const

ppoirier · July 9, 2021, 10:56am

Please note the reason why I dedicated a TFMINI as RangeFinder on I2C is because the POC generates a LOT of traffic and sometimes it affects the Telemetry channels. There a now method to alleviate this by keeping traffic local.

RAZOR15000 · July 9, 2021, 10:59am

thank you very much sir, this is great
it worked. highly appreciate the help

elia_ray_salem · July 19, 2021, 1:33pm

Hello @ppoirier,

I have 5 TFMINI here in Serial mode and i read practically every single post where you talk about the POC and all the implementation and feedbacks from other users but i am a bit lost.
Can the TFMINI be reconfigured to work in I2C so that i could attach them directly to my I2C port on the carrier board ? if yes could you you guide me a bit because i am a bit stuck !

thanks

rickyg32 · July 19, 2021, 1:38pm

So here is how to get the TF Mini Plus working on Pixhawk I2C.
Connect your TF Mini via the supplied cables to a TTL to USB adapter.

Connect via the USB cable to your PC.
If you have not already downloaded the TFMini Gui…do so from http://en.benewake.com/support
Open the Gui

Set the product type to TFMini Plus
Select the correct Com Port
Click connect.

You should start seeing distance information on the graph in the gui.

Using the command line function issue the following two commands by pasting each in the command line and pressing send

5A 05 0A 01 6A
5A 04 11 6F
The first sets the device to I2C and the second forces a save.
Nothing should appear on the graph from this point on.
Next you need to connect it to your Pixhawk or I2C hub.
Please note that the pinout on the supplied connector has the SCL and SDA reversed. You need to pull out the pins and swap them.

Please note the SCL and SDA are reversed.

You need to configure a few settings in Arducopter.
RNGFND1_ADDR,16 This is the I2C address for the Lidar
RNGFND1_GNDCLEAR,34 This the distance from the Lidar to the ground…easiest way to set it is to have the drone report the distance to the ground when sitting on your workbench and then enter that distance.
RNGFND1_MAX_CM,600 Set this as the max distance the device will be used.
RNGFND1_MIN_CM,30 This is the minimum distance for the lidar.
RNGFND1_ORIENT,25 This is telling Arducopter that the lidar is pointing down
RNGFND1_TYPE,25 This sets the lidar type to TFMini I2C
It should be working now.
Good luck

elia_ray_salem · July 19, 2021, 2:49pm

thank you very much for your prompt answer.

however i do not seem to have a TF Mini Plus !

As soon as i send the command 5A 05 0A 01 6A it says “Wrong Command, please check !”

However on the back of the PCB i clearly see “TF Mini V1.8”.

I have been tinkering with this the whole day unsuccessfully !

thanks again

elia_ray_salem · July 30, 2021, 8:08am

@rickyg32 thank you for your feedback…i had to dig a bit into it but what you suggested worked and the issue was on my side. the commands have to be sent in HEX in order to work.

I was able to change the address of my 4 TF MINI S on I2C and attached to my I2C port on my carrier board and can see them (they are working). I even tested them while flying and they are preventing the drone to crash (with a bit of delay though). I tried going straight into a big bush and at some point it stopped and did not let me go further even if my pitch stick was full up (which is awesome).

However i noticed a strange behavior in the quadcopter when landing/disarming and the moment i take off.
The system hardly disarms (i have to keep the stick in the disarm position for a while before it actually disarms) and motors have some small burst of throttle randomly. Secondly it is a bit unstable (it’s shaky and lean into one direction on either roll/pitch). I am sure that this is due to some interference on the TFmini but do you have thoughts on what to change in the parameters to make it smoother ?

thanks for your answer.

ppoirier · July 30, 2021, 8:57am

You can enable/disable avoidance using a rc channel so it dont interfere during takeoff and landing

elia_ray_salem · August 4, 2021, 3:09pm

Thanks for the support ! all works now much better !

I got one last thing that i am not able to do…terrain following/surface tracking using the downfacing TF02

something like in this tutorial https://ardupilot.org/copter/docs/terrain-following-manual-modes.html#terrain-following-manual-modes.
It does not seem to follow the shape of the terrain at all but keep a straight line movement without adjusting altitude.

do you have suggestion on that matter ? thanks

Dewa · August 7, 2021, 2:29pm

Hello sir, i have 2 or 3 lidar lite using i2c port
but if i connected all, just 1 lidar lite can reading in arducopter, any idea for that ?

ppoirier · August 7, 2021, 2:46pm

Please open a new thread with details of the configuration and sensors used

aaron · August 8, 2021, 10:46pm

Thanks for the Code Example.
It has served me as a base for my project.
We have connected 8 Sonic and 8 Lidar Sensors via I2C (through a multiplexer)
and sending the the mavlink distance message via serial.

I could not find the library you used to send the mavlink messages, or the ones i found would not compile.

i have used this library and GitHub - mavlink/c_library_v2: Official reference C / C++ library for the v2 protocol

I Hope someone finds this usefull: