Fixed a couple issues with the driver code. The calibration routine is still providing rather strange performance. I need a more systematic way of testing this.