flight controller algorithm