PID dev, MPU data fix and refactor