From c172c327c2b927427c24a7e3a5f345673d0e5929 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Fri, 3 Aug 2018 04:43:28 +0000
Subject: logic/pipeline: rotation order fix
v2: fix comments
v3: fix reltrans
---
logic/pipeline.cpp | 121 +++++++++++++++++++++++++++++++++--------------------
logic/pipeline.hpp | 34 ++++++++-------
2 files changed, 93 insertions(+), 62 deletions(-)
(limited to 'logic')
diff --git a/logic/pipeline.cpp b/logic/pipeline.cpp
index a7817bc8..81a6989b 100644
--- a/logic/pipeline.cpp
+++ b/logic/pipeline.cpp
@@ -80,7 +80,7 @@ Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value,
if (state != reltrans_disabled)
{
{
- bool tcomp_in_zone_ = progn(
+ bool in_zone_ = progn(
if (state == reltrans_non_center)
{
const bool looking_down = value(Pitch) < 20;
@@ -90,7 +90,7 @@ Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value,
return true;
);
- if (!cur && in_zone != tcomp_in_zone_)
+ if (!cur && in_zone != in_zone_)
{
//qDebug() << "reltrans-interp: START" << tcomp_in_zone_;
cur = true;
@@ -99,15 +99,16 @@ Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value,
RC_phase = 0;
}
- in_zone = tcomp_in_zone_;
+ in_zone = in_zone_;
}
// only when looking behind or downward
if (in_zone)
{
- const rmat R = euler_to_rmat(euler_t(value(Yaw) * d2r * !disable(Yaw),
- value(Pitch) * d2r * !disable(Pitch),
- value(Roll) * d2r * !disable(Roll)));
+ const rmat R = euler_to_rmat(
+ euler_t(value(Yaw) * d2r * !disable(Yaw),
+ value(Pitch) * d2r * !disable(Pitch),
+ value(Roll) * d2r * !disable(Roll)));
rel = rotate(R, rel, &disable[TX]);
@@ -213,8 +214,8 @@ pipeline::~pipeline()
double pipeline::map(double pos, Map& axis)
{
bool altp = (pos < 0) && axis.opts.altp;
- axis.spline_main.set_tracking_active( !altp );
- axis.spline_alt.set_tracking_active( altp );
+ axis.spline_main.set_tracking_active(!altp);
+ axis.spline_alt.set_tracking_active(altp);
auto& fc = altp ? axis.spline_alt : axis.spline_main;
return double(fc.get_value(pos));
}
@@ -234,15 +235,17 @@ static bool is_nan(const dmat<u,w>& r)
}
template<typename x>
-static cc_forceinline bool nan_check_(const x& datum)
+static cc_forceinline
+bool nan_check_(const x& datum)
{
return is_nan(datum);
}
template<typename x, typename y, typename... xs>
-static cc_forceinline bool nan_check_(const x& datum, const y& next, const xs&... rest)
+static cc_forceinline
+bool nan_check_(const x& datum, const y& next, const xs&... rest)
{
- return is_nan(datum) ? true : nan_check_(next, rest...);
+ return nan_check_(datum) || nan_check_(next, rest...);
}
static cc_noinline
@@ -302,9 +305,6 @@ bool pipeline::maybe_enable_center_on_tracking_started()
void pipeline::maybe_set_center_pose(const Pose& value, bool own_center_logic)
{
- euler_t tmp = d2r * euler_t(&value[Yaw]);
- state.rotation = euler_to_rmat(tmp);
-
if (get(f_center | f_held_center))
{
if (libs.pFilter)
@@ -312,33 +312,50 @@ void pipeline::maybe_set_center_pose(const Pose& value, bool own_center_logic)
if (own_center_logic)
{
+#if 0
state.inv_rot_center = rmat::eye();
- state.t_center = euler_t();
+ state.t_center = {};
+#endif
+
+ scaled_state.inv_rot_center = rmat::eye();
+ scaled_state.t_center = {};
}
else
{
- state.inv_rot_center = state.rotation.t();
- state.t_center = euler_t((const double*)(value));
+#if 0
+ state.inv_rot_center = state.inv_rot_center;
+ state.t_center = (const double*)(value);
+#endif
+
+ scaled_state.inv_rot_center = scaled_state.rotation.t();
+ scaled_state.t_center = (const double*)(value);
}
}
}
+void pipeline::store_tracker_pose(const Pose& value)
+{
+#if 0
+ euler_t tmp(d2r * euler_t(&value[Yaw]));
+ state.rotation = euler_to_rmat(tmp);
+#endif
+ // alas, this is poor man's gimbal lock "prevention"
+ // this is some kind of "canonical" representation,
+ // if we can even talk of one
+ scaled_state.rotation = euler_to_rmat(scale_inv_c * d2r * euler_t(&value[Yaw]));
+}
+
Pose pipeline::clamp_value(Pose value) const
{
// hatire, udp, and freepie trackers can mess up here
for (unsigned i = 3; i < 6; i++)
{
- using std::fmod;
- using std::copysign;
- using std::fabs;
-
- value(i) = fmod(value(i), 360);
+ value(i) = std::fmod(value(i), 360);
const double x = value(i);
- if (fabs(x) - 1e-2 > 180)
- value(i) = fmod(x + copysign(180, x), 360) - copysign(180, x);
- else
- value(i) = clamp(x, -180, 180);
+ if (std::fabs(x) - 1e-2 > 180)
+ value(i) = std::fmod(x + std::copysign(180, x), 360) - std::copysign(180, x);
+ value(i) = clamp(x, -180, 180);
}
return value;
@@ -346,10 +363,12 @@ Pose pipeline::clamp_value(Pose value) const
Pose pipeline::apply_center(Pose value) const
{
- euler_t T = euler_t(value) - state.t_center;
- euler_t R = r2d * rmat_to_euler(state.rotation * state.inv_rot_center);
+ euler_t T = euler_t(value) - scaled_state.t_center;
+ euler_t R = scale_c * r2d * rmat_to_euler(scaled_state.rotation * scaled_state.inv_rot_center);
- T = rel.rotate(state.inv_rot_center, T, vec3_bool());
+ // XXX check these lines, it's been here forever
+ T = rel.rotate(scaled_state.inv_rot_center, T, {});
+ T = T * scale_c;
for (int i = 0; i < 3; i++)
{
@@ -372,7 +391,7 @@ Pose pipeline::apply_center(Pose value) const
}
std::tuple<Pose, Pose, vec6_bool>
-pipeline::get_selected_axis_value(const Pose& newpose) const
+pipeline::get_selected_axis_values(const Pose& newpose) const
{
Pose value;
vec6_bool disabled;
@@ -456,7 +475,7 @@ void pipeline::logic()
newpose = tmp;
}
- auto [raw, value, disabled] = get_selected_axis_value(newpose);
+ auto [raw, value, disabled] = get_selected_axis_values(newpose);
logger.write_pose(raw); // raw
nan_check(newpose, raw, value);
@@ -465,6 +484,7 @@ void pipeline::logic()
{
maybe_enable_center_on_tracking_started();
+ store_tracker_pose(value);
maybe_set_center_pose(value, own_center_logic);
value = apply_center(value);
// "corrected" - after various transformations to account for camera position
@@ -473,6 +493,8 @@ void pipeline::logic()
{
ev.run_events(EV::ev_before_filter, value);
+ // we must proceed with all the filtering since the filter
+ // needs fresh values to prevent deconvergence
Pose tmp = maybe_apply_filter(value);
nan_check(tmp);
if (!center_ordered)
@@ -482,7 +504,7 @@ void pipeline::logic()
{
ev.run_events(EV::ev_before_mapping, value);
- // CAVEAT rotation only, due to tcomp
+ // CAVEAT rotation only, due to reltrans
for (int i = 3; i < 6; i++)
value(i) = map(value(i), m(i));
}
@@ -496,10 +518,6 @@ void pipeline::logic()
nan_check(value);
}
- // we must proceed with all the filtering since the filter
- // needs fresh values to prevent deconvergence
- // same for other stuff that needs dt, it could go stupid
-
if (!hold_ordered)
goto ok;
@@ -549,7 +567,7 @@ void pipeline::run()
static const char* const datachannels[5] = { "dt", "raw", "corrected", "filtered", "mapped" };
logger.write(datachannels[0]);
- char buffer[128];
+ char buffer[16];
for (unsigned j = 1; j < 5; ++j)
{
for (unsigned i = 0; i < 6; ++i)
@@ -569,26 +587,37 @@ void pipeline::run()
{
logic();
- constexpr ns const_sleep_ms(time_cast<ns>(ms(4)));
+ constexpr ns const_sleep_ms(ms{4});
const ns elapsed_nsecs = t.elapsed<ns>();
t.start();
if (backlog_time > secs(3) || backlog_time < secs(-3))
{
qDebug() << "tracker: backlog interval overflow"
- << time_cast<ms>(backlog_time).count() << "ms";
- backlog_time = ns::zero();
+ << ms{backlog_time}.count() << "ms";
+ backlog_time = ns{};
}
- backlog_time += ns(elapsed_nsecs - const_sleep_ms);
+ backlog_time += ns{elapsed_nsecs - const_sleep_ms};
- const int sleep_time_ms = time_cast<ms>(clamp(const_sleep_ms - backlog_time,
- ms::zero(), ms(10))).count();
+ const int sleep_time_ms = ms{clamp(const_sleep_ms - backlog_time,
+ ms{}, ms{10})}.count() + .1f;
#if 0
- qDebug() << "sleepy time" << sleep_time_ms
- << "elapsed" << time_cast<ms>(elapsed_nsecs).count()
- << "backlog" << time_cast<ms>(backlog_time).count();
+ {
+ static int cnt;
+ static Timer tt;
+ if (tt.elapsed_seconds() >= 1)
+ {
+ tt.start();
+ qDebug() << cnt << "Hz"
+ << "sleepy time" << sleep_time_ms
+ << "elapsed" << ms{elapsed_nsecs}.count()
+ << "backlog" << ms{backlog_time}.count();
+ cnt = 0;
+ }
+ cnt++;
+ }
#endif
portable::sleep(sleep_time_ms);
diff --git a/logic/pipeline.hpp b/logic/pipeline.hpp
index 0b48d741..b293663f 100644
--- a/logic/pipeline.hpp
+++ b/logic/pipeline.hpp
@@ -31,6 +31,16 @@ using euler_t = euler::euler_t;
using vec6_bool = Mat<bool, 6, 1>;
using vec3_bool = Mat<bool, 6, 1>;
+static constexpr inline double scale_c = 8;
+static constexpr inline double scale_inv_c = 1/scale_c;
+
+struct state_
+{
+ rmat inv_rot_center { rmat::eye() };
+ rmat rotation { rmat::eye() };
+ euler_t t_center;
+};
+
class reltrans
{
euler_t interp_pos;
@@ -85,7 +95,7 @@ DEFINE_ENUM_OPERATORS(bits::flags);
class OTR_LOGIC_EXPORT pipeline : private QThread, private bits
{
Q_OBJECT
-private:
+
QMutex mtx;
main_settings s;
Mappings& m;
@@ -101,21 +111,12 @@ private:
// the logger while the tracker is running.
TrackLogger& logger;
- struct state_
- {
- rmat inv_rot_center;
- rmat rotation;
- euler_t t_center;
-
- state_() : inv_rot_center(rmat::eye())
- {}
- };
-
reltrans rel;
- state_ state;
+ //state_ state, scaled_state;
+ state_ scaled_state;
- ns backlog_time = ns(0);
+ ns backlog_time { ns{} };
bool tracking_started = false;
@@ -124,16 +125,17 @@ private:
void run() override;
bool maybe_enable_center_on_tracking_started();
void maybe_set_center_pose(const Pose& value, bool own_center_logic);
+ void store_tracker_pose(const Pose& value);
Pose clamp_value(Pose value) const;
Pose apply_center(Pose value) const;
- std::tuple<Pose, Pose, vec6_bool> get_selected_axis_value(const Pose& newpose) const;
+ std::tuple<Pose, Pose, vec6_bool> get_selected_axis_values(const Pose& newpose) const;
Pose maybe_apply_filter(const Pose& value) const;
Pose apply_reltrans(Pose value, vec6_bool disabled, bool centerp);
Pose apply_zero_pos(Pose value) const;
public:
pipeline(Mappings& m, runtime_libraries& libs, event_handler& ev, TrackLogger& logger);
- ~pipeline();
+ ~pipeline() override;
void raw_and_mapped_pose(double* mapped, double* raw) const;
void start() { QThread::start(QThread::HighPriority); }
@@ -149,4 +151,4 @@ public:
} // ns pipeline_impl
-using pipeline_impl::pipeline;
+using pipeline = pipeline_impl::pipeline;
--
cgit v1.2.3