summaryrefslogtreecommitdiffhomepage
path: root/logic/pipeline.cpp
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2018-08-03 04:43:28 +0000
committerStanislaw Halik <sthalik@misaki.pl>2018-08-04 13:41:56 +0000
commitc172c327c2b927427c24a7e3a5f345673d0e5929 (patch)
treed1e2d0a1fcb3619bfa0b453b743db78b37e2b727 /logic/pipeline.cpp
parente841b3324655b5862dc6d79d482b2af474239791 (diff)
logic/pipeline: rotation order fixrevert-geometry-stuff
v2: fix comments v3: fix reltrans
Diffstat (limited to 'logic/pipeline.cpp')
-rw-r--r--logic/pipeline.cpp121
1 files changed, 75 insertions, 46 deletions
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);