Added drift acceleratometer based drift correction
The gear switch controls the on/off for the motors
Added correction for invalid receiver readdings (+-1000ns)
Fixed the accelerator reading signs