-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathsensors2quat.pd
179 lines (179 loc) · 4.32 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
#N canvas 76 178 799 789 10;
#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 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 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 73 674 outlet;
#X obj 71 592 pack f f f f;
#X text 572 350 * 0.0018;
#X obj 376 495 > 0;
#X obj 330 399 * 1;
#X obj 376 516 * 2;
#X obj 376 537 - 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 403 515 w correction depending on closest identity (-1/1);
#X obj 75 183 unpack f f f;
#X obj 375 183 unpack f f f;
#X obj 72 623 list prepend quat;
#X obj 72 644 list trim;
#X obj 131 294 * -0.05;
#X obj 199 294 * -0.05;
#X obj 267 294 * 0.05;
#X connect 2 0 18 4;
#X connect 3 0 5 1;
#X connect 4 0 5 3;
#X connect 5 0 15 1;
#X connect 5 0 23 0;
#X connect 5 0 25 0;
#X connect 5 1 18 1;
#X connect 5 1 23 1;
#X connect 5 2 18 2;
#X connect 5 2 23 2;
#X connect 5 3 18 3;
#X connect 5 3 23 3;
#X connect 6 0 15 0;
#X connect 6 1 2 0;
#X connect 6 2 8 0;
#X connect 8 0 38 0;
#X connect 9 0 39 0;
#X connect 10 0 40 0;
#X connect 11 0 4 0;
#X connect 12 0 5 2;
#X connect 13 0 3 0;
#X connect 14 0 29 0;
#X connect 15 0 18 0;
#X connect 16 0 18 0;
#X connect 17 0 34 0;
#X connect 17 1 35 0;
#X connect 17 2 16 0;
#X connect 18 0 14 0;
#X connect 18 1 13 0;
#X connect 18 2 12 0;
#X connect 18 3 11 0;
#X connect 19 0 32 0;
#X connect 19 1 31 0;
#X connect 19 3 30 0;
#X connect 21 0 17 0;
#X connect 23 0 36 0;
#X connect 25 0 27 0;
#X connect 26 0 29 1;
#X connect 27 0 28 0;
#X connect 28 0 26 1;
#X connect 29 0 5 0;
#X connect 30 0 4 1;
#X connect 31 0 3 1;
#X connect 32 0 26 0;
#X connect 34 0 6 0;
#X connect 34 1 9 0;
#X connect 34 2 10 0;
#X connect 35 0 19 0;
#X connect 35 1 19 1;
#X connect 35 2 19 2;
#X connect 36 0 37 0;
#X connect 37 0 22 0;
#X connect 38 0 18 5;
#X connect 39 0 18 6;
#X connect 40 0 18 7;