blob: b03562ede6212cfc422079f02f122f4674460e4f (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
|
#include "point-filter.hpp"
#include <algorithm>
#include <cmath>
#include <QDebug>
namespace pt_point_filter_impl {
void point_filter::reset()
{
t = std::nullopt;
}
const PointOrder& point_filter::operator()(const PointOrder& input)
{
using std::fmod;
using std::sqrt;
using std::pow;
using std::clamp;
if (!s.enable_point_filter)
{
t = std::nullopt;
state_ = input;
return state_;
}
if (!t)
{
t.emplace();
state_ = input;
return state_;
}
constexpr auto E = (f)1.75;
const f limit = (f)*s.point_filter_limit;
const f C = progn(
constexpr int A = 1'000'000;
double K = *s.point_filter_coefficient;
f log10_pos = -2 + (int)K, rest = (f)(.999-fmod(K, 1.)*.9);
return A * pow((f)10, (f)-log10_pos) * rest;
);
f dist = 0, dz = (float)s.point_filter_deadzone / 800; // sqrt(640^2 + 480^2)
for (unsigned i = 0; i < 3; i++)
{
vec2 tmp = input[i] - state_[i];
f x = sqrt(tmp.dot(tmp));
x = std::max((f)0, x - dz);
dist = std::max(dist, x);
}
if (dist < (f)1e-6)
return state_;
f dt = (f)t->elapsed_seconds(); t->start();
f delta = pow(dist, E) * C * dt; // gain
//qDebug() << "gain" << std::min((f)1, delta);
for (unsigned i = 0; i < 3; i++)
{
f x = clamp(delta, (f)0, limit);
state_[i] += x*(input[i] - state_[i]);
}
return state_;
}
point_filter::point_filter(const pt_settings& s) : s{s} {}
} // ns pt_point_filter_impl
|
