Fixed yaw angle calculation and added vertical pitch option

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