Update to calibration routine and DMP setup

Better calibration and also setup FSR to keep DMP happy