Added slightly hacky way of measuring sensor sample rate

The timestamps of the two newest packets received from the arduino are used to
estimate this, giving a very noisy estimate. In any case, it seems to work :).