Fix start time delay

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<bool> osemReceived { false };
 	std::atomic<bool> on { true };
 
-	std::chrono::milliseconds estimatedNetworkDelay = std::chrono::milliseconds(0);
+	std::chrono::microseconds estimatedNetworkDelay = std::chrono::microseconds(0);
 };
 } // namespace ISO22133
 

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<milliseconds>(networkDelay));
+	auto networkDelay = duration_cast<microseconds>(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;
 }

src/iso22133state.cpp

@@ -163,30 +163,38 @@ void ISO22133::State::handleSTRT(TestObject& obj, StartMessageType& strt) {
 
 	struct timeval diff;
 	timersub(&strt.startTime, &currentTime, &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
 			obj.strtSig(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;