Fixed PiEstimatorCpuBSP (numMessages in cleanup and added DEBUG option)