Calculating the distances

For calculating the distances I used some heuristics. First of all, I don't care how much the copter is tilted in the x- or y-axis, since our copter should only tilt by a few degrees. So calculating the distances to the walls only happens in one dimension, which is much easier. Further, to calculate the top and bottom distances, we don't really care about our tilt. Once again, this makes the calculations much easier and faster.

I also used another heuristic for measuring the distances. To avoid having to measure the distance to every wall from every point a sensor can be, we use a state machine to simulate being turned a certain way. If we are between -45° and 45° turned in yaw, we face the front wall. So the front sensor only measures the distance to the front wall. This of course does not mean that it measures the distance to the closest wall in his line of sight (imagine if he is in the back right of the room and turned 40° to the right. The front sensor should measure the distance to the right wall, but instead measures the distance to the front wall). As an explanation, this will still be safe enough, since the sensor facing to the right will output a distance closer to the wall then the front sensor. This way the copter should put distance between himself and the wall. If the copter then turns a little more, his front sensor will now face the right wall, and output the distance to this wall.

The state machine

This is pretty self explanatory if you understood the text before.

Distance Calculations

This is a representative calculation of the distance measurement while facing front. This is fairly standard pythagorean geometry.

Update: To accommodate the mistake here, I just subtracted 45° from the yaw-angle output to the distance calculations. I could have let it be and just called the outputs differently, but I wanted to keep the current outputs.