mc_pos_control: yaw offset initialization in FOLLOW mode fixed