Refactoring, low pass filter and failsafe added.