g1smo
/
c2-utopia
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Kotni vektorji, kup popravkov

main
Jurij Podgoršek 2024-09-26 15:04:21 +02:00
parent 8ca3b235b7
commit 825fa13c36
1 changed files with 208 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

278
AHRSensor/AHRSensor.sc
View File

@ -1,13 +1,21 @@
AHRSensor { AHRSensor {
var <id, var <id,
<parent, <>parent,
// Kalibracijski quaternion (kao nula)
<>calibQuat,
// Surov kvaternion iz senzorjev
<>quat, <>quat,
// Kalibracijski quaternion (kao "nula")
<>refQuat,
// Relativni kvaternion (razlika od parenta)
<relQuat,
// Razlika od prejsnjega do trenutnega quaterniona
<quatDiff,
<>euler, <>euler,
<>eulerD, // "Prednji" vektor
<>vecF,
// "Zgornji" vektor
<>vecT,
<>accel, <>accel,
<>accelSum, <>accelSum,
@ -20,24 +28,42 @@ AHRSensor {
<gQx, <gQx,
<gQy, <gQy,
<gQz, <gQz,
// Kalibriraj quaternion
<gQc, // Referencni quaternion
<gQr,
// Eulerjevi koti // Eulerjevi koti
<gEx, <gEx,
<gEy, <gEy,
<gEz, <gEz,
// Relativni eulerjevi koti <gExl,
<gEdx, <gEyl,
<gEdy, <gEzl,
<gEdz,
// Rotacijski vektor
<gVfx,
<gVfy,
<gVfz,
<gVfxl,
<gVfyl,
<gVfzl,
<gVtx,
<gVty,
<gVtz,
<gVtxl,
<gVtyl,
<gVtzl,
// Pospeskomer // Pospeskomer
<gAx, <gAx,
<gAy, <gAy,
<gAz, <gAz,
<gAs, <gAs,
// Baterija // Baterija
<gB, <gB,
<gBi, <gBi,
// Dogodkov na sekundo // Dogodkov na sekundo
<gEps; <gEps;
@ -48,16 +74,22 @@ AHRSensor {
quat2euler { |quat| quat2euler { |quat|
// Quaternion // Quaternion
//var w = quat.a, x = quat.b, y = quat.c, z = quat.d, //var w = quat.a, x = quat.b, y = quat.c, z = quat.d,
//var q = [quat.b, quat.c, quat.d, quat.a],
// Quaternion v supercolliderju gre po zaporedju wxyz = abcd
// Iz foruma (BNO055): x = -y, y = x, z = -z
var q = [quat.a, quat.b, quat.c, quat.d], var q = [quat.a, quat.b, quat.c, quat.d],
//var q = [quat.a, quat.c.neg, quat.b, quat.d.neg],
// Euler angles; roll, pitch and yaw // Euler angles; roll, pitch and yaw
e = [0, 0, 0]; e = [0, 0, 0];
/**/ e[0] = atan2((2 * q[2] * q[0]) - (2 * q[1] * q[3]), 1 - (2 * q[2] * q[2]) - (2 * q[3] * q[3]));
e[1] = asin((2 * q[1] * q[2]) + (2 * q[3] * q[0]));
e[2] = atan2((2 * q[1] * q[0]) - (2 * q[2] * q[3]), 1 - (2 * q[1] * q[1]) - (2 * q[3] * q[3]));
/*
e[0] = atan2( (2 * q[1] * q[2]) - (2 * q[0] * q[3]), (2 * q[0]*q[0]) + (2 * q[1] * q[1]) - 1); // psi e[0] = atan2( (2 * q[1] * q[2]) - (2 * q[0] * q[3]), (2 * q[0]*q[0]) + (2 * q[1] * q[1]) - 1); // psi
e[1] = asin( (2 * q[1] * q[3]) + (2 * q[0] * q[2])).neg; // theta e[1] = asin( (2 * q[1] * q[3]) + (2 * q[0] * q[2])).neg; // theta
e[2] = atan2( (2 * q[2] * q[3]) - (2 * q[0] * q[1]), (2 * q[0] * q[0]) + (2 * q[3] * q[3]) - 1); // phi e[2] = atan2( (2 * q[2] * q[3]) - (2 * q[0] * q[1]), (2 * q[0] * q[0]) + (2 * q[3] * q[3]) - 1); // phi
/**/ */
/* /*
// Variables // Variables
@ -116,12 +148,15 @@ AHRSensor {
} }
init { init {
quat = Quaternion; quat = Quaternion.new(1, 0, 0, 0);
calibQuat = Quaternion.new(1, 0, 0, 0); refQuat = Quaternion.new(1, 0, 0, 0);
relQuat = Quaternion.new(1, 0, 0, 0);
euler = [0, 0, 0]; euler = [0, 0, 0];
accel = [0, 0, 0]; accel = [0, 0, 0];
battery = 0; battery = 0;
eps = 0; eps = 0;
vecF = Cartesian.new(1, 0, 0);
vecT = Cartesian.new(0, 0, 1);
this.guiInit; this.guiInit;
} }
@ -140,11 +175,23 @@ AHRSensor {
gEx = StaticText().string_(0).stringColor_(cRed); gEx = StaticText().string_(0).stringColor_(cRed);
gEy = StaticText().string_(0).stringColor_(cGreen); gEy = StaticText().string_(0).stringColor_(cGreen);
gEz = StaticText().string_(0).stringColor_(cBlue); gEz = StaticText().string_(0).stringColor_(cBlue);
gExl = LevelIndicator();
gEyl = LevelIndicator();
gEzl = LevelIndicator();
// Razlika v eulerju gVfx = StaticText().string_(0).stringColor_(cRed);
gEdx = StaticText().string_(0).stringColor_(cRed); gVfy = StaticText().string_(0).stringColor_(cGreen);
gEdy = StaticText().string_(0).stringColor_(cGreen); gVfz = StaticText().string_(0).stringColor_(cBlue);
gEdz = StaticText().string_(0).stringColor_(cBlue); gVfxl = LevelIndicator();
gVfyl = LevelIndicator();
gVfzl = LevelIndicator();
gVtx = StaticText().string_(0).stringColor_(cRed);
gVty = StaticText().string_(0).stringColor_(cGreen);
gVtz = StaticText().string_(0).stringColor_(cBlue);
gVtxl = LevelIndicator();
gVtyl = LevelIndicator();
gVtzl = LevelIndicator();
//gAx = StaticText().string_(0).stringColor_(cRed); //gAx = StaticText().string_(0).stringColor_(cRed);
//gAy = StaticText().string_(0).stringColor_(cGreen); //gAy = StaticText().string_(0).stringColor_(cGreen);
@ -159,8 +206,8 @@ AHRSensor {
gQy = StaticText().string_(0).stringColor_(cGreen); gQy = StaticText().string_(0).stringColor_(cGreen);
gQz = StaticText().string_(0).stringColor_(cBlue); gQz = StaticText().string_(0).stringColor_(cBlue);
gQc = Button().string_("cal").action_({ |butt| gQr = Button().string_("ref").action_({ |butt|
this.calibrateQuat; this.setRefQuat;
}); });
gB = StaticText().string_(0).stringColor_(cRed); gB = StaticText().string_(0).stringColor_(cRed);
@ -170,19 +217,21 @@ AHRSensor {
getGui { getGui {
var napis = "Sensor " ++ id, elementi; var napis = "Sensor " ++ id, elementi;
if (parent != nil) { napis = napis ++ " (parent " ++ parent ++ ")"}; if (parent != nil) { napis = napis ++ " (parent " ++ parent.id ++ ")"};
elementi = [ elementi = [
[ [
StaticText().font_(Font("OpenSans", 12, true)).string_(napis), StaticText().font_(Font("OpenSans", 12, true)).string_(napis),
nil, nil,
[StaticText().string_("bat: "), align: \right], [StaticText().string_("bat: "), align: \right],
gB, [gB, columns: 2],
gBi, gBi,
nil,
[StaticText().string_("events/s: "), align: \right], [StaticText().string_("events/s: "), align: \right],
gEps, gEps,
gQc gQr
], ],
/*
[ [
StaticText().string_("quaternion: "), StaticText().string_("quaternion: "),
[StaticText().string_("w: "), align: \right], [StaticText().string_("w: "), align: \right],
@ -194,77 +243,162 @@ AHRSensor {
[StaticText().string_("z: "), align: \right], [StaticText().string_("z: "), align: \right],
gQz, gQz,
], ],
*/
[ [
StaticText().string_("euler: "), [StaticText().string_("e"), align: \right],
[gExl, columns: 2],
[gEyl, columns: 2],
[gEzl, columns: 2],
gEx, gEy, gEz
],
/*
[
StaticText().string_("vec F: "),
[StaticText().string_("x: "), align: \right], [StaticText().string_("x: "), align: \right],
gEx, gVfx,
[StaticText().string_("y: "), align: \right], [StaticText().string_("y: "), align: \right],
gEy, gVfy,
[StaticText().string_("z: "), align: \right], [StaticText().string_("z: "), align: \right],
gEz, gVfz
], ],
[ [
StaticText().string_("accel: "), [StaticText().string_("vec F: "), align: \right],
[gVfxl, columns: 2],
[gVfyl, columns: 2],
[gVfzl, columns: 2]
],
*/
/*
[
StaticText().string_("vec T: "),
[StaticText().string_("x: "), align: \right], [StaticText().string_("x: "), align: \right],
gAx, gVtx,
[StaticText().string_("y: "), align: \right], [StaticText().string_("y: "), align: \right],
gAy, gVty,
[StaticText().string_("z: "), align: \right], [StaticText().string_("z: "), align: \right],
gAz, gVtz
[StaticText().string_("sum: "), align: \right], ],
[
[StaticText().string_("vec T:"), align: \right],
[gVtxl, columns: 2],
[gVtyl, columns: 2],
[gVtzl, columns: 2]
],
*/
[
[StaticText().string_("accel: "), align: \right],
[gAx, columns: 2],
[gAy, columns: 2],
[gAz, columns: 2],
gAs gAs
], ]
if (parent != nil) {
[
StaticText().string_("euler d: "),
[StaticText().string_("x: "), align: \right],
gEdx,
[StaticText().string_("y: "), align: \right],
gEdy,
[StaticText().string_("z: "), align: \right],
gEdz,
];
}
]; ];
^elementi; ^elementi;
} }
calibrateQuat { setQuat { |q|
calibQuat = quat; var rq;
// Razlika od prejsnjega quaterniona
quatDiff = quat.conjugate * q;
// "Surov quat"
quat = q;
// Referencen quat
rq = refQuat.conjugate * q;
// Ce imamo parenta, shrani razliko, sicer samo referencen
relQuat = if ((parent != nil), {
parent.relQuat.conjugate * rq;
// TODO ali bi tudi childe poapdejtali? Mislim, da ni treba, ker dokaj pogosto pride nov q
}, {
rq;
});
} }
updateEuler { |newQuat| setRefQuat {
euler = this.quat2euler(newQuat); refQuat = quat;
euler = [0, 0, 0];
} }
updateEulerD { |newQuat| updateEuler { |q|
eulerD = this.quat2euler(newQuat); // Normalize first ?
euler = [q.tilt, q.tumble, q.rotate];
//euler = euler + [quatDiff.tilt, quatDiff.tumble, quatDiff.rotate];
//euler = this.quat2euler(newQuat);
}
rotateVector { |q|
// rotiramo vektor s quaternionom
var vF = Cartesian.new(1, 0, 0), vT = Cartesian.new(0, 0, 1),
u, s, newVecF, newVecT;
u = Cartesian.new(q.b, q.c, q.d);
s = q.a;
vecF = (
(u * (2.0 * this.vecDotProd(u, vF)))
+ (vF * ((s * s) - this.vecDotProd(u, u)))
) + (
this.vecCrossProd(u, vF) * (2.0 * s)
);
vecT = (
(u * (2.0 * this.vecDotProd(u, vT)))
+ (vT * ((s * s) - this.vecDotProd(u, u)))
) + (
this.vecCrossProd(u, vT) * (2.0 * s)
);
}
vecDotProd { |v1, v2|
^((v1.x * v2.x) + (v1.y * v2.y) + (v1.z * v2.z));
}
vecCrossProd { |v1, v2|
^Cartesian.new(
(v1.y * v2.z) - (v1.z * v2.y),
(v1.z * v2.x) - (v1.x * v2.z),
(v1.x * v2.y) - (v1.y * v2.x));
}
getDecimal { |num|
// Stevilo decimalk
var div = 0.01, rem;
rem = num % div;
^(num - rem);
} }
refreshGuiQuat { refreshGuiQuat {
// Stevilo decimalk gQw.string_(this.getDecimal(quat.a));
var prec = 3; gQx.string_(this.getDecimal(quat.b));
gQw.string_(quat.a.asStringPrec(prec)); gQy.string_(this.getDecimal(quat.c));
gQx.string_(quat.b.asStringPrec(prec)); gQz.string_(this.getDecimal(quat.d));
gQy.string_(quat.c.asStringPrec(prec));
gQz.string_(quat.d.asStringPrec(prec));
} }
refreshGuiEuler { refreshGuiEuler {
// Stevilo decimalk gEx.string_(this.getDecimal(euler[0]));
var prec = 3; gEy.string_(this.getDecimal(euler[1]));
gEx.string_(euler[0].asStringPrec(prec)); gEz.string_(this.getDecimal(euler[2]));
gEy.string_(euler[1].asStringPrec(prec)); gExl.value_(sin(euler[0]).linlin(-1, 1, 0, 1));
gEz.string_(euler[2].asStringPrec(prec)); gEyl.value_(sin(euler[1]).linlin(-1, 1, 0, 1));
gEzl.value_(sin(euler[2]).linlin(-1, 1, 0, 1));
} }
refreshGuiEulerD {
// Stevilo decimalk refreshGuiVector {
var prec = 3; gVfx.string_(this.getDecimal(vecF.x));
gEdx.string_(eulerD[0].asStringPrec(prec)); gVfy.string_(this.getDecimal(vecF.y));
gEdy.string_(eulerD[1].asStringPrec(prec)); gVfz.string_(this.getDecimal(vecF.z));
gEdz.string_(eulerD[2].asStringPrec(prec)); gVfxl.value_(vecF.x.linlin(-1, 1, 0, 1));
gVfyl.value_(vecF.y.linlin(-1, 1, 0, 1));
gVfzl.value_(vecF.z.linlin(-1, 1, 0, 1));
gVtx.string_(this.getDecimal(vecT.x));
gVty.string_(this.getDecimal(vecT.y));
gVtz.string_(this.getDecimal(vecT.z));
gVtxl.value_(vecT.x.linlin(-1, 1, 0, 1));
gVtyl.value_(vecT.y.linlin(-1, 1, 0, 1));
gVtzl.value_(vecT.z.linlin(-1, 1, 0, 1));
} }
refreshGuiAccel { refreshGuiAccel {
// Stevilo decimalk // Stevilo decimalk
//var prec = 2; //var prec = 2;
@ -275,13 +409,11 @@ AHRSensor {
[gAx, gAy, gAz].do({|el, i| [gAx, gAy, gAz].do({|el, i|
el.value_(accel[i].linlin(from, to, 0, 1)); el.value_(accel[i].linlin(from, to, 0, 1));
}); });
gAs.value_(accelSum.linlin(0, 100, 0, 1)); gAs.value_(accelSum.linlin(0, 50, 0, 1));
} }
refreshGuiBat { refreshGuiBat {
// Stevilo decimalk gB.string_(this.getDecimal(battery));
var prec = 2;
gB.string_(battery.asStringPrec(prec));
gBi.value_(battery.linlin(3.6, 4.2, 0, 1)); gBi.value_(battery.linlin(3.6, 4.2, 0, 1));
} }