diff --git a/static/scripts/cubes.js b/static/scripts/cubes.js index 27611d0..4f18b4f 100644 --- a/static/scripts/cubes.js +++ b/static/scripts/cubes.js @@ -55,15 +55,19 @@ scene.add(pointLight); cube.position.x = 0 cube2.position.x = 0 -let manCalibration = new THREE.Euler( 0, 0, 0, 'XYZ' ) +// calibration globals +let manCalibration = new THREE.Euler( 0, 0, 0, 'YXZ' ) let calibrationRot = new THREE.Quaternion() +let calPitch = 0 +let calRoll = 0 +let calYaw = 0 let quaternionOffset = document.getElementById("quaternionOffset") function renderTCP(x, y, z) { let calibration = new THREE.Quaternion().setFromEuler(manCalibration) // let calibration = new THREE.Quaternion().setFromEuler(new THREE.Euler( radFromAngle, 0, 0, 'YXZ' )) - let eul = new THREE.Euler( x, z, y, 'YXZ' ); + let eul = new THREE.Euler( x, y, z, 'YXZ' ); // let eul = new THREE.Euler( x, z, y, 'ZXY' ); // XYZ XZY YZX YXZ ZXY ZYX cube2.quaternion.setFromEuler(eul).multiply(calibrationRot).multiply(calibration) @@ -76,9 +80,7 @@ function renderTCP(x, y, z) { function renderSerial(x, y, z) { // requestAnimationFrame(renderSerial); - // let eul = new THREE.Euler( x, y, z, 'YXZ' ); // XYZ XZY YZX YXZ ZXY ZYX - // let eul = new THREE.Euler( x, z, y, 'ZXY' ); // XYZ XZY YZX YXZ ZXY ZYX - let eul = new THREE.Euler( x, z, y, 'YXZ' ); + let eul = new THREE.Euler( x, y, z, 'YXZ' ); // XYZ XZY YZX YXZ ZXY ZYX cube.quaternion.setFromEuler(eul) @@ -106,10 +108,12 @@ rollRange.oninput = () => { function delCalibration(evt) { calibrationRot = new THREE.Quaternion() - manCalibration = new THREE.Euler( 0, 0, 0, 'XYZ' ) + manCalibration = new THREE.Euler( 0, 0, 0, 'YXZ' ) pitchRange.value = 0 yawRange.value = 0 rollRange.value = 0 + calRoll = 0 + calPitch = 0 } var calState = false; @@ -126,7 +130,7 @@ function manualCalibration(evt) { calState = !calState console.log("mancal OFF") con.style.display = "none" - manCalibration = new THREE.Euler( 0, 0, 0, 'XYZ' ) + manCalibration = new THREE.Euler( 0, 0, 0, 'YXZ' ) pitchRange.value = 0 yawRange.value = 0 rollRange.value = 0 @@ -137,6 +141,14 @@ function calibrate(evt) { let serOrientation = cube.quaternion.clone() let tcpOrientation = cube2.quaternion.clone().multiply(calibrationRot.clone().invert()) let diff = tcpOrientation.invert().multiply(serOrientation) + + let old = new THREE.Euler().setFromQuaternion( cube2.quaternion, 'YXZ' ) + let dif = new THREE.Euler().setFromQuaternion( diff ) + console.log("OLD:","pitch", old.x * 180/Math.PI, "yaw", old.y * 180/Math.PI, "roll", old.z * 180/Math.PI) + console.log("DIFF:","pitch", dif.x * 180/Math.PI, "yaw", dif.y * 180/Math.PI, "roll", dif.z * 180/Math.PI) + calPitch = dif.x // * 180/Math.PI + calYaw = dif.y // * 180/Math.PI + calRoll = dif.z // * 180/Math.PI calibrationRot = diff }; document.getElementById("deleteCalibration").onclick = delCalibration @@ -176,9 +188,9 @@ function setIndicatorsTcp(sensordata) { if (sensordata.Orientation[0] !== 0 && sensordata.Orientation[1] !== 0) { //attitudeTcp.setPitch(sensordata.Orientation[0]) - attitudeTcp.setPitch(-q.x * 180 / Math.PI * -1) + attitudeTcp.setPitch(q.x * 180 / Math.PI) //attitudeTcp.setRoll(sensordata.Orientation[1]) - attitudeTcp.setRoll(-q.z * 180 / Math.PI * -1) + attitudeTcp.setRoll(q.z * 180 / Math.PI) } // Heading from quaternion @@ -197,8 +209,8 @@ function setIndicatorsSer(sensordata) { let q = new THREE.Euler().setFromQuaternion( cube.quaternion, 'YXZ' ) if (sensordata.Orientation[0] !== 0 && sensordata.Orientation[1] !== 0) { - attitudeSer.setPitch(-sensordata.Orientation[0]) - attitudeSer.setRoll(-sensordata.Orientation[1]) + attitudeSer.setPitch(sensordata.Orientation[0]) + attitudeSer.setRoll(sensordata.Orientation[1]) } // Heading from quaternion diff --git a/static/scripts/websocket.js b/static/scripts/websocket.js index add721d..9460ec6 100644 --- a/static/scripts/websocket.js +++ b/static/scripts/websocket.js @@ -30,17 +30,17 @@ window.addEventListener("load", function(evt) { } const wsOnMessageF = function (evt) { let dat = JSON.parse(evt.data) - //console.log(dat) + // console.log(dat) if ('SOURCE_TCP' in dat) { setIndicatorsTcp(dat.SOURCE_TCP) } try{ if(!(dat.SOURCE_TCP.Orientation[0] === 0) && !(dat.SOURCE_TCP.Orientation[1] === 0) && !(dat.SOURCE_TCP.Orientation[2] === 0)){ - let heading = (dat.SOURCE_TCP.Orientation[2]+90)%360 // dat.SOURCE_TCP.Orientation[2]// + let heading = (dat.SOURCE_TCP.Orientation[2]+90)%360 // dat.SOURCE_TCP.Orientation[2] // // if (dat.SOURCE_TCP.HeadDevice !== 0) { heading = dat.SOURCE_TCP.HeadDevice; } // if (dat.SOURCE_TCP.HeadMotion !== 0) { heading = dat.SOURCE_TCP.HeadMotion; console.log("head motion") } - renderTCP(dat.SOURCE_TCP.Orientation[0]*Math.PI/180,-dat.SOURCE_TCP.Orientation[1]*Math.PI/180,heading*Math.PI/180) + renderTCP((dat.SOURCE_TCP.Orientation[0]*Math.PI/180),heading*Math.PI/180,-(dat.SOURCE_TCP.Orientation[1]*Math.PI/180)) } if(!(dat.SOURCE_TCP.Position[1] === 0) && !(dat.SOURCE_TCP.Position[0] === 0)){ document.getElementById("TCPlong").innerHTML = "Smartphone long: " + dat.SOURCE_TCP.Position[1] @@ -70,7 +70,7 @@ window.addEventListener("load", function(evt) { if(!(dat.SOURCE_SERIAL.Orientation[0] === 0) && /*!(dat.SOURCE_SERIAL.Orientation[1] === 0) &&*/ !(dat.SOURCE_SERIAL.Orientation[2] === 0)){ // document.getElementById("gyroscopeSERIAL").style.transform = `rotateX(${dat.SOURCE_SERIAL.Orientation[0]}deg) rotateY(${dat.SOURCE_SERIAL.Orientation[1]}deg) rotateZ(0deg)` // document.getElementById("gyroscopeSERIAL").style.transform = `rotateX(${dat.SOURCE_SERIAL.Orientation[0]}deg) rotateY(${dat.SOURCE_SERIAL.Orientation[1]}deg) rotateZ(${dat.SOURCE_SERIAL.Orientation[2]}deg)` - renderSerial(dat.SOURCE_SERIAL.Orientation[0]*Math.PI/180,dat.SOURCE_SERIAL.Orientation[1]*Math.PI/180,-dat.SOURCE_SERIAL.Orientation[2]*Math.PI/180) + renderSerial(dat.SOURCE_SERIAL.Orientation[0]*Math.PI/180,-dat.SOURCE_SERIAL.Orientation[2]*Math.PI/180,dat.SOURCE_SERIAL.Orientation[1]*Math.PI/180) } if(!(dat.SOURCE_SERIAL.Position[1] === 0) && !(dat.SOURCE_SERIAL.Position[0] === 0)){ document.getElementById("SERIALlong").innerHTML = "Ublox long: " + dat.SOURCE_SERIAL.Position[1] diff --git a/templates/index.html b/templates/index.html index c8cd8af..12d5af6 100644 --- a/templates/index.html +++ b/templates/index.html @@ -182,11 +182,11 @@
Manuelle Kalibrierung