RC mode algorithm help required