PID control and mavlink_offboard fix