Servers by jDrones

Help with object avoidance

(Laa) #1

I am trying to figure out how to add IR sensors for object avoidance, but I don’t have much experience in coding. I read this page. I’m not really sure what I should do.
Say I have the sensors connected to my autopilot, what next? What should I do with the given AP_Proximity and AC_Avoidance libraries? Should I copy them somewhere? Should I add/remove something?
From where can I fill the DISTANCE_SENSOR messages, directly from Mission Planner? How can I send the message for each direction? Thank you in advance for your time and any detailed description is much appreciated.
I’m sorry if there’s already a detailed description somwhere, I just haven’t been able to find one.

Sonar or LIDAR as obstacle avoidance system
(ppoirier) #2


I have successfully implemented Avoidance in AltHold

If you look at bottom of wiki there is a reference to add support for cheaper proximity sensors:
You will see more details of implementation and the prototype I have developped using cheap VL53 Laser ToF Range finders I called the POC .

It is using an ARDUINO PRO MINI to send the MavLINK DISTANCE_SENSOR message for each direction the sensor is capable of. It is connected to the FC through Serial Port this port is configured 115200 Baud as a MAVLINK Type. Then the proximity library consume this message when you set proximity type to 2 (mavlink)
and the avoid library consumes the proximity sensor when setting the option to define AC_AVOID_USE_PROXIMITY_SENSOR 2 ( stop based on proximity sensor output)

All these are configured using the advanced options within mission planner.
Here is a demonstration of the POC in GPS denied (my garage) in AltHold mode:

(Laa) #3

Thank you for your reply!

Ardupilot page says: "…or with any sensor capable of providing distances using the MAVLink DISTANCE_SENSOR message."
How exactly do I make sure my sensors are capable of providing distances using mavlink distance_sensor message?

About your post in Github:

Each distance message is
int sysid = 1;
int compid = 158;
uint32_t time_boot_ms = 0; /< Time since system boot/
uint16_t min_distance = 1; /< Minimum distance the sensor can measure in centimeters/
uint16_t max_distance = 160; /< Maximum distance the sensor can measure in centimeters/
uint16_t current_distance = dist1; /< Current distance reading/
uint8_t type = 0;
uint8_t id = 1;
uint8_t orientation = 4;
uint8_t covariance = 0;

and for each message we associate distance , id and orientation of the associated sensor.
The actual setup is
dist 1 - id 1 - orient 4 == looking at 6 oclock
dist 2 - id 2 - orient 6 == looking at 9 oclock
dist 3 - id 3 - orient 7 == looking at 11 oclock
dist 4 - id 4 - orient 0 == looking at 12 oclock
dist 5 - id 5 - orient 1 == looking at 1 oclock
dist 6 - id 6 - orient 2 == looking at 3 oclock

So all of these parameters are configured via Mission Planner?

Sorry for any dumb questions, I am new to this :slight_smile:

(ppoirier) #4


Look here for all the parameter related to this subject (search for avoidance and proximity):
All these can be changed-adjusted through mission planner.

Concerning the DIstance Mavlink message, when you look at the message structure, you can see that in my prototype - POC- ,the Arduino is sequentially reading the 6 sensors (VL53L) and sending the distance and angle according to the orientation parameter (orient 0 - 7) on the serial port. This message is then read by the Flight Controller and passed to the proximity library.

Hope it can help, this avoidance system is still considered experimental and this is why there is not very much information available at the moment, I encourage you to experiment with the simulator -SITL- in order to master the principle of adding features and options to the Flight Controller, you can even simulate the POC by sending fake Mavlink DISTANCE SENSOR message using a Python Script.

(Laa) #5

Thank you for your reply.
So you use 6 Arduino pro mini boards, one for each sensor? are these the sensors?
You don’t happen to have a step by step tutorial for your system or a picture of the complete system?

(ppoirier) #6

Its actually 1 Arduino talking to multiple vl53l, each being read individually in sequence. Unfortunately I have not published any wiki or how-to yet, it is still in development but you can certainly experiment with this setup, there is lot of information available on the web. Other option is to use any commercial available product like the one showned here:

(Jiadong Guo) #7

hi Patrick,

I am having trouble reading the command.

As you already know, I use rplidar to generate sensor message and then send it through the mavros plugin upstream as MavLink message, type distance_sensor.

After setting the proximity sensor type and enable avoidance to 3, I still dont see QGroundControl Mavlink Inspector publishes DISTANCE_SENSOR type.

What do you see on your MavLink inspector? I am sure my topic is being published.

I cant really use mission planner because i need to manually run somethings first over linux.


(ppoirier) #8

Its Avoid 2 and Proximity 2

(Jiadong Guo) #9

I changed that now and I am tempted to test it. Could you share what you are able to read on QGC?

(ppoirier) #10

I’ll get the POC back to work later on today and upload pics

(ppoirier) #11

The Analyse tool makes no sense .
Using Mavlink Inspector gets more intelligible update

(ppoirier) #12

Too Bad You cannot Use Mission Planner, it gives such a great radar screen:

"Radar"/DISTANCE_SENSOR screen
(Jiadong Guo) #13

interesting. So there is only one DISTANCE_SENSOR showing up? The radar looks so professional

(ppoirier) #14

Mavlink Inspector is scanning (quite randomly) through the different id & associated orientations

(Fabrício Silva) #15

I couldn’t find where to enable this radar screen to be displayed on Mission Planner (1.3.48). How can I do it?

(ppoirier) #16

There is no setting , this radar pops automatically on a window if you have valid proximity sensor(s) configured and sending signal

(Fabrício Silva) #17

I’m having a hard time to get DISTANCE_SENSOR messages to be used, despite spending hours on this. I simply cannot validate that ArduCopter is using it.

I’ll describe my scenario below, in the hope that someone spots the missing piece or my fault.

I configured ArduCopter (3.4.6) with: AVOID_ENABLE=2 and PRX_TYPE=2.

I’m running a companion computer (RPi). There, I run cmavnode as as mavlink hub. cmavnode has 4 endpoints:

EP1: ArduCopter on Pixhawk, connected via serial (ttyAMA0, baud=921600).
EP2: Mission Planner (1.3.48) connected via UDP.
EP3: Custom built software (I call it LOGGER) that listens to messages and prints them, connected via UDP.
EP4: Custom built software (I call it SENDER) that generates DISTANCE_SENSOR messages, connected via UDP.

At first, I start cmavnode. It promptly reports that EP1 (ArduCopter) is connected with sysId 1.
Then, I connect EP2 (MP). cmavnode reports that EP2 is connected with sysId 255.
Then, I connect EP3 (LOGGER). cmavnode stays quiet, possibly because LOGGER does not send messages. LOGGER starts reporing various messages with sysId 1, presumably from Ardupilot and some debug messages with sysId 255 (from MP).
Then, I connect EP4 (SENDER). cmavnode reports that EP4 is connected with sysId 1. SENDER starts sending DISTANCE_SENSOR messages at 10Hz with sysId=1, compId=154, min_distance=4, max_distance=400, current_distance=120 and orientation=0. LOGGER starts reporing DISTANCE_SENSOR messages with correct values.

I don’t know how to validade that ArduCopter is receiving the messages and that it is using (accepting) them. Is there a way to do this? I expected that ArduCopter would send new DISTANCE_SENSOR messages to everybody else, but it doesn’t.

I don’t know how to validade that Mission Planner is receiving the messages and that it is using (accepting) them. @ppoirier said that the “radar” window should appear, but nothing happened. Mission Planner “Tuning” feature does not have"distance variables", so I could graph them. Is there a way to do this?

LOGGER and SENDER are written in Java, using MAVLinkJava library. They don’t send heartbeat messages.

What am I missing?

"Radar"/DISTANCE_SENSOR screen
(ppoirier) #18


Take note that the POC sends a Heartbeat


I dont use cmavnode, so I cannot comment on this part.
My preferred tool for debugging are QGroundControl Widgets : Analyse & Mavlink Inspector as showned on the picture above, you will be able to confirm the operation and values of Distance Sensor.

(Fabrício Silva) #19

Which values does POC send in heatbeat message parameters?
Which values does POC send in DISTANCE_SENSOR message parameters?

(Fabrício Silva) #20

Where can I find POC code? Is it opensource?