430 lines
10 KiB
Python
430 lines
10 KiB
Python
AHRSensor {
|
|
var <id,
|
|
<>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,
|
|
// "Prednji" vektor
|
|
<>vecF,
|
|
// "Zgornji" vektor
|
|
<>vecT,
|
|
<>accel,
|
|
<>accelSum,
|
|
|
|
<>battery,
|
|
<>eps,
|
|
|
|
// GUI elementi
|
|
// Quaternion
|
|
<gQw,
|
|
<gQx,
|
|
<gQy,
|
|
<gQz,
|
|
|
|
// Referencni quaternion
|
|
<gQr,
|
|
|
|
// Eulerjevi koti
|
|
<gEx,
|
|
<gEy,
|
|
<gEz,
|
|
<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;
|
|
|
|
*new { |id, parent = nil|
|
|
^super.newCopyArgs(id, parent).init;
|
|
}
|
|
|
|
quat2euler { |quat|
|
|
// Quaternion
|
|
//var w = quat.a, x = quat.b, y = quat.c, z = quat.d,
|
|
|
|
// 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
|
|
sinr_cosp, cosr_cosp, sinp, cosp, siny_cosp, cosy_cosp;
|
|
// Conversion source:
|
|
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_code_2
|
|
|
|
sinr_cosp = 2 * (w * x * y * z);
|
|
cosr_cosp = 1 - (2 * (x * x * y * y));
|
|
// roll (x-axis rotation)
|
|
e[0] = atan2(sinr_cosp, cosr_cosp);
|
|
|
|
sinp = sqrt(1 + (2 * ((w * y) - (x * z))));
|
|
cosp = sqrt(1 - (2 * ((w * y) - (x * z))));
|
|
// pitch (y-axis rotation)
|
|
e[1] = 2 * atan2(sinp, cosp) - 0.5pi;
|
|
|
|
siny_cosp = 2 * ((w * z) + (x * y));
|
|
cosy_cosp = 1 - (2 * ((y * y) + (z * z)));
|
|
// yaw (z-axis rotation)
|
|
e[2] = atan2(siny_cosp, cosy_cosp);
|
|
*/
|
|
|
|
/*
|
|
x2, y2, z2, xx, xy, xz, yy, yz, zz, wx, wy, wz,
|
|
matrix, m11, m12, m13, m21, m22, m23, m31, m32, m33;
|
|
|
|
x2 = x + x; y2 = y + y; z2 = z + z;
|
|
xx = x * x2; xy = x * y2; xz = x * z2;
|
|
yy = y * y2; yz = y * z2; zz = z * z2;
|
|
wx = w * x2; wy = w * y2; wz = w * z2;
|
|
|
|
matrix = [
|
|
1 - ( yy + zz ), xy + wz, xz - wy, 0,
|
|
xy - wz, 1 - ( xx + zz ), yz + wx, 0,
|
|
xz + wy, yz - wx, 1 - ( xx + yy ), 0,
|
|
0, 0, 0, 1
|
|
];
|
|
|
|
m11 = matrix[0]; m12 = matrix[4]; m13 = matrix[8];
|
|
m21 = matrix[1]; m22 = matrix[5]; m23 = matrix[9];
|
|
m31 = matrix[2]; m32 = matrix[6]; m33 = matrix[10];
|
|
|
|
e[1] = asin(m13.clip(-1, 1));
|
|
|
|
if ((m13.abs < 0.9999999), {
|
|
e[0] = atan2(m23.neg, m33);
|
|
e[2] = atan2(m12.neg, m11);
|
|
}, {
|
|
e[0] = atan2(m32, m22);
|
|
e[2] = 0;
|
|
});
|
|
*/
|
|
|
|
^e;
|
|
}
|
|
|
|
init {
|
|
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;
|
|
}
|
|
|
|
guiInit {
|
|
// Barve
|
|
var cRed, cGreen, cBlue, cPurple, cI;
|
|
// Intenziteta
|
|
cI = 0.8;
|
|
|
|
cRed = Color.new(cI, 0, 0);
|
|
cGreen = Color.new(0, cI, 0);
|
|
cBlue = Color.new(0, 0, cI);
|
|
cPurple = Color.new(cI, 0, cI);
|
|
|
|
gEx = StaticText().string_(0).stringColor_(cRed);
|
|
gEy = StaticText().string_(0).stringColor_(cGreen);
|
|
gEz = StaticText().string_(0).stringColor_(cBlue);
|
|
gExl = LevelIndicator();
|
|
gEyl = LevelIndicator();
|
|
gEzl = LevelIndicator();
|
|
|
|
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);
|
|
//gAz = StaticText().string_(0).stringColor_(cBlue);
|
|
gAx = LevelIndicator();
|
|
gAy = LevelIndicator();
|
|
gAz = LevelIndicator();
|
|
gAs = LevelIndicator();
|
|
|
|
gQw = StaticText().string_(0).stringColor_(cPurple);
|
|
gQx = StaticText().string_(0).stringColor_(cRed);
|
|
gQy = StaticText().string_(0).stringColor_(cGreen);
|
|
gQz = StaticText().string_(0).stringColor_(cBlue);
|
|
|
|
gQr = Button().string_("ref").action_({ |butt|
|
|
this.setRefQuat;
|
|
});
|
|
|
|
gB = StaticText().string_(0).stringColor_(cRed);
|
|
gBi = LevelIndicator();
|
|
gEps = StaticText().string_(0);
|
|
}
|
|
|
|
getGui {
|
|
var napis = "Sensor " ++ id, elementi;
|
|
if (parent != nil) { napis = napis ++ " (parent " ++ parent.id ++ ")"};
|
|
|
|
elementi = [
|
|
[
|
|
StaticText().font_(Font("OpenSans", 12, true)).string_(napis),
|
|
nil,
|
|
[StaticText().string_("bat: "), align: \right],
|
|
[gB, columns: 2],
|
|
gBi,
|
|
nil,
|
|
[StaticText().string_("events/s: "), align: \right],
|
|
gEps,
|
|
gQr
|
|
],
|
|
[
|
|
StaticText().string_("quaternion: "),
|
|
[StaticText().string_("w: "), align: \right],
|
|
gQw,
|
|
[StaticText().string_("x: "), align: \right],
|
|
gQx,
|
|
[StaticText().string_("y: "), align: \right],
|
|
gQy,
|
|
[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_("vec F: "),
|
|
[StaticText().string_("x: "), align: \right],
|
|
gVfx,
|
|
[StaticText().string_("y: "), align: \right],
|
|
gVfy,
|
|
[StaticText().string_("z: "), align: \right],
|
|
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: "), align: \right],
|
|
[gAx, columns: 2],
|
|
[gAy, columns: 2],
|
|
[gAz, columns: 2],
|
|
gAs
|
|
]
|
|
];
|
|
^elementi;
|
|
}
|
|
|
|
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;
|
|
});
|
|
}
|
|
|
|
setRefQuat {
|
|
refQuat = quat;
|
|
euler = [0, 0, 0];
|
|
}
|
|
|
|
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 {
|
|
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 {
|
|
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));
|
|
}
|
|
|
|
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;
|
|
//gAx.string_(accel[0].asStringPrec(prec));
|
|
//gAy.string_(accel[1].asStringPrec(prec));
|
|
//gAz.string_(accel[2].asStringPrec(prec));
|
|
var from = -50, to = 50;
|
|
[gAx, gAy, gAz].do({|el, i|
|
|
el.value_(accel[i].linlin(from, to, 0, 1));
|
|
});
|
|
gAs.value_(accelSum.linlin(0, 50, 0, 1));
|
|
}
|
|
|
|
refreshGuiBat {
|
|
gB.string_(this.getDecimal(battery));
|
|
gBi.value_(battery.linlin(3.6, 4.2, 0, 1));
|
|
}
|
|
|
|
refreshGuiEps {
|
|
// Stevilo decimalk
|
|
var prec = 2;
|
|
gEps.string_(eps);
|
|
}
|
|
|
|
refreshGui {
|
|
this.refreshGuiQuat;
|
|
this.refreshGuiEuler;
|
|
this.refreshGuiAccel;
|
|
this.refreshGuiBat;
|
|
}
|
|
} |