joshua Elsdon / Mbed OS DeltaRobotFirmware

Delta Robot example

Dependencies:   BufferedSerial Eigen

Fork of TCPSocket_Example by mbed_example

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?

UserRevisionLine numberNew 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 }