Add calib mode, final demo stuff