Working, but not perfect simulation code of the RC Pilot Controller. Code works for the square trajectory.