Using Sonar for obstacle avoidance

Blue Robotics Software ArduSub
,
5

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

AVOID_ENABLE    3  
RNGFND1_TYPE   10     // mavlink
RNGFND1_ORIENT  0
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?