Help with collision avoidance

First of all, hello to everyone.
This is my first post in this site, as I just discovered Ardupilot a couple of weeks ago.
Right now I’m trying to do a school project with a drone and some ultrasonic sensors, what I’m trying to do is to avoid collision in a indoor environment.
I used a PIC microcontroller to handle all the ultrasonic sensors, and sent all the measures through the serial port, which is connected to a Pixhawk.
Until this point everything is going pretty fine, the pixhawk is receiving all the data correctly, but once I got into this point I don’t really know how to continue, as my knowledge of AC is pretty small.
I’m not trying to do something really fancy, more something like this:

If any of the data is below a certain threshold, I would want the drone to just pull back a certain distance at the other direction.
I thought about creating a new flight mode, based on an existing one like POS_HOLD or STABILIZE and adding some additional code that would compare the data, and execute the collision avoidance sequence if needed.
I know that the motors are controlled with a pwm, and that pwm value is set in the AP_Motors library, but there are so many things in that library that I’m getting pretty lost, and don’t know what to do.
Any help would be really appreciated.
And sorry if my english was kinda potato

I am also very interested in the same topic, do you have answer now,can you share with me? Thanks!