diff --git a/inc/iso22133object.hpp b/inc/iso22133object.hpp index d30f6be..344cb95 100644 --- a/inc/iso22133object.hpp +++ b/inc/iso22133object.hpp @@ -164,7 +164,7 @@ class TestObject { std::chrono::milliseconds maxSafeNetworkDelay = std::chrono::milliseconds(200); //! Used to get estimated network delay - std::chrono::milliseconds getNetworkDelay(); + std::chrono::microseconds getNetworkDelay(); ISO22133::State* state; private: @@ -194,7 +194,7 @@ class TestObject { void checkHeabTimeout(); //! Set estimated network delay from HEAB times - void setNetworkDelay(std::chrono::milliseconds); + void setNetworkDelay(std::chrono::microseconds); //! Get the Next message counter to send char getNextSentMessageCounter() { return sentMessageCounter = (sentMessageCounter + 1) % 256; } @@ -242,7 +242,7 @@ class TestObject { std::atomic osemReceived { false }; std::atomic on { true }; - std::chrono::milliseconds estimatedNetworkDelay = std::chrono::milliseconds(0); + std::chrono::microseconds estimatedNetworkDelay = std::chrono::microseconds(0); }; } // namespace ISO22133 diff --git a/src/iso22133object.cpp b/src/iso22133object.cpp index 5a62441..2fbd93a 100644 --- a/src/iso22133object.cpp +++ b/src/iso22133object.cpp @@ -395,8 +395,8 @@ void TestObject::handleHEAB(HeabMessageDataType& heab) { // Requires the system clocks of ATOS // and object to be synced!! auto heabTime = seconds(heab.dataTimestamp.tv_sec) + microseconds(heab.dataTimestamp.tv_usec); - auto networkDelay = system_clock::now().time_since_epoch() - heabTime; - setNetworkDelay(duration_cast(networkDelay)); + auto networkDelay = duration_cast(system_clock::now().time_since_epoch()) - heabTime; + setNetworkDelay(networkDelay); if (networkDelay > maxSafeNetworkDelay) { std::stringstream ss; @@ -426,15 +426,15 @@ void TestObject::handleHEAB(HeabMessageDataType& heab) { return; } -std::chrono::milliseconds TestObject::getNetworkDelay() { +std::chrono::microseconds TestObject::getNetworkDelay() { std::scoped_lock lock(netwrkDelayMutex); if (awaitingFirstHeab) { - return std::chrono::milliseconds(0); + return std::chrono::microseconds(0); } return estimatedNetworkDelay; } -void TestObject::setNetworkDelay(std::chrono::milliseconds delay) { +void TestObject::setNetworkDelay(std::chrono::microseconds delay) { std::scoped_lock lock(netwrkDelayMutex); estimatedNetworkDelay = delay; } diff --git a/src/iso22133state.cpp b/src/iso22133state.cpp index e2a6b56..465ed35 100644 --- a/src/iso22133state.cpp +++ b/src/iso22133state.cpp @@ -163,18 +163,17 @@ void ISO22133::State::handleSTRT(TestObject& obj, StartMessageType& strt) { struct timeval diff; timersub(&strt.startTime, ¤tTime, &diff); - uint32_t diffmySec = diff.tv_sec*1e6 + diff.tv_usec; - int diffint = diff.tv_sec*1e6 + diff.tv_usec; - - // Start time already passed. Request abort from Control Center - // resolution is 0,25ms (250 microseconds) in ISO spec. - if(diffint > -250) { + int diff_us = diff.tv_sec*1e6 + diff.tv_usec; + + const int tolerance_us = -1000; + if(diff_us > 0) { + std::stringstream ss; ss << "Got STRT message with start time in " << diff.tv_sec << " seconds, " << diff.tv_usec << " mySecs. Waiting" << std::endl; std::cout << ss.str(); - obj.delayedStrtThread = std::thread([&, diffmySec]() { - std::this_thread::sleep_for(std::chrono::microseconds(diffmySec)); + obj.delayedStrtThread = std::thread([&, diff_us]() { + std::this_thread::sleep_for(std::chrono::microseconds(diff_us)); // Order matters here, below changes state // causing the signal to not be triggered if placed // after the handleEvent() calls @@ -182,11 +181,20 @@ void ISO22133::State::handleSTRT(TestObject& obj, StartMessageType& strt) { this->handleEvent(obj, ISO22133::Events::S); }); } + else if(diff_us < 0 && diff_us > tolerance_us) { + + std::stringstream ss; + ss << "Got STRT message in the past (" << diff_us << " mySecs) but within tolerance (" << tolerance_us << " mySecs). Starting immediately." << std::endl; + std::cout << ss.str(); + obj.strtSig(strt); + this->handleEvent(obj, ISO22133::Events::S); + return; + } else { std::stringstream ss; ss << "Got STRT message with start time in the past. Requesting abort." << std::endl; ss << "Requested time: " << strt.startTime.tv_sec << " seconds, " << strt.startTime.tv_usec << " mySecs." << std::endl; - ss << "Current time: " << currentTime.tv_sec << " seconds, " << currentTime.tv_usec << " mySecs." << std::endl; + ss << "Current time w network delay compensation: " << currentTime.tv_sec << " seconds, " << currentTime.tv_usec << " mySecs." << std::endl; ss << "Estimated network delay: " << obj.getNetworkDelay().count() << " mySecs." << std::endl; std::cout << ss.str(); uint8_t error = 0;