fix correction direction for negative quaternion

closes #15
ben-wes · Sep 22, 2024 · c36ba4a · c36ba4a
1 parent 58897b1
commit c36ba4a
Showing 1 changed file with 78 additions and 67 deletions.
diff --git a/sensors2quat.pd b/sensors2quat.pd
@@ -1,12 +1,11 @@
-#N canvas 65 88 647 748 10;
-#X msg 56 553 quat w \$1;
-#X msg 113 574 quat x \$1;
-#X msg 227 616 quat z \$1;
-#X msg 170 595 quat y \$1;
+#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 97 462 +;
 #X obj 153 463 +;
 #X obj 283 463 +;
 #X obj 59 519 quaternion/normalize;
@@ -54,14 +53,11 @@
 #X obj 267 294 * 0.067;
 #X obj 199 294 * -0.067;
 #X obj 131 294 * -0.067;
-#X obj 506 350 * 0.0018;
-#X obj 389 350 * 0.0018;
-#X obj 330 350 * 0.0018;
 #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 97 364 f 1;
+#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;
@@ -116,62 +112,77 @@
 #X restore 330 289 pd add_acceleration_quaternion;
 #X text 40 364 feedback;
 #X obj 77 51 inlet;
-#X obj 65 678 outlet;
+#X obj 65 728 outlet;
 #X obj 372 673 outlet;
 #X obj 372 642 pack f f f f;
-#X connect 0 0 35 0;
-#X connect 1 0 35 0;
-#X connect 2 0 35 0;
-#X connect 3 0 35 0;
-#X connect 6 0 31 4;
-#X connect 7 0 10 0;
-#X connect 8 0 10 1;
-#X connect 9 0 10 3;
-#X connect 10 0 26 1;
-#X connect 10 0 0 0;
-#X connect 10 0 37 0;
-#X connect 10 1 1 0;
-#X connect 10 1 31 1;
-#X connect 10 1 37 1;
-#X connect 10 2 3 0;
-#X connect 10 2 31 2;
-#X connect 10 2 37 2;
-#X connect 10 3 2 0;
-#X connect 10 3 31 3;
-#X connect 10 3 37 3;
-#X connect 11 0 26 0;
-#X connect 11 1 6 0;
-#X connect 11 2 13 0;
-#X connect 13 0 18 0;
-#X connect 14 0 17 0;
-#X connect 15 0 16 0;
-#X connect 16 0 31 7;
-#X connect 17 0 31 6;
-#X connect 18 0 31 5;
-#X connect 19 0 9 1;
-#X connect 20 0 8 1;
-#X connect 21 0 7 1;
-#X connect 22 0 9 0;
-#X connect 23 0 10 2;
-#X connect 24 0 8 0;
-#X connect 25 0 7 0;
-#X connect 26 0 31 0;
-#X connect 27 0 31 0;
-#X connect 28 0 11 0;
-#X connect 28 1 14 0;
-#X connect 28 2 15 0;
-#X connect 29 0 32 0;
-#X connect 29 1 32 1;
-#X connect 29 2 32 2;
-#X connect 30 0 28 0;
-#X connect 30 1 29 0;
-#X connect 30 2 27 0;
-#X connect 31 0 25 0;
-#X connect 31 1 24 0;
-#X connect 31 2 23 0;
-#X connect 31 3 22 0;
-#X connect 32 0 21 0;
-#X connect 32 1 20 0;
-#X connect 32 3 19 0;
-#X connect 34 0 30 0;
-#X connect 37 0 36 0;
+#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;