Add the odometry telemetry to the existing opmode

[bsneade]

Be able to measure how far we have traveled. Publish the current measurements to the controller (telemetry interface).

• init the sensor, check status
• map the hardware to local variables
• in the while loop take a reading write the reading to the telemetry interface