There should be `return RAD_TO_DEG * atan2(Yp - v_center, Fyp);` instead of `return RAD_TO_DEG * atan2(Yp - h_center, Fyp);`