206 lines
4.4 KiB
Python
206 lines
4.4 KiB
Python
AHRSensor {
|
|
var <id,
|
|
<>calibrationQuat,
|
|
<>quat,
|
|
<>euler,
|
|
<>accel,
|
|
|
|
<>battery,
|
|
<>eps,
|
|
|
|
// GUI elementi
|
|
// Quaternion
|
|
<gQw,
|
|
<gQx,
|
|
<gQy,
|
|
<gQz,
|
|
// Eulerjevi koti
|
|
<gEx,
|
|
<gEy,
|
|
<gEz,
|
|
// Pospeskomer
|
|
<gAx,
|
|
<gAy,
|
|
<gAz,
|
|
// Baterija
|
|
<gB,
|
|
// Dogodkov na sekundo
|
|
<gEps;
|
|
|
|
*new { |id|
|
|
^super.newCopyArgs(id).init;
|
|
}
|
|
|
|
quat2euler { |quat|
|
|
var w = quat.a, x = quat.b, y = quat.c, z = quat.d,
|
|
|
|
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
|
|
];
|
|
|
|
var m11 = matrix[0], m12 = matrix[4], m13 = matrix[8];
|
|
var m21 = matrix[1], m22 = matrix[5], m23 = matrix[9];
|
|
var m31 = matrix[2], m32 = matrix[6], m33 = matrix[10];
|
|
|
|
var e = [0, 0, 0];
|
|
|
|
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;
|
|
calibrationQuat = Quaternion;
|
|
euler = [0, 0, 0];
|
|
accel = [0, 0, 0];
|
|
battery = 0;
|
|
eps = 0;
|
|
|
|
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);
|
|
|
|
//gAx = StaticText().string_(0).stringColor_(cRed);
|
|
//gAy = StaticText().string_(0).stringColor_(cGreen);
|
|
//gAz = StaticText().string_(0).stringColor_(cBlue);
|
|
gAx = LevelIndicator();
|
|
gAy = LevelIndicator();
|
|
gAz = 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);
|
|
|
|
gB = StaticText().string_(0).stringColor_(cRed);
|
|
gEps = StaticText().string_(0);
|
|
}
|
|
|
|
getGui {
|
|
^[
|
|
[
|
|
StaticText().font_(Font("OpenSans", 12, true)).string_("Sensor " ++ id),
|
|
nil,
|
|
nil,
|
|
[StaticText().string_("bat: "), align: \right],
|
|
gB,
|
|
[StaticText().string_("events/s: "), align: \right],
|
|
gEps
|
|
],
|
|
[
|
|
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_("euler: "),
|
|
[StaticText().string_("x: "), align: \right],
|
|
gEx,
|
|
[StaticText().string_("y: "), align: \right],
|
|
gEy,
|
|
[StaticText().string_("z: "), align: \right],
|
|
gEz,
|
|
],
|
|
[
|
|
StaticText().string_("accel: "),
|
|
[StaticText().string_("x: "), align: \right],
|
|
gAx,
|
|
[StaticText().string_("y: "), align: \right],
|
|
gAy,
|
|
[StaticText().string_("z: "), align: \right],
|
|
gAz,
|
|
],
|
|
[]
|
|
];
|
|
}
|
|
|
|
updateEuler { |newQuat|
|
|
var quatDiff = newQuat / quat;
|
|
euler = euler + this.quat2euler(quatDiff);
|
|
}
|
|
|
|
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));
|
|
}
|
|
|
|
refreshGuiEuler {
|
|
// Stevilo decimalk
|
|
var prec = 3;
|
|
gEx.string_(euler[0].asStringPrec(prec));
|
|
gEy.string_(euler[1].asStringPrec(prec));
|
|
gEz.string_(euler[2].asStringPrec(prec));
|
|
}
|
|
|
|
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));
|
|
});
|
|
}
|
|
|
|
refreshGuiBat {
|
|
// Stevilo decimalk
|
|
var prec = 2;
|
|
gB.string_(battery.asStringPrec(prec));
|
|
}
|
|
|
|
refreshGuiEps {
|
|
// Stevilo decimalk
|
|
var prec = 2;
|
|
gEps.string_(eps);
|
|
}
|
|
|
|
refreshGui {
|
|
this.refreshGuiQuat;
|
|
this.refreshGuiEuler;
|
|
this.refreshGuiAccel;
|
|
this.refreshGuiBat;
|
|
}
|
|
} |