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

View File

@ -1,13 +1,21 @@
AHRSensor {
var <id,
<parent,
// Kalibracijski quaternion (kao nula)
<>calibQuat,
<>parent,
// Surov kvaternion iz senzorjev
<>quat,
// Kalibracijski quaternion (kao "nula")
<>refQuat,
// Relativni kvaternion (razlika od parenta)
<relQuat,
// Razlika od prejsnjega do trenutnega quaterniona
<quatDiff,
<>euler,
<>eulerD,
// "Prednji" vektor
<>vecF,
// "Zgornji" vektor
<>vecT,
<>accel,
<>accelSum,
@ -20,24 +28,42 @@ AHRSensor {
<gQx,
<gQy,
<gQz,
// Kalibriraj quaternion
<gQc,
// Referencni quaternion
<gQr,
// Eulerjevi koti
<gEx,
<gEy,
<gEz,
// Relativni eulerjevi koti
<gEdx,
<gEdy,
<gEdz,
<gExl,
<gEyl,
<gEzl,
// Rotacijski vektor
<gVfx,
<gVfy,
<gVfz,
<gVfxl,
<gVfyl,
<gVfzl,
<gVtx,
<gVty,
<gVtz,
<gVtxl,
<gVtyl,
<gVtzl,
// Pospeskomer
<gAx,
<gAy,
<gAz,
<gAs,
// Baterija
<gB,
<gBi,
// Dogodkov na sekundo
<gEps;
@ -48,16 +74,22 @@ AHRSensor {
quat2euler { |quat|
// Quaternion
//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.c.neg, quat.b, quat.d.neg],
// Euler angles; roll, pitch and yaw
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[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
/**/
*/
/*
// Variables
@ -116,12 +148,15 @@ AHRSensor {
}
init {
quat = Quaternion;
calibQuat = Quaternion.new(1, 0, 0, 0);
quat = Quaternion.new(1, 0, 0, 0);
refQuat = Quaternion.new(1, 0, 0, 0);
relQuat = Quaternion.new(1, 0, 0, 0);
euler = [0, 0, 0];
accel = [0, 0, 0];
battery = 0;
eps = 0;
vecF = Cartesian.new(1, 0, 0);
vecT = Cartesian.new(0, 0, 1);
this.guiInit;
}
@ -140,11 +175,23 @@ AHRSensor {
gEx = StaticText().string_(0).stringColor_(cRed);
gEy = StaticText().string_(0).stringColor_(cGreen);
gEz = StaticText().string_(0).stringColor_(cBlue);
gExl = LevelIndicator();
gEyl = LevelIndicator();
gEzl = LevelIndicator();
// Razlika v eulerju
gEdx = StaticText().string_(0).stringColor_(cRed);
gEdy = StaticText().string_(0).stringColor_(cGreen);
gEdz = StaticText().string_(0).stringColor_(cBlue);
gVfx = StaticText().string_(0).stringColor_(cRed);
gVfy = StaticText().string_(0).stringColor_(cGreen);
gVfz = 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);
//gAy = StaticText().string_(0).stringColor_(cGreen);
@ -159,8 +206,8 @@ AHRSensor {
gQy = StaticText().string_(0).stringColor_(cGreen);
gQz = StaticText().string_(0).stringColor_(cBlue);
gQc = Button().string_("cal").action_({ |butt|
this.calibrateQuat;
gQr = Button().string_("ref").action_({ |butt|
this.setRefQuat;
});
gB = StaticText().string_(0).stringColor_(cRed);
@ -170,19 +217,21 @@ AHRSensor {
getGui {
var napis = "Sensor " ++ id, elementi;
if (parent != nil) { napis = napis ++ " (parent " ++ parent ++ ")"};
if (parent != nil) { napis = napis ++ " (parent " ++ parent.id ++ ")"};
elementi = [
[
StaticText().font_(Font("OpenSans", 12, true)).string_(napis),
nil,
[StaticText().string_("bat: "), align: \right],
gB,
[gB, columns: 2],
gBi,
nil,
[StaticText().string_("events/s: "), align: \right],
gEps,
gQc
gQr
],
/*
[
StaticText().string_("quaternion: "),
[StaticText().string_("w: "), align: \right],
@ -194,77 +243,162 @@ AHRSensor {
[StaticText().string_("z: "), align: \right],
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],
gEx,
gVfx,
[StaticText().string_("y: "), align: \right],
gEy,
gVfy,
[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],
gAx,
gVtx,
[StaticText().string_("y: "), align: \right],
gAy,
gVty,
[StaticText().string_("z: "), align: \right],
gAz,
[StaticText().string_("sum: "), align: \right],
gVtz
],
[
[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
],
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;
}
calibrateQuat {
calibQuat = quat;
setQuat { |q|
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|
euler = this.quat2euler(newQuat);
setRefQuat {
refQuat = quat;
euler = [0, 0, 0];
}
updateEulerD { |newQuat|
eulerD = this.quat2euler(newQuat);
updateEuler { |q|
// 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 {
// Stevilo decimalk
var prec = 3;
gQw.string_(quat.a.asStringPrec(prec));
gQx.string_(quat.b.asStringPrec(prec));
gQy.string_(quat.c.asStringPrec(prec));
gQz.string_(quat.d.asStringPrec(prec));
gQw.string_(this.getDecimal(quat.a));
gQx.string_(this.getDecimal(quat.b));
gQy.string_(this.getDecimal(quat.c));
gQz.string_(this.getDecimal(quat.d));
}
refreshGuiEuler {
// Stevilo decimalk
var prec = 3;
gEx.string_(euler[0].asStringPrec(prec));
gEy.string_(euler[1].asStringPrec(prec));
gEz.string_(euler[2].asStringPrec(prec));
gEx.string_(this.getDecimal(euler[0]));
gEy.string_(this.getDecimal(euler[1]));
gEz.string_(this.getDecimal(euler[2]));
gExl.value_(sin(euler[0]).linlin(-1, 1, 0, 1));
gEyl.value_(sin(euler[1]).linlin(-1, 1, 0, 1));
gEzl.value_(sin(euler[2]).linlin(-1, 1, 0, 1));
}
refreshGuiEulerD {
// Stevilo decimalk
var prec = 3;
gEdx.string_(eulerD[0].asStringPrec(prec));
gEdy.string_(eulerD[1].asStringPrec(prec));
gEdz.string_(eulerD[2].asStringPrec(prec));
refreshGuiVector {
gVfx.string_(this.getDecimal(vecF.x));
gVfy.string_(this.getDecimal(vecF.y));
gVfz.string_(this.getDecimal(vecF.z));
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 {
// Stevilo decimalk
//var prec = 2;
@ -275,13 +409,11 @@ AHRSensor {
[gAx, gAy, gAz].do({|el, i|
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 {
// Stevilo decimalk
var prec = 2;
gB.string_(battery.asStringPrec(prec));
gB.string_(this.getDecimal(battery));
gBi.value_(battery.linlin(3.6, 4.2, 0, 1));
}