Copter: SuperSimple mode bug

SuperSimple bearing was updated without checking for GPS 3D Fix availability.
Copter: calculate home position and bearing when we have 2D Fix