Added extra yaw calculation checks to avoid teleporting facing the wall

Signed-off-by: Irmo van den Berge <bergerkiller@gmail.com>