hi, i’m working on web which visualize object rotation using euler angle. here’s my flow to get euler angle value:
sensor data (array) -> madgwick (quaternion) -> euler
I tried to rotate using cube.rotation.x = roll in function render but it just rotate based on last value. I need help to rotate based on all value (i have 5 value). Thank you
var sampleFreq = 100.0; // sample frequency in Hz
var betaDef = 1; // 2 * proportional gain
// //---------------------------------------------------------------------------------------------------
// // Variable definitions
var beta = betaDef; // 2 * proportional gain (Kp)
var w = 1.0, x = 0.0, y = 0.0, z = 0.0; // quaternion of sensor frame relative to auxiliary frame
// console.log("Madgwick here");
var gx= [4, 9, 16, 25, 5];
var gy= [4, 9, 2, 5, 4];
var gz= [6, 6, 3, 9, 4];
var ax= [7, 9, 8, 5, 1];
var ay= [4, 8, 2, 1, 4];
var az= [6, 3, 8, 5, 2];
var mx= [7, 5, 3, 6, 9];
var my= [4, 9, 2, 7, 8];
var mz= [8, 12, 5, 7, 3];
var i =0;
for (i = 0; i < 5; i++) {
madgwickAHRSupdate(gx[i], gy[i], gz[i], ax[i], ay[i], az[i], mx[i], my[i], mz[i])
}
function madgwickAHRSupdate(gx, gy, gz, ax, ay, az, mx, my, mz) {
var recipNorm;
var s0, s1, s2, s3;
var qDot1, qDot2, qDot3, qDot4;
var hx, hy;
var V_2wmx, V_2wmy, V_2wmz, V_2xmx, V_2bx, V_2bz, V_4bx, V_4bz, V_2w, V_2x, V_2y, V_2z, V_2wy, V_2yz;
var ww, wx, wy, wz, xx, xy, xz, yy, yz, zz;
// // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if ((mx === 0.0) && (my === 0.0) && (mz === 0.0)) {
madgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
return;
}
// // Rate of change of quaternion from gyroscope
qDot1 = 0.5 * (-x * gx - y * gy - z * gz);
qDot2 = 0.5 * (w * gx + y * gz - z * gy);
qDot3 = 0.5 * (w * gy - x * gz + z * gx);
qDot4 = 0.5 * (w * gz + x * gy - y * gx);
// // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if (!((ax === 0.0) && (ay === 0.0) && (az === 0.0))) {
// // Normalise accelerometer measurement
recipNorm = Math.pow(ax * ax + ay * ay + az * az, -0.5);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// // Normalise magnetometer measurement
recipNorm = Math.pow(mx * mx + my * my + mz * mz, -0.5);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// // Auxiliary variables to avoid repeated arithmetic
V_2wmx = 2.0 * w * mx;
V_2wmy = 2.0 * w * my;
V_2wmz = 2.0 * w * mz;
V_2xmx = 2.0 * x * mx;
V_2w = 2.0 * w;
V_2x = 2.0 * x;
V_2y = 2.0 * y;
V_2z = 2.0 * z;
V_2wy = 2.0 * w * y;
V_2yz = 2.0 * y * z;
ww = w * w;
wx = w * x;
wy = w * y;
wz = w * z;
xx = x * x;
xy = x * y;
xz = x * z;
yy = y * y;
yz = y * z;
zz = z * z;
// // Reference direction of Earth's magnetic field
hx = mx * ww - V_2wmy * z + V_2wmz * y + mx * xx + V_2x * my * y + V_2x * mz * z - mx * yy - mx * zz;
hy = V_2wmx * z + my * ww - V_2wmz * x + V_2xmx * y - my * xx + my * yy + V_2y * mz * z - my * zz;
V_2bx = Math.sqrt(hx * hx + hy * hy);
V_2bz = -V_2wmx * y + V_2wmy * x + mz * ww + V_2xmx * z - mz * xx + V_2y * my * z - mz * yy + mz * zz;
V_4bx = 2.0 * V_2bx;
V_4bz = 2.0 * V_2bz;
// // Gradient decent algorithm corrective step
s0 = -V_2y * (2.0 * xz - V_2wy - ax) + V_2x * (2.0 * wx + V_2yz - ay) - V_2bz * y * (V_2bx * (0.5 - yy - zz) + V_2bz * (xz - wy) - mx) + (-V_2bx * z + V_2bz * x) * (V_2bx * (xy - wz) + V_2bz * (wx + yz) - my) + V_2bx * y * (V_2bx * (wy + xz) + V_2bz * (0.5 - xx - yy) - mz);
s1 = V_2z * (2.0 * xz - V_2wy - ax) + V_2w * (2.0 * wx + V_2yz - ay) - 4.0 * x * (1 - 2.0 * xx - 2.0 * yy - az) + V_2bz * z * (V_2bx * (0.5 - yy - zz) + V_2bz * (xz - wy) - mx) + (V_2bx * y + V_2bz * w) * (V_2bx * (xy - wz) + V_2bz * (wx + yz) - my) + (V_2bx * z - V_4bz * x) * (V_2bx * (wy + xz) + V_2bz * (0.5 - xx - yy) - mz);
s2 = -V_2w * (2.0 * xz - V_2wy - ax) + V_2z * (2.0 * wx + V_2yz - ay) - 4.0 * y * (1 - 2.0 * xx - 2.0 * yy - az) + (-V_4bx * y - V_2bz * w) * (V_2bx * (0.5 - yy - zz) + V_2bz * (xz - wy) - mx) + (V_2bx * x + V_2bz * z) * (V_2bx * (xy - wz) + V_2bz * (wx + yz) - my) + (V_2bx * w - V_4bz * y) * (V_2bx * (wy + xz) + V_2bz * (0.5 - xx - yy) - mz);
s3 = V_2x * (2.0 * xz - V_2wy - ax) + V_2y * (2.0 * wx + V_2yz - ay) + (-V_4bx * z + V_2bz * x) * (V_2bx * (0.5 - yy - zz) + V_2bz * (xz - wy) - mx) + (-V_2bx * w + V_2bz * y) * (V_2bx * (xy - wz) + V_2bz * (wx + yz) - my) + V_2bx * x * (V_2bx * (wy + xz) + V_2bz * (0.5 - xx - yy) - mz);
recipNorm = Math.pow(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3, -0.5); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// // Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// // Integrate rate of change of quaternion to yield quaternion
w += qDot1 * (1.0 / sampleFreq);
x += qDot2 * (1.0 / sampleFreq);
y += qDot3 * (1.0 / sampleFreq);
z += qDot4 * (1.0 / sampleFreq);
// // Normalise quaternion
recipNorm = Math.pow(w * w + x * x + y * y + z * z, -0.5);
w *= recipNorm;
x *= recipNorm;
y *= recipNorm;
z *= recipNorm;
var result = toEuler(x, y, z, w);
return result;
}
var pitch;
var yaw ;
var roll;
function toEuler(x, y, z, w){
var qw = parseFloat(w);
var qx = parseFloat(x);
var qy = parseFloat(y);
var qz = parseFloat(z)
var qw2 = qw*qw;
var qx2 = qx*qx;
var qy2 = qy*qy;
var qz2 = qz*qz;
var test= qx*qy + qz*qw;
if (test > 0.499) {
pitch = 90;
yaw = 360/Math.PI*Math.atan2(qx,qw);
roll = 0;
}
if (test < -0.499) {
pitch = -90;
yaw = -360/Math.PI*Math.atan2(qx,qw);
roll = 0;
}
var p = Math.asin(2*qx*qy+2*qz*qw);
var y = Math.atan2(2*qy*qw-2*qx*qz,1-2*qy2-2*qz2);
var r = Math.atan2(2*qx*qw-2*qy*qz,1-2*qx2-2*qz2);
pitch = Math.round(p*180/Math.PI);
yaw = Math.round(y*180/Math.PI);
roll = Math.round(r*180/Math.PI);
var objnya = {
x: roll,
y: pitch,
z: yaw
};
console.log(objnya);
document.getElementById("pitch").innerHTML=pitch;
document.getElementById("roll").innerHTML=roll;
document.getElementById("yaw").innerHTML=yaw;
return objnya;
}
//scene
var scene = new THREE.Scene();
//camera
var camera = new THREE.PerspectiveCamera(45, window.innerWidth/window.innerHeight, 1, 10000);
var renderer = new THREE.WebGLRenderer();
camera.position.x = 20;
camera.position.y = 10;
camera.position.z = 600;
camera.lookAt(new THREE.Vector3(0,0,0));
renderer.setClearColor(0xEEEEEE);
renderer.setSize(window.innerWidth, window.innerHeight);
document.body.appendChild(renderer.domElement);
//load cube
var geometry = new THREE.BoxGeometry(100, 180, 100);
var material0 = new THREE.MeshBasicMaterial( {
color: 0xff2222,
} );
var material1 = new THREE.MeshBasicMaterial( {
color: 0xffffff,
} );
var material2 = new THREE.MeshBasicMaterial( {
color: 0xff5800,
} );
var material3 = new THREE.MeshBasicMaterial( {
color: 0x009e60
} );
var material4 = new THREE.MeshBasicMaterial( {
color: 0xffd500
} );
var material5 = new THREE.MeshBasicMaterial( {
color: 0xC41E3A
} );
var materials = [ material0, material1, material2, material3, material4, material5 ];
var cube = new THREE.Mesh(geometry, materials);
scene.add(cube);
function render() {
//requestAnimationFrame(render);
setTimeout( function() {
requestAnimationFrame( render );
cube.rotation.x =roll;
cube.rotation.y =pitch;
cube.rotation.z =yaw;
}, 1000 / 1 );
renderer.render(scene, camera);
//console.log(cube.rotation.x=roll)
};
render();