Obstacle avoidance on pixhawk using sonar


As part of a Senior project I am attempting to implement obstacle avoidance on a DJI F450 quadcopter using the pixhawk, PX4, and 4 HC-SR04 ultrasonic sensor units. My idea to accomplish this is, a raspberry pi would interface the 4 sonar sensors and when a sensor had been triggered the pi would relay mavlink messages to the pixhawk to instruct it to navigate away from the object, and then give control back to the Manual RC input or Qgroundcontrol. I initially intended to utilize MAVproxy to allow the pi to send mavlink messages, however after seeing this UART mavlink repository:

I felt it was more suited to the task. Simply put, to me it seems it allows you to run an executable that, initiates offboard control, executes a list of mavlink commands, and then returns control the previous entity (qgroundcontrol, manual RC, etc.). In my inital testing I am currently running a C program on the pi that gathers sonar data and executes, system("./mavlink_control -d /dev/ttyAMC0") upon a sonar reading within a threshold, which executes a set_position mavlink command.

As you can probably tell I am very new to this and was hoping a more experienced developer could answer a few of my questions that I can’t seem to find the answer for:

are the axis directions in:
set_position(float x, float y, float z, mavlink_set_position_target_local_ned_t &sp)
in reference to the quads orientation as in, setting x = x+1.0 will cause the quad to move 1 meter forward or will it cause the quad to move 1 meter true north?

would I be better served to simply set the pitch/roll for a short period of time in the appropriate direction?

does this sound like a reasonable design for a rudimentary avoidance system?

Thanks so much for any help!


can anyone help me out?

In my view sonar is not quite reliable when used for obstacle avoidance. So probably should consider other type of sensors.

I will definitely keep that in mind. A big reason we choose these sensors is because of cost, as our budget provided to us is quite small. So id like to see if i can make them work. I believe that through output filtering, and shields on the sensors I can cut down on interference and eliminate the possibility for a false positive. It should also be noted that our system doest have to be super robust, were looking for some basic functionality here since this is just a prototype.

However I’m a bit more concerned with how I’m communicating from the Pi to the Pixhawk. Will Mavlink be fast enough to make corrections in time? Also is it safe to frequently switch in and out of offboard computer control mode as my design would be doing?

Mavlink should be plenty fast enough for this application.
The sonar should work well enough for the proof of concept you wish to demonstrate.
I don’t know of any reason that it would be un-safe, but then I have never tried it.

Thanks so much for your response, I’ll definitely post how it goes.

I have a video some early testing results for this system: (turn the quality up to 1080p)

Test Video

to describe what you see in the video:

1.)we take off in altitude control mode.

2.) the raspberry pi on the quad executes: system(./mavlink_control -d /dev/ttyACM0)
after one of the sonar sensors detects an object within 90 cm 7 consecutive times times

3.) within the mavlink_control I have mostly left the code the same as found on github:

with the exception that instead of moving 5 meters south west I have told the pixhawk to maintain its current position and altitude for 3 seconds with the following commands:

set_position( ip.x , // [m]
ip.y , // [m]
ip.z , // [m]
sp );

set_yaw( ip.yaw , // [rad]
sp );

4.) after we both freak out a little it seems to correctly hold its position for 3 seconds.

5.) then it goes back into altitude control and comes down

for the most part it appears that everything worked with the exception of the strange glitch right after the sensor is triggered does anyone know what this might be??

it appears to me that maybe the time it takes for the pixhawk to switch from RC controlled setpoints to mavlink controlled setpoints might cause the pixhawk to be confused for a short period of time.

we did this test a few times with similar results every time

I think you can probably have a reference check on this issue:

I’m sorry its taken me so long to respond in this post. We have had some success with our project but also some disappointing problems. in the videos below you can see one of the limitations. We have found some strangely long delay times when trying to pull data from the pixhawk over mavlink using the c_uart_example as a guide. Specifically, we’ve found that when trying to, for instance, obtain heading data the initial value we get will be the same as the last time we grabbed the value. then after the value has updated 2 - 3 times the value will be correct, occasionally taking up to and over 1 full second. I believe we must be doing something very wrong as these delays are terrible.

As rolandash stated above the sonar is not ideal and can occasionally have malfunctions itself. However I believe it worked decent for this system. the large mavlink delays are ultimately what caused us the most problems.

video 1

video 2

video 3


and also the issue I referenced earlier about the violent glitch in the position setpoint command has seemed to gone away. I think it may have been linked to bad GPS signal.

Nice video. Good to see it worked for you.

I am not quite sure where exactly the delay come from. A general suggestion is, in my view, you actually do not need an external Ras-pi to do this kind of job. You can put all logic into PX4 and the flight controller can certainly handle the sonar array, and that probably can help a lot to reduce the delay.

Of course then deeper knowledge of PX4 coding will be necessary.

Hello, I am working on a project that has a same problem statement on which you have worked.It would be very useful if you can share your experience like : Docementation, codes.