In the ptp servo the adjustment of the time was too slow. When the
difference of the time with the master was less than 1 second, the
adjustment was done only with the frequency. In case if the difference
is 0.999s, it can take a very long time to adjust the time.
Changes made :
- Adjust with frequency only when time difference is below 20ms
- Check the timestamp validity before each calculation
- Create a new function to check the timestamps and use it on all
servos. This improve significantly the size of the code.
- Add verification in order not to set an invalid UNIX time