Delta Robot example
Dependencies: BufferedSerial Eigen
Fork of TCPSocket_Example by
syncTime.cpp@5:01e1e68309ae, 2018-10-15 (annotated)
- Committer:
- je310
- Date:
- Mon Oct 15 18:30:20 2018 +0000
- Revision:
- 5:01e1e68309ae
testing eigen;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
je310 | 5:01e1e68309ae | 1 | #include "syncTime.h" |
je310 | 5:01e1e68309ae | 2 | extern BufferedSerial buffered_pc; |
je310 | 5:01e1e68309ae | 3 | SyncTime::SyncTime( int seconds, int nSeconds ){ |
je310 | 5:01e1e68309ae | 4 | refTime.seconds = seconds; |
je310 | 5:01e1e68309ae | 5 | refTime.nSeconds = nSeconds; |
je310 | 5:01e1e68309ae | 6 | sinceRefTimer.reset(); |
je310 | 5:01e1e68309ae | 7 | sinceRefTimer.start(); |
je310 | 5:01e1e68309ae | 8 | |
je310 | 5:01e1e68309ae | 9 | } |
je310 | 5:01e1e68309ae | 10 | const int million = 1000000; |
je310 | 5:01e1e68309ae | 11 | const int billion = 1000000000; |
je310 | 5:01e1e68309ae | 12 | |
je310 | 5:01e1e68309ae | 13 | rosTime SyncTime::getTime(){ |
je310 | 5:01e1e68309ae | 14 | rosTime ret; |
je310 | 5:01e1e68309ae | 15 | uint64_t uSecs = sinceRefTimer.read_high_resolution_us(); |
je310 | 5:01e1e68309ae | 16 | ret.seconds = (int)(uSecs/million); |
je310 | 5:01e1e68309ae | 17 | ret.nSeconds = (int)((uSecs% million)*1000); |
je310 | 5:01e1e68309ae | 18 | ret.seconds += refTime.seconds; |
je310 | 5:01e1e68309ae | 19 | ret.nSeconds += refTime.nSeconds; |
je310 | 5:01e1e68309ae | 20 | if(ret.nSeconds >= (int)1e9){ |
je310 | 5:01e1e68309ae | 21 | ret.seconds ++; |
je310 | 5:01e1e68309ae | 22 | ret.nSeconds -= (int)1e9; |
je310 | 5:01e1e68309ae | 23 | } |
je310 | 5:01e1e68309ae | 24 | if(ret.nSeconds < 0){ |
je310 | 5:01e1e68309ae | 25 | ret.seconds --; |
je310 | 5:01e1e68309ae | 26 | ret.nSeconds += (int)1e9; |
je310 | 5:01e1e68309ae | 27 | } |
je310 | 5:01e1e68309ae | 28 | return ret; |
je310 | 5:01e1e68309ae | 29 | } |
je310 | 5:01e1e68309ae | 30 | |
je310 | 5:01e1e68309ae | 31 | void SyncTime::resetOffsetCounter(){ |
je310 | 5:01e1e68309ae | 32 | rosTime update = getTime(); |
je310 | 5:01e1e68309ae | 33 | refTime = update; |
je310 | 5:01e1e68309ae | 34 | sinceRefTimer.reset(); |
je310 | 5:01e1e68309ae | 35 | //buffered_pc.printf("resetting counter %dus\r\n", sinceRefTimer.read_high_resolution_us()); |
je310 | 5:01e1e68309ae | 36 | |
je310 | 5:01e1e68309ae | 37 | } |
je310 | 5:01e1e68309ae | 38 | |
je310 | 5:01e1e68309ae | 39 | void SyncTime::hardReset(int seconds, int nSeconds){ |
je310 | 5:01e1e68309ae | 40 | refTime.seconds = seconds; |
je310 | 5:01e1e68309ae | 41 | refTime.nSeconds = nSeconds; |
je310 | 5:01e1e68309ae | 42 | sinceRefTimer.reset(); |
je310 | 5:01e1e68309ae | 43 | sinceRefTimer.start(); |
je310 | 5:01e1e68309ae | 44 | } |
je310 | 5:01e1e68309ae | 45 | |
je310 | 5:01e1e68309ae | 46 | |
je310 | 5:01e1e68309ae | 47 | void SyncTime::updateTime(float correction){ |
je310 | 5:01e1e68309ae | 48 | int seconds = (int)correction; |
je310 | 5:01e1e68309ae | 49 | int nSeconds = (correction - (float)seconds) * 1e9; |
je310 | 5:01e1e68309ae | 50 | // buffered_pc.printf("time before %ds %dns \n\r", refTime.seconds, refTime.nSeconds); |
je310 | 5:01e1e68309ae | 51 | // buffered_pc.printf("updating by %ds %dns \n\r", seconds, nSeconds); |
je310 | 5:01e1e68309ae | 52 | refTime.seconds += seconds; |
je310 | 5:01e1e68309ae | 53 | refTime.nSeconds += nSeconds; |
je310 | 5:01e1e68309ae | 54 | if(refTime.nSeconds >= billion){ |
je310 | 5:01e1e68309ae | 55 | refTime.seconds ++; |
je310 | 5:01e1e68309ae | 56 | refTime.nSeconds -= billion; |
je310 | 5:01e1e68309ae | 57 | } |
je310 | 5:01e1e68309ae | 58 | if(refTime.nSeconds < 0){ |
je310 | 5:01e1e68309ae | 59 | refTime.seconds --; |
je310 | 5:01e1e68309ae | 60 | refTime.nSeconds += billion; |
je310 | 5:01e1e68309ae | 61 | } |
je310 | 5:01e1e68309ae | 62 | //buffered_pc.printf("time after %ds %dns \n\r", refTime.seconds, refTime.nSeconds); |
je310 | 5:01e1e68309ae | 63 | resetOffsetCounter(); |
je310 | 5:01e1e68309ae | 64 | } |