Using Sonar for obstacle avoidance

I’ve got a sonar connected to a Pi that can process the data. I want to send the OBSTACLE_DISTANCE message to ArduSub. I have the message being sent from the Pi to ArduSub and I’d like to know what it takes to activate a minimal response from the sub. A response like just stopping.

I had something working in 4.0.1 based on the Copter code but I’ve upgrades to 4.1 and now the Copter code and the Sub code are more diverged.

Any suggestions? Anything that keeps me out of some obvious dead ends.



I would be interested in this as well. Sorry no ideas here, are you using a scanning or bottom tracking sonar? Or want to use both? We are working on a 360 LiDAR and radar altimeter on a multi rotor for water sampling in steep mining pits, the next project is for one of our subs for similar application.

Hi @Tautala,

Are you saying ArduSub 4.0 supports obstacle avoidance while 4.1 doesn’t, or are you saying you modified 4.0 to do so and are now looking to do something similar for 4.1?

Which Copter branch/version are you comparing to? I don’t believe we’ve touched the proximity stuff, so perhaps the current relevant code structure for Sub 4.1 is similar to Copter 4.1? I’d recommend checking the different branches.

If that’s not helpful I can try to get some insight from our main ArduSub maintainer, but they’re away for the next couple of weeks.

Sorry, I wasn’t clear and forget what I said. I was actually very easy to look in the copter code and see what I needed. It only needed a few lines in the ArduSub code. I might try to go back and see what I did and post it.

I did get hung up when trying to arm the sub but that wasn’t a code problem. I wasn’t starting the obstacle code to send the MAVLink message right away. The sub needs to be getting the OBSTACLE_DISTANCE messages before it arms.

It took a while to figure that out but it makes sense that it works that way.

Thanks for responding anyway.

1 Like

I have my sonar code set up so that it can send out an OBSTACLE_DISTANCE and/or DISTANCE_SENSOR message. I can see that ArduSub gets the messages but doesn’t take any action. The sub keeps moving forward. I can see the distances displayed on QGroundControl.

I’m a little confused about the setup. in AP_RangeFinder_MAVLINK.cpp the code says it only accepts the distance_sensor message when the orientation == MAV_SENSOR_ROTATION_PITCH_270. So I set it to that value.

I have the following parameters set

RNGFND1_TYPE   10     // mavlink
PRX_TYPE        4      // RangeFinder

I can see that AP_RangeFinder_MAVLINK::handle_msg and AP_Proximity::handle_msg get called.

Any idea

Also, how do I tell the sub how close it is allowed to get?

Hey @Tautala ,
Have you pushed this anywhere public? I would be happy to help making it as a PR in ArduSub.