Optimize and fix PPM to control ESC