104 lines
2.1 KiB
Python
104 lines
2.1 KiB
Python
AHRSensor {
|
|
var <id,
|
|
<>calibrationQuat,
|
|
<>quat,
|
|
<>euler,
|
|
<>accel,
|
|
|
|
<battery,
|
|
|
|
<gEx,
|
|
<gEy,
|
|
<gEz;
|
|
|
|
*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 euler = [0, 0, 0];
|
|
|
|
euler[1] = asin(m13.clip(- 1, 1));
|
|
|
|
if ((m13.abs < 0.9999999), {
|
|
euler[0] = atan2(m23.neg, m33);
|
|
euler[2] = atan2(m12.neg, m11);
|
|
}, {
|
|
euler[0] = atan2(m32, m22);
|
|
euler[2] = 0;
|
|
});
|
|
|
|
euler.postln;
|
|
euler;
|
|
}
|
|
|
|
init {
|
|
quat = Quaternion;
|
|
calibrationQuat = Quaternion;
|
|
euler = [0, 0, 0];
|
|
accel = [0, 0, 0];
|
|
battery = 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);
|
|
}
|
|
|
|
getGui {
|
|
^VLayout(
|
|
StaticText().string_("Sensor " ++ id),
|
|
HLayout(
|
|
StaticText().string_("euler: "),
|
|
[StaticText().string_("x: "), align: \left],
|
|
[gEx, align: \left],
|
|
StaticText().string_("y: "),
|
|
gEy,
|
|
StaticText().string_("z: "),
|
|
gEz,
|
|
)
|
|
);
|
|
}
|
|
|
|
updateEuler { |newQuat|
|
|
var quatDiff = newQuat / quat;
|
|
euler += quat2euler(quatDiff);
|
|
}
|
|
|
|
refreshGui {
|
|
gEx.string_(euler[0]);
|
|
gEy.string_(euler[1]);
|
|
gEz.string_(euler[2]);
|
|
}
|
|
} |