Fix for-loop scoping error
Move sensor stuff into cpp file

Some calibration routines