Improvements on driver, actually tested with sensor. Added a few more utility programs