I'm building a control system with a Parrot AR 2.0 drone where I have access to thrust controls for up/down (z), left/right (y), forward/backwards (x), turn left and turn right (yaw) through a Ruby library on my computer.
The goal of the system is to keep the drone a particular distance from and parallel to a wall while moving in the up/down and left/right directions. We have added two sonar distance sensors to the left and right forward props.
The main problem I am having is figuring out how the two distance sensors equal a yaw reading (ψ) so I can feed that into the PID and then take action on thrust to the turn left or right for correction.
Maybe just getting some help with the conversion from two distances to the yaw angle would be a big help, but any thoughts on the PID are greatly appreciated too since it is my first time working with it.