1 // Copyright 2012 The ChromiumOS Authors
2 // Use of this source code is governed by a BSD-style license that can be
3 // found in the LICENSE file.
4
5 #include "include/iir_filter_interpreter.h"
6
7 #include <utility>
8 #include <vector>
9
10 namespace gestures {
11
Increment()12 void IirFilterInterpreter::IoHistory::Increment() {
13 out_head = NextOutHead();
14 in_head = NextInHead();
15 }
16
17 // Uses exact comparisions, rather than approximate float comparisions,
18 // for operator==
operator ==(const IirFilterInterpreter::IoHistory & that) const19 bool IirFilterInterpreter::IoHistory::operator==(
20 const IirFilterInterpreter::IoHistory& that) const {
21 for (size_t i = 0; i < kInSize; i++)
22 if (in[i] != that.in[i])
23 return false;
24 for (size_t i = 0; i < kOutSize; i++)
25 if (out[i] != that.out[i])
26 return false;
27 return true;
28 }
29
30 // The default filter is a low-pass 2nd order Butterworth IIR filter with a
31 // normalized cutoff frequency of 0.2.
IirFilterInterpreter(PropRegistry * prop_reg,Interpreter * next,Tracer * tracer)32 IirFilterInterpreter::IirFilterInterpreter(PropRegistry* prop_reg,
33 Interpreter* next,
34 Tracer* tracer)
35 : FilterInterpreter(nullptr, next, tracer, false),
36 using_iir_(true),
37 b0_(prop_reg, "IIR b0", 0.0674552738890719),
38 b1_(prop_reg, "IIR b1", 0.134910547778144),
39 b2_(prop_reg, "IIR b2", 0.0674552738890719),
40 b3_(prop_reg, "IIR b3", 0.0),
41 a1_(prop_reg, "IIR a1", -1.1429805025399),
42 a2_(prop_reg, "IIR a2", 0.412801598096189),
43 iir_dist_thresh_(prop_reg, "IIR Distance Threshold", 10),
44 adjust_iir_on_warp_(prop_reg, "Adjust IIR History On Warp", false) {
45 InitName();
46 b0_.SetDelegate(this);
47 b1_.SetDelegate(this);
48 b2_.SetDelegate(this);
49 b3_.SetDelegate(this);
50 a1_.SetDelegate(this);
51 a2_.SetDelegate(this);
52 iir_dist_thresh_.SetDelegate(this);
53 }
54
SyncInterpretImpl(HardwareState & hwstate,stime_t * timeout)55 void IirFilterInterpreter::SyncInterpretImpl(HardwareState& hwstate,
56 stime_t* timeout) {
57 const char name[] = "IirFilterInterpreter::SyncInterpretImpl";
58 LogHardwareStatePre(name, hwstate);
59
60 // Delete old entries from map
61 std::vector<short> dead_ids;
62 dead_ids.reserve(histories_.size());
63
64 for (std::map<short, IoHistory>::iterator it = histories_.begin(),
65 e = histories_.end(); it != e; ++it)
66 if (!hwstate.GetFingerState((*it).first))
67 dead_ids.push_back((*it).first);
68 for (auto dead_id : dead_ids)
69 histories_.erase(dead_id);
70
71 // Modify current hwstate
72 for (size_t i = 0; i < hwstate.finger_cnt; i++) {
73 FingerState& fs = hwstate.fingers[i];
74 std::map<short, IoHistory>::iterator history =
75 histories_.find(fs.tracking_id);
76 if (history == histories_.end()) {
77 // new finger
78 IoHistory hist(fs);
79 histories_[fs.tracking_id] = hist;
80 continue;
81 }
82 // existing finger, apply filter
83 IoHistory* hist = &(*history).second;
84
85 // Finger WARP detected, adjust the IO history
86 if (adjust_iir_on_warp_.val_) {
87 float dx = 0.0, dy = 0.0;
88
89 if (fs.flags & GESTURES_FINGER_WARP_X_MOVE)
90 dx = fs.position_x - hist->PrevIn(0)->position_x;
91 if (fs.flags & GESTURES_FINGER_WARP_Y_MOVE)
92 dy = fs.position_y - hist->PrevIn(0)->position_y;
93
94 hist->WarpBy(dx, dy);
95 }
96
97 float dx = fs.position_x - hist->PrevOut(0)->position_x;
98 float dy = fs.position_y - hist->PrevOut(0)->position_y;
99
100 // IIR filter is too smooth for a quick finger movement. We do a simple
101 // rolling average if the position change between current and previous
102 // frames is larger than iir_dist_thresh_.
103 if (dx * dx + dy * dy > iir_dist_thresh_.val_ * iir_dist_thresh_.val_)
104 using_iir_ = false;
105 else
106 using_iir_ = true;
107
108 // TODO(adlr): consider applying filter to other fields
109 float FingerState::*fields[] = { &FingerState::position_x,
110 &FingerState::position_y,
111 &FingerState::pressure };
112 for (size_t f_idx = 0; f_idx < arraysize(fields); f_idx++) {
113 float FingerState::*field = fields[f_idx];
114 // Keep the current pressure reading, so we could make sure the pressure
115 // values will be same if there is two fingers on a SemiMT device.
116 if (hwprops_ && hwprops_->support_semi_mt &&
117 (field == &FingerState::pressure)) {
118 hist->NextOut()->pressure = fs.pressure;
119 continue;
120 }
121
122 if (adjust_iir_on_warp_.val_) {
123 if (field == &FingerState::position_x &&
124 (fs.flags & GESTURES_FINGER_WARP_X_MOVE)) {
125 hist->NextOut()->position_x = fs.position_x;
126 continue;
127 }
128
129 if (field == &FingerState::position_y &&
130 (fs.flags & GESTURES_FINGER_WARP_Y_MOVE)) {
131 hist->NextOut()->position_y = fs.position_y;
132 continue;
133 }
134 }
135
136 if (using_iir_) {
137 hist->NextOut()->*field =
138 b3_.val_ * hist->PrevIn(2)->*field +
139 b2_.val_ * hist->PrevIn(1)->*field +
140 b1_.val_ * hist->PrevIn(0)->*field +
141 b0_.val_ * fs.*field -
142 a2_.val_ * hist->PrevOut(1)->*field -
143 a1_.val_ * hist->PrevOut(0)->*field;
144 } else {
145 hist->NextOut()->*field = 0.5 * (fs.*field + hist->PrevOut(0)->*field);
146 }
147 }
148 float FingerState::*pass_fields[] = { &FingerState::touch_major,
149 &FingerState::touch_minor,
150 &FingerState::width_major,
151 &FingerState::width_minor,
152 &FingerState::orientation };
153 for (size_t f_idx = 0; f_idx < arraysize(pass_fields); f_idx++)
154 hist->NextOut()->*pass_fields[f_idx] = fs.*pass_fields[f_idx];
155 hist->NextOut()->flags = fs.flags;
156 *hist->NextIn() = fs;
157 fs = *hist->NextOut();
158 hist->Increment();
159 }
160 LogHardwareStatePost(name, hwstate);
161 next_->SyncInterpret(hwstate, timeout);
162 }
163
DoubleWasWritten(DoubleProperty * prop)164 void IirFilterInterpreter::DoubleWasWritten(DoubleProperty* prop) {
165 histories_.clear();
166 }
167
WarpBy(float dx,float dy)168 void IirFilterInterpreter::IoHistory::WarpBy(float dx, float dy) {
169 for (size_t i = 0; i < kInSize; i++) {
170 PrevIn(i)->position_x += dx;
171 PrevIn(i)->position_y += dy;
172 }
173
174 for (size_t i = 0; i < kOutSize; i++) {
175 PrevOut(i)->position_x += dx;
176 PrevOut(i)->position_y += dy;
177 }
178 }
179
180 } // namespace gestures
181