code cleaning, ready for navigation setpoint, add yaw control with rc in auto mode