300 lines
7.0 KiB
Python
300 lines
7.0 KiB
Python
AHRSensor {
|
|
var <id,
|
|
<parent,
|
|
|
|
// Kalibracijski quaternion (kao nula)
|
|
<>calibQuat,
|
|
|
|
<>quat,
|
|
<>euler,
|
|
<>eulerD,
|
|
<>accel,
|
|
<>accelSum,
|
|
|
|
<>battery,
|
|
<>eps,
|
|
|
|
// GUI elementi
|
|
// Quaternion
|
|
<gQw,
|
|
<gQx,
|
|
<gQy,
|
|
<gQz,
|
|
// Kalibriraj quaternion
|
|
<gQc,
|
|
// Eulerjevi koti
|
|
<gEx,
|
|
<gEy,
|
|
<gEz,
|
|
// Relativni eulerjevi koti
|
|
<gEdx,
|
|
<gEdy,
|
|
<gEdz,
|
|
// 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,
|
|
//var q = [quat.b, quat.c, quat.d, quat.a],
|
|
var q = [quat.a, quat.b, quat.c, quat.d],
|
|
// Euler angles; roll, pitch and yaw
|
|
e = [0, 0, 0];
|
|
|
|
/**/
|
|
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;
|
|
calibQuat = Quaternion.new(1, 0, 0, 0);
|
|
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);
|
|
|
|
// Razlika v eulerju
|
|
gEdx = StaticText().string_(0).stringColor_(cRed);
|
|
gEdy = StaticText().string_(0).stringColor_(cGreen);
|
|
gEdz = 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();
|
|
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);
|
|
|
|
gQc = Button().string_("cal").action_({ |butt|
|
|
this.calibrateQuat;
|
|
});
|
|
|
|
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 ++ ")"};
|
|
|
|
elementi = [
|
|
[
|
|
StaticText().font_(Font("OpenSans", 12, true)).string_(napis),
|
|
nil,
|
|
[StaticText().string_("bat: "), align: \right],
|
|
gB,
|
|
gBi,
|
|
[StaticText().string_("events/s: "), align: \right],
|
|
gEps,
|
|
gQc
|
|
],
|
|
[
|
|
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,
|
|
[StaticText().string_("sum: "), align: \right],
|
|
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;
|
|
}
|
|
|
|
updateEuler { |newQuat|
|
|
euler = this.quat2euler(newQuat);
|
|
}
|
|
|
|
updateEulerD { |newQuat|
|
|
eulerD = this.quat2euler(newQuat);
|
|
}
|
|
|
|
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));
|
|
}
|
|
refreshGuiEulerD {
|
|
// Stevilo decimalk
|
|
var prec = 3;
|
|
gEdx.string_(eulerD[0].asStringPrec(prec));
|
|
gEdy.string_(eulerD[1].asStringPrec(prec));
|
|
gEdz.string_(eulerD[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));
|
|
});
|
|
gAs.value_(accelSum.linlin(0, 100, 0, 1));
|
|
}
|
|
|
|
refreshGuiBat {
|
|
// Stevilo decimalk
|
|
var prec = 2;
|
|
gB.string_(battery.asStringPrec(prec));
|
|
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;
|
|
}
|
|
} |