AutoFocus Self Calibration for Cortex-M