Firmware with USQUE filter and simple PID control