-
Notifications
You must be signed in to change notification settings - Fork 0
/
sensors2quat.pd
188 lines (188 loc) · 4.52 KB
/
sensors2quat.pd
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
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
#N canvas 73 178 799 789 10;
#X msg 56 603 quat w \$1;
#X msg 113 624 quat x \$1;
#X msg 227 666 quat z \$1;
#X msg 170 645 quat y \$1;
#X text 80 165 gyro;
#X text 382 165 accel;
#X msg 94 266 1;
#X obj 153 463 +;
#X obj 283 463 +;
#X obj 59 519 quaternion/normalize;
#X obj 75 231 t b b f;
#X text 499 165 reset;
#N canvas 92 117 450 300 stop_drift 0;
#X obj 40 41 inlet;
#X obj 40 232 outlet;
#X msg 67 142 0;
#X obj 40 100 moses -0.05;
#X obj 67 121 moses 0.05;
#X connect 0 0 3 0;
#X connect 2 0 1 0;
#X connect 3 0 1 0;
#X connect 3 1 4 0;
#X connect 4 0 2 0;
#X connect 4 1 1 0;
#X restore 122 263 pd stop_drift;
#N canvas 92 117 450 300 stop_drift 0;
#X obj 40 41 inlet;
#X obj 40 232 outlet;
#X msg 67 142 0;
#X obj 40 100 moses -0.05;
#X obj 67 121 moses 0.05;
#X connect 0 0 3 0;
#X connect 2 0 1 0;
#X connect 3 0 1 0;
#X connect 3 1 4 0;
#X connect 4 0 2 0;
#X connect 4 1 1 0;
#X restore 168 238 pd stop_drift;
#N canvas 92 117 450 300 stop_drift 0;
#X obj 40 41 inlet;
#X obj 40 232 outlet;
#X msg 67 142 0;
#X obj 40 100 moses -0.05;
#X obj 67 121 moses 0.05;
#X connect 0 0 3 0;
#X connect 2 0 1 0;
#X connect 3 0 1 0;
#X connect 3 1 4 0;
#X connect 4 0 2 0;
#X connect 4 1 1 0;
#X restore 218 215 pd stop_drift;
#X obj 267 294 * 0.067;
#X obj 199 294 * -0.067;
#X obj 131 294 * -0.067;
#X obj 283 442 * 0.9982;
#X obj 215 442 * 0.9982;
#X obj 153 442 * 0.9982;
#X obj 97 441 * 0.9982;
#X obj 99 365 f 1;
#X msg 505 183 1 0 0 0;
#X obj 75 183 route x y z;
#X obj 375 183 route x y z;
#X obj 75 73 route gyro accel reset;
#N canvas 150 175 618 321 quaternion_mul 0;
#X obj 57 61 inlet;
#X obj 94 61 inlet;
#X obj 131 61 inlet;
#X obj 57 204 outlet;
#X obj 144 204 outlet;
#X obj 231 204 outlet;
#X obj 318 204 outlet;
#X text 340 115 w1*w2 - x1*x2 - y1*y2 - z1*z2 \; w1*x2 + x1*w2 + y1*z2 - z1*y2 \; w1*y2 - x1*z2 + y1*w2 + z1*x2 \; w1*z2 + x1*y2 - y1*x2 + z1*w2;
#X obj 57 115 expr $f1*$f5 - $f2*$f6 - $f3*$f7 - $f4*$f8 \; $f1*$f6 + $f2*$f5 + $f3*$f8 - $f4*$f7 \; $f1*$f7 - $f2*$f8 + $f3*$f5 + $f4*$f6 \; $f1*$f8 + $f2*$f7 - $f3*$f6 + $f4*$f5, f 44;
#X obj 168 61 inlet;
#X obj 206 61 inlet;
#X obj 243 61 inlet;
#X obj 280 61 inlet;
#X obj 318 61 inlet;
#X connect 0 0 8 0;
#X connect 1 0 8 1;
#X connect 2 0 8 2;
#X connect 8 0 3 0;
#X connect 8 1 4 0;
#X connect 8 2 5 0;
#X connect 8 3 6 0;
#X connect 9 0 8 3;
#X connect 10 0 8 4;
#X connect 11 0 8 5;
#X connect 12 0 8 6;
#X connect 13 0 8 7;
#X restore 100 405 pd quaternion_mul;
#N canvas 880 305 506 495 add_acceleration_quaternion 0;
#X obj 77 59 inlet;
#X obj 127 59 inlet;
#X obj 177 59 inlet;
#X obj 108 210 + 1;
#X obj 77 160 t b f;
#X obj 77 236 f;
#X obj 77 312 outlet;
#X obj 127 312 outlet;
#X obj 177 312 outlet;
#X obj 227 312 outlet;
#X text 176 331 dummy;
#X connect 0 0 4 0;
#X connect 1 0 3 0;
#X connect 2 0 7 0;
#X connect 3 0 5 1;
#X connect 4 0 5 0;
#X connect 4 1 9 0;
#X connect 5 0 6 0;
#X restore 330 289 pd add_acceleration_quaternion;
#X text 40 364 feedback;
#X obj 77 51 inlet;
#X obj 65 728 outlet;
#X obj 372 673 outlet;
#X obj 372 642 pack f f f f;
#X text 572 350 * 0.0018;
#X obj 422 469 > 0;
#X obj 330 399 * 1;
#X obj 422 490 * 2;
#X obj 422 511 - 1;
#X obj 97 462 +;
#X obj 506 350 * 0.0018;
#X obj 389 350 * 0.0018;
#X obj 330 350 * 0.0018;
#X text 449 489 w correction depending on closest identity (-1/1);
#X connect 0 0 31 0;
#X connect 1 0 31 0;
#X connect 2 0 31 0;
#X connect 3 0 31 0;
#X connect 6 0 27 4;
#X connect 7 0 9 1;
#X connect 8 0 9 3;
#X connect 9 0 22 1;
#X connect 9 0 0 0;
#X connect 9 0 33 0;
#X connect 9 0 35 0;
#X connect 9 1 1 0;
#X connect 9 1 27 1;
#X connect 9 1 33 1;
#X connect 9 2 3 0;
#X connect 9 2 27 2;
#X connect 9 2 33 2;
#X connect 9 3 2 0;
#X connect 9 3 27 3;
#X connect 9 3 33 3;
#X connect 10 0 22 0;
#X connect 10 1 6 0;
#X connect 10 2 12 0;
#X connect 12 0 17 0;
#X connect 13 0 16 0;
#X connect 14 0 15 0;
#X connect 15 0 27 7;
#X connect 16 0 27 6;
#X connect 17 0 27 5;
#X connect 18 0 8 0;
#X connect 19 0 9 2;
#X connect 20 0 7 0;
#X connect 21 0 39 0;
#X connect 22 0 27 0;
#X connect 23 0 27 0;
#X connect 24 0 10 0;
#X connect 24 1 13 0;
#X connect 24 2 14 0;
#X connect 25 0 28 0;
#X connect 25 1 28 1;
#X connect 25 2 28 2;
#X connect 26 0 24 0;
#X connect 26 1 25 0;
#X connect 26 2 23 0;
#X connect 27 0 21 0;
#X connect 27 1 20 0;
#X connect 27 2 19 0;
#X connect 27 3 18 0;
#X connect 28 0 42 0;
#X connect 28 1 41 0;
#X connect 28 3 40 0;
#X connect 30 0 26 0;
#X connect 33 0 32 0;
#X connect 35 0 37 0;
#X connect 36 0 39 1;
#X connect 37 0 38 0;
#X connect 38 0 36 1;
#X connect 39 0 9 0;
#X connect 40 0 8 1;
#X connect 41 0 7 1;
#X connect 42 0 36 0;