float val = getSensorAngle();

if (val<0) // sensor angles are strictly non-negative. Negative values are used to signal errors.

return; // TODO signal error, e.g. via a flag and counter

angle_prev_ts = _micros();

float d_angle = val - angle_prev;

// if overflow happened track it as full rotation

if(abs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1;

angle_prev = val;

// calculate sample time

float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6f;

if (Ts < 0.0f) { // handle micros() overflow - we need to reset vel_angle_prev_ts

vel_angle_prev = angle_prev;

vel_full_rotations = full_rotations;

vel_angle_prev_ts = angle_prev_ts;

return velocity;

}

if (Ts < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small

velocity = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts;

vel_angle_prev = angle_prev;

vel_full_rotations = full_rotations;

vel_angle_prev_ts = angle_prev_ts;

return velocity;

// initialize all the internal variables of Sensor to ensure a "smooth" startup (without a 'jump' from zero)

getSensorAngle(); // call once

delayMicroseconds(1);

vel_angle_prev = getSensorAngle(); // call again

vel_angle_prev_ts = _micros();

delay(1);

getSensorAngle(); // call once

delayMicroseconds(1);

angle_prev = getSensorAngle(); // call again

angle_prev_ts = _micros();