summaryrefslogtreecommitdiffhomepage
path: root/opentrack-logic
diff options
context:
space:
mode:
Diffstat (limited to 'opentrack-logic')
-rw-r--r--opentrack-logic/tracker.cpp52
-rw-r--r--opentrack-logic/tracker.h15
2 files changed, 40 insertions, 27 deletions
diff --git a/opentrack-logic/tracker.cpp b/opentrack-logic/tracker.cpp
index ab1a7406..d060fff1 100644
--- a/opentrack-logic/tracker.cpp
+++ b/opentrack-logic/tracker.cpp
@@ -12,10 +12,12 @@
* originally written by Wim Vriend.
*/
+#include "opentrack-compat/sleep.hpp"
#include "tracker.h"
#include <cmath>
#include <algorithm>
+#include <cstdio>
#if defined(_WIN32)
# include <windows.h>
@@ -24,15 +26,15 @@
Tracker::Tracker(Mappings &m, SelectedLibraries &libs, TrackLogger &logger) :
m(m),
newpose {0,0,0, 0,0,0},
- centerp(s.center_at_startup),
- enabledp(true),
- zero_(false),
- should_quit(false),
libs(libs),
logger(logger),
r_b(get_camera_offset_matrix(c_div).t()),
r_b_real(get_camera_offset_matrix(1).t()),
- t_b {0,0,0}
+ t_b {0,0,0},
+ centerp(s.center_at_startup),
+ enabledp(true),
+ zero_(false),
+ should_quit(false)
{
}
@@ -117,6 +119,9 @@ void Tracker::logic()
{
using namespace euler;
+ logger.write_dt();
+ logger.reset_dt();
+
Pose value, raw;
for (int i = 0; i < 6; i++)
@@ -284,8 +289,6 @@ void Tracker::logic()
(void) map(value(i), m(i));
}
- logger.next_line();
-
libs.pProtocol->pose(value);
last_mapped = value;
@@ -294,16 +297,19 @@ void Tracker::logic()
QMutexLocker foo(&mtx);
output_pose = value;
raw_6dof = raw;
+
+ logger.reset_dt();
+ logger.next_line();
}
void Tracker::run()
{
- const int sleep_ms = 3;
-
#if defined(_WIN32)
(void) timeBeginPeriod(1);
#endif
+ setPriority(QThread::HighPriority);
+
{
static constexpr const char* posechannels[6] = { "TX", "TY", "TZ", "Yaw", "Pitch", "Roll" };
static constexpr const char* datachannels[5] = { "dt", "raw", "corrected", "filtered", "mapped" };
@@ -313,21 +319,18 @@ void Tracker::run()
{
for (unsigned i = 0; i < 6; ++i)
{
- snprintf(buffer, 128, "%s%s", datachannels[j], posechannels[i]);
+ std::sprintf(buffer, "%s%s", datachannels[j], posechannels[i]);
logger.write(buffer);
}
}
+ logger.next_line();
}
- logger.next_line();
+
+ t.start();
+ logger.reset_dt();
while (!should_quit)
{
- {
- double dt = t.elapsed_seconds();
- logger.write(&dt, 1);
- }
- t.start();
-
double tmp[6] {0,0,0, 0,0,0};
libs.pTracker->data(tmp);
@@ -337,10 +340,19 @@ void Tracker::run()
logic();
- long q = long(sleep_ms * 1000L - t.elapsed()/1000L);
+ static constexpr long const_sleep_us = 4000;
+
using std::max;
- using ulong = unsigned long;
- usleep(ulong(max(1L, q)));
+ using std::min;
+
+ const long elapsed_usecs = t.elapsed_usecs();
+ const long sleep_us = const_sleep_us * 2 - elapsed_usecs;
+
+ const unsigned sleep_time = unsigned(max(1l, min(const_sleep_us * 4, max(1l, (sleep_us + 200l)/1000l))));
+
+ t.start();
+
+ portable::sleep(unsigned(max(1u, sleep_time)));
}
{
diff --git a/opentrack-logic/tracker.h b/opentrack-logic/tracker.h
index 2153f7c0..6dc48074 100644
--- a/opentrack-logic/tracker.h
+++ b/opentrack-logic/tracker.h
@@ -33,6 +33,9 @@ class OPENTRACK_LOGIC_EXPORT Tracker : private QThread
{
Q_OBJECT
private:
+ using rmat = euler::rmat;
+ using euler_t = euler::euler_t;
+
QMutex mtx;
main_settings s;
Mappings& m;
@@ -41,22 +44,20 @@ private:
Pose output_pose, raw_6dof, last_mapped, last_raw;
double newpose[6];
- volatile bool centerp;
- volatile bool enabledp;
- volatile bool zero_;
- volatile bool should_quit;
SelectedLibraries const& libs;
// The owner of the reference is the main window.
// This design might be usefull if we decide later on to swap out
// the logger while the tracker is running.
TrackLogger &logger;
- using rmat = euler::rmat;
- using euler_t = euler::euler_t;
-
rmat r_b, r_b_real;
double t_b[3];
+ volatile bool centerp;
+ volatile bool enabledp;
+ volatile bool zero_;
+ volatile bool should_quit;
+
double map(double pos, Map& axis);
void logic();
void t_compensate(const rmat& rmat, const euler_t& ypr, euler_t& output, bool rz);