Kotni vektorji, kup popravkov
parent
8ca3b235b7
commit
825fa13c36
|
|
@ -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_("e"), align: \right],
|
||||
[gExl, columns: 2],
|
||||
[gEyl, columns: 2],
|
||||
[gEzl, columns: 2],
|
||||
gEx, gEy, gEz
|
||||
],
|
||||
/*
|
||||
[
|
||||
StaticText().string_("euler: "),
|
||||
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_("vec F: "), align: \right],
|
||||
[gVfxl, columns: 2],
|
||||
[gVfyl, columns: 2],
|
||||
[gVfzl, columns: 2]
|
||||
],
|
||||
*/
|
||||
/*
|
||||
[
|
||||
StaticText().string_("vec T: "),
|
||||
[StaticText().string_("x: "), align: \right],
|
||||
gVtx,
|
||||
[StaticText().string_("y: "), align: \right],
|
||||
gVty,
|
||||
[StaticText().string_("z: "), align: \right],
|
||||
gVtz
|
||||
],
|
||||
[
|
||||
[StaticText().string_("vec T:"), align: \right],
|
||||
[gVtxl, columns: 2],
|
||||
[gVtyl, columns: 2],
|
||||
[gVtzl, columns: 2]
|
||||
],
|
||||
*/
|
||||
[
|
||||
StaticText().string_("accel: "),
|
||||
[StaticText().string_("x: "), align: \right],
|
||||
gAx,
|
||||
[StaticText().string_("y: "), align: \right],
|
||||
gAy,
|
||||
[StaticText().string_("z: "), align: \right],
|
||||
gAz,
|
||||
[StaticText().string_("sum: "), align: \right],
|
||||
[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));
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue