From 594e2c09c30e734d67fed6b56d3f197a6863e4d5 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Thu, 20 Aug 2020 19:10:48 +0300
Subject: [PATCH] Add timer periodics

---
 components/common/cpp/include/ftl/timer.hpp   | 13 ++++
 components/common/cpp/src/timer.cpp           | 72 ++++++++++++++-----
 components/common/cpp/test/timer_unit.cpp     | 52 ++++++++++++++
 .../src/sources/stereovideo/pylon.cpp         | 25 +++----
 .../src/sources/stereovideo/pylon.hpp         |  2 +-
 components/streams/test/sender_unit.cpp       |  4 +-
 6 files changed, 135 insertions(+), 33 deletions(-)

diff --git a/components/common/cpp/include/ftl/timer.hpp b/components/common/cpp/include/ftl/timer.hpp
index 9ccb0b31f..6530aeadd 100644
--- a/components/common/cpp/include/ftl/timer.hpp
+++ b/components/common/cpp/include/ftl/timer.hpp
@@ -84,6 +84,19 @@ void setClockSlave(bool);
  */
 ftl::Handle add(timerlevel_t, const std::function<bool(int64_t ts)> &);
 
+/**
+ * Same as other add function except that a multiplier is given to indicate
+ * how often this should be triggered in numbers of ticks.
+ */
+ftl::Handle add(timerlevel_t, size_t multiplier, const std::function<bool(int64_t ts)> &);
+
+/**
+ * Same as other add function except that a period in seconds is given. Note that
+ * the period should be a multiple of frames otherwise it will not be accurate
+ * but will still work.
+ */
+ftl::Handle add(timerlevel_t, double seconds, const std::function<bool(int64_t ts)> &);
+
 /**
  * Initiate the timer and optionally block the current process.
  */
diff --git a/components/common/cpp/src/timer.cpp b/components/common/cpp/src/timer.cpp
index ec9473178..e49a955cf 100644
--- a/components/common/cpp/src/timer.cpp
+++ b/components/common/cpp/src/timer.cpp
@@ -35,8 +35,8 @@ struct TimerJob {
 	std::atomic_bool active=false;
 	// TODO: (Nick) Implement richer forms of timer
 	//bool paused;
-	//int multiplier;
-	//int countdown;
+	int multiplier=0;		// Number of ticks before trigger
+	int counter=0;		// Current tick counter
 	std::string name;
 };
 
@@ -76,14 +76,21 @@ static void waitTimePoint() {
 		UNIQUE_LOCK(mtx, lk);
 		auto idle_job = jobs[kTimerIdle10].begin();
 		while (idle_job != jobs[kTimerIdle10].end() && msdelay >= 10 && sincelast != mspf) {
-			(*idle_job).active = true;
-			bool doremove = !(*idle_job).job.trigger(now);
-
-			if (doremove) {
-				idle_job = jobs[kTimerIdle10].erase(idle_job);
-				LOG(INFO) << "Timer job removed";
+			auto &job = *idle_job;
+
+			if (++job.counter >= job.multiplier) {
+				job.counter = 0;
+				job.active = true;
+				bool doremove = !job.job.trigger(now);
+
+				if (doremove) {
+					idle_job = jobs[kTimerIdle10].erase(idle_job);
+					LOG(INFO) << "Timer job removed";
+				} else {
+					(*idle_job++).active = false;
+				}
 			} else {
-				(*idle_job++).active = false;
+				++idle_job;
 			}
 			now = get_time();
 			msdelay = mspf - (now % mspf);
@@ -100,14 +107,21 @@ static void waitTimePoint() {
 		UNIQUE_LOCK(mtx, lk);
 		auto idle_job = jobs[kTimerIdle1].begin();
 		while (idle_job != jobs[kTimerIdle1].end() && msdelay >= 2 && sincelast != mspf) {
-			(*idle_job).active = true;
-			bool doremove = !(*idle_job).job.trigger(now);
-
-			if (doremove) {
-				idle_job = jobs[kTimerIdle1].erase(idle_job);
-				LOG(INFO) << "Timer job removed";
+			auto &job = *idle_job;
+
+			if (++job.counter >= job.multiplier) {
+				job.counter = 0;
+				job.active = true;
+				bool doremove = !job.job.trigger(now);
+
+				if (doremove) {
+					idle_job = jobs[kTimerIdle1].erase(idle_job);
+					LOG(INFO) << "Timer job removed";
+				} else {
+					(*idle_job++).active = false;
+				}
 			} else {
-				(*idle_job++).active = false;
+				++idle_job;
 			}
 			now = get_time();
 			msdelay = mspf - (now % mspf);
@@ -171,6 +185,32 @@ ftl::Handle ftl::timer::add(timerlevel_t l, const std::function<bool(int64_t ts)
 	return h;
 }
 
+ftl::Handle ftl::timer::add(timerlevel_t l, size_t multiplier, const std::function<bool(int64_t ts)> &f) {
+	if (l < 0 || l >= kTimerMAXLEVEL) return {};
+
+	UNIQUE_LOCK(mtx, lk);
+	int newid = last_id++;
+	auto &j = jobs[l].emplace_back();
+	j.id = newid;
+	j.name = "NoName";
+	j.multiplier = multiplier;
+	ftl::Handle h = j.job.on(f);
+	return h;
+}
+
+ftl::Handle ftl::timer::add(timerlevel_t l, double seconds, const std::function<bool(int64_t ts)> &f) {
+	if (l < 0 || l >= kTimerMAXLEVEL) return {};
+
+	UNIQUE_LOCK(mtx, lk);
+	int newid = last_id++;
+	auto &j = jobs[l].emplace_back();
+	j.id = newid;
+	j.name = "NoName";
+	j.multiplier = int(seconds*1000.0 / double(getInterval()));
+	ftl::Handle h = j.job.on(f);
+	return h;
+}
+
 static void removeJob(int id) {
 	UNIQUE_LOCK(mtx, lk);
 	if (id < 0) return;
diff --git a/components/common/cpp/test/timer_unit.cpp b/components/common/cpp/test/timer_unit.cpp
index 2c7602646..1f8f63b69 100644
--- a/components/common/cpp/test/timer_unit.cpp
+++ b/components/common/cpp/test/timer_unit.cpp
@@ -133,6 +133,58 @@ TEST_CASE( "Timer::add() Idle10 job" ) {
 	}
 }
 
+TEST_CASE( "Timer::add() Idle10 job periodic" ) {
+	SECTION( "Quick idle job" ) {
+		bool didrun = false;
+
+		ftl::timer::reset();
+
+		int count = 0;
+		auto rcc = ftl::timer::add(ftl::timer::kTimerIdle10, [&count](int64_t ts) {
+			++count;
+			return true;
+		});
+
+		auto rc = ftl::timer::add(ftl::timer::kTimerIdle10, size_t(20), [&didrun](int64_t ts) {
+			didrun = true;
+			ftl::timer::stop(false);
+			return true;
+		});
+
+		REQUIRE( (rc.id() >= 0) );
+
+		ftl::timer::start(true);
+		REQUIRE( didrun == true );
+		REQUIRE( count == 20 );
+	}
+}
+
+TEST_CASE( "Timer::add() Idle1 job periodic" ) {
+	SECTION( "Quick idle job" ) {
+		bool didrun = false;
+
+		ftl::timer::reset();
+
+		int count = 0;
+		auto rcc = ftl::timer::add(ftl::timer::kTimerIdle1, [&count](int64_t ts) {
+			++count;
+			return true;
+		});
+
+		auto rc = ftl::timer::add(ftl::timer::kTimerIdle1, size_t(20), [&didrun](int64_t ts) {
+			didrun = true;
+			ftl::timer::stop(false);
+			return true;
+		});
+
+		REQUIRE( (rc.id() >= 0) );
+
+		ftl::timer::start(true);
+		REQUIRE( didrun == true );
+		REQUIRE( count == 20 );
+	}
+}
+
 TEST_CASE( "Timer::add() Main job" ) {
 	SECTION( "Quick main job" ) {
 		bool didrun = false;
diff --git a/components/rgbd-sources/src/sources/stereovideo/pylon.cpp b/components/rgbd-sources/src/sources/stereovideo/pylon.cpp
index fcf2a6c3d..89bc33812 100644
--- a/components/rgbd-sources/src/sources/stereovideo/pylon.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/pylon.cpp
@@ -124,26 +124,23 @@ PylonDevice::PylonDevice(nlohmann::json &config)
 	});
 
 	monitor_ = true;
-	temperature_monitor_ = ftl::pool.push([this](int){
-		while (ftl::running && monitor_) {
-			for (auto* cam : {lcam_, rcam_}) {
-				// is this thread safe?
-				float temperature = cam->DeviceTemperature();
-				LOG_IF(WARNING, temperature > 53.0)
-					<< "Camera temperature over 50C (value: " << temperature << ")";
-
-				// TODO: stop if camera temperature exceeds threshold
-			}
-			std::this_thread::sleep_for(std::chrono::seconds(3));
+	temperature_monitor_ = ftl::timer::add(ftl::timer::timerlevel_t::kTimerIdle1, 3.0, [this](int64_t ts) {
+		for (auto* cam : {lcam_, rcam_}) {
+			// is this thread safe?
+			float temperature = cam->DeviceTemperature();
+			LOG_IF(WARNING, temperature > 53.0)
+				<< "Camera temperature over 50C (value: " << temperature << ")";
+
+			// TODO: stop if camera temperature exceeds threshold
 		}
+
+		return true;
 	});
 }
 
 PylonDevice::~PylonDevice() {
 	monitor_ = false;
-	if (temperature_monitor_.valid()) {
-		temperature_monitor_.wait();
-	}
+	temperature_monitor_.cancel();
 	lcam_->Close();
 	rcam_->Close();
 }
diff --git a/components/rgbd-sources/src/sources/stereovideo/pylon.hpp b/components/rgbd-sources/src/sources/stereovideo/pylon.hpp
index 02bcb05fe..096d504ac 100644
--- a/components/rgbd-sources/src/sources/stereovideo/pylon.hpp
+++ b/components/rgbd-sources/src/sources/stereovideo/pylon.hpp
@@ -55,7 +55,7 @@ class PylonDevice : public ftl::rgbd::detail::Device {
 	int interpolation_;
 
 	std::atomic_bool monitor_;
-	std::future<void> temperature_monitor_;
+	ftl::Handle temperature_monitor_;
 
 	void _configureCamera(Pylon::CBaslerUniversalInstantCamera *cam);
 	bool _retrieveFrames(Pylon::CGrabResultPtr &result, Pylon::CBaslerUniversalInstantCamera *cam);
diff --git a/components/streams/test/sender_unit.cpp b/components/streams/test/sender_unit.cpp
index 753fde010..11c44ae33 100644
--- a/components/streams/test/sender_unit.cpp
+++ b/components/streams/test/sender_unit.cpp
@@ -210,13 +210,13 @@ TEST_CASE( "Sender::post() video frames" ) {
 		REQUIRE( count == 1 );
 		REQUIRE( spkt.version == 5 );
 		REQUIRE( spkt.timestamp == 1000 );
-		REQUIRE( (int)spkt.frame_number == 1 );
+		REQUIRE( (int)spkt.frame_number == 0 );
 		REQUIRE( spkt.streamID == 0 );
 		REQUIRE( spkt.channel == Channel::Depth );
 		REQUIRE( pkt.codec == codec_t::HEVC );
 		REQUIRE( pkt.data.size() > 0 );
 		REQUIRE( pkt.flags == (ftl::codecs::kFlagFloat | ftl::codecs::kFlagMappedDepth) );
-		REQUIRE( pkt.frame_count == 3 );
+		REQUIRE( pkt.frame_count == 4 );
 		REQUIRE( ftl::codecs::hevc::validNAL(pkt.data.data(), pkt.data.size()) );
 	}
 
-- 
GitLab