From 6cda14bc32de66c86bfa84043161538003ef183a Mon Sep 17 00:00:00 2001 From: Timo Volkmann Date: Fri, 15 Jan 2021 12:18:09 +0100 Subject: [PATCH] comments and fixes --- static/scripts/accChart.js | 86 ++-------------------- static/scripts/distanceCalc.js | 78 ++++++++++++++++++++ static/scripts/{cubes.js => indicators.js} | 10 +-- static/scripts/websocket.js | 45 ++++++----- templates/index.html | 19 +++-- templates/replayFull.html | 1 + ublox/messages.go | 6 -- 7 files changed, 123 insertions(+), 122 deletions(-) create mode 100644 static/scripts/distanceCalc.js rename static/scripts/{cubes.js => indicators.js} (96%) diff --git a/static/scripts/accChart.js b/static/scripts/accChart.js index a5d5fbd..b32b9c4 100644 --- a/static/scripts/accChart.js +++ b/static/scripts/accChart.js @@ -87,8 +87,13 @@ function addDistances(data){ // return distVincenty(el.ser.Position, el.tcp.Position) const plaindist = distVincenty(el.ser.Position, el.tcp.Position) arr[i]['distance'] = plaindist + + // if closest measurements not happening in the exact same millisecond, + // calculate traveled distance for the time difference and substract it from result + // bear in mind that this is not exact because the two measurements are not always exactly aligned with + // the direction of motion. that's okay because we only need to estimate the accuracy arr[i]['distanceCleanAbs'] = plaindist - Math.abs(el.ser.Speed / 1000 * el.differenceMs) - return plaindist - Math.abs(el.ser.Speed / 1000 * el.differenceMs) + return arr[i].distanceCleanAbs // plaindist - Math.abs(el.ser.Speed / 1000 * el.differenceMs) }) let tcpTimes = data.map(el => { @@ -102,82 +107,3 @@ function addDistances(data){ accChart.data.datasets[2].data = distances accChart.update() } - -/** - * Function to calculate distance between two coordinates using the haversine algorithm (less precise). - * @param coord1 first set of coordinates. - * @param coord2 second set of coordinates. - * @returns {number} distance between the two points in meters. - */ -function distanceInMetersBetweenEarthCoordinates(coord1, coord2) { - let long1 = coord1[0] - let lat1 = coord1[1] - let long2 = coord2[0] - let lat2 = coord2[1] - - let earthRadiusM = 6371000 - - let phi1 = lat1 * Math.PI / 180 - let phi2 = lat2 * Math.PI / 180 - - let dlat = (lat2-lat1) * Math.PI / 180 - let dlong = (long2 - long1) * Math.PI / 180 - - let a = Math.sin(dlat/2) * Math.sin(dlat/2) + - Math.cos(phi1) * Math.cos(phi2) * - Math.sin(dlong/2) * Math.sin(dlong/2) - let c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a)) - return earthRadiusM * c -} - - -Number.prototype.toRad = function () { return this * Math.PI / 180; } - -/** - * Function to calculate distance between two coordinates using the vincenty algorithm (more precise). - * @param coord1 first set of coordinates. - * @param coord2 second set of coordinates. - * @returns {string|number} distance between the two point is meters. - */ -function distVincenty(coord1, coord2) { - const lon1 = coord1[0] - const lat1 = coord1[1] - const lon2 = coord2[0] - const lat2 = coord2[1] - var a = 6378137, b = 6356752.314245, f = 1/298.257223563; // WGS-84 ellipsoid params - var L = (lon2-lon1).toRad() - var U1 = Math.atan((1-f) * Math.tan(lat1.toRad())); - var U2 = Math.atan((1-f) * Math.tan(lat2.toRad())); - var sinU1 = Math.sin(U1), cosU1 = Math.cos(U1); - var sinU2 = Math.sin(U2), cosU2 = Math.cos(U2); - - var lambda = L, lambdaP, iterLimit = 100; - do { - var sinLambda = Math.sin(lambda), cosLambda = Math.cos(lambda); - var sinSigma = Math.sqrt((cosU2*sinLambda) * (cosU2*sinLambda) + - (cosU1*sinU2-sinU1*cosU2*cosLambda) * (cosU1*sinU2-sinU1*cosU2*cosLambda)); - if (sinSigma===0) return 0; // co-incident points - var cosSigma = sinU1*sinU2 + cosU1*cosU2*cosLambda; - var sigma = Math.atan2(sinSigma, cosSigma); - var sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma; - var cosSqAlpha = 1 - sinAlpha*sinAlpha; - var cos2SigmaM = cosSigma - 2*sinU1*sinU2/cosSqAlpha; - if (isNaN(cos2SigmaM)) cos2SigmaM = 0; // equatorial line: cosSqAlpha=0 (§6) - var C = f/16*cosSqAlpha*(4+f*(4-3*cosSqAlpha)); - lambdaP = lambda; - lambda = L + (1-C) * f * sinAlpha * - (sigma + C*sinSigma*(cos2SigmaM+C*cosSigma*(-1+2*cos2SigmaM*cos2SigmaM))); - } while (Math.abs(lambda-lambdaP) > 1e-12 && --iterLimit>0); - - if (iterLimit===0) return NaN // formula failed to converge - - var uSq = cosSqAlpha * (a*a - b*b) / (b*b); - var A = 1 + uSq/16384*(4096+uSq*(-768+uSq*(320-175*uSq))); - var B = uSq/1024 * (256+uSq*(-128+uSq*(74-47*uSq))); - var deltaSigma = B*sinSigma*(cos2SigmaM+B/4*(cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)- - B/6*cos2SigmaM*(-3+4*sinSigma*sinSigma)*(-3+4*cos2SigmaM*cos2SigmaM))); - var s = b*A*(sigma-deltaSigma); - - s = s.toFixed(3); // round to 1mm precision - return s; -} \ No newline at end of file diff --git a/static/scripts/distanceCalc.js b/static/scripts/distanceCalc.js new file mode 100644 index 0000000..b08607b --- /dev/null +++ b/static/scripts/distanceCalc.js @@ -0,0 +1,78 @@ +/** + * Function to calculate distance between two coordinates using the haversine algorithm (less precise). + * @param coord1 first set of coordinates. + * @param coord2 second set of coordinates. + * @returns {number} distance between the two points in meters. + */ +function distanceInMetersBetweenEarthCoordinates(coord1, coord2) { + let long1 = coord1[0] + let lat1 = coord1[1] + let long2 = coord2[0] + let lat2 = coord2[1] + + let earthRadiusM = 6371000 + + let phi1 = lat1 * Math.PI / 180 + let phi2 = lat2 * Math.PI / 180 + + let dlat = (lat2-lat1) * Math.PI / 180 + let dlong = (long2 - long1) * Math.PI / 180 + + let a = Math.sin(dlat/2) * Math.sin(dlat/2) + + Math.cos(phi1) * Math.cos(phi2) * + Math.sin(dlong/2) * Math.sin(dlong/2) + let c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a)) + return earthRadiusM * c +} + + +Number.prototype.toRad = function () { return this * Math.PI / 180; } + +/** + * Function to calculate distance between two coordinates using the vincenty algorithm (more precise). + * @param coord1 first set of coordinates. + * @param coord2 second set of coordinates. + * @returns {string|number} distance between the two point is meters. + */ +function distVincenty(coord1, coord2) { + const lon1 = coord1[0] + const lat1 = coord1[1] + const lon2 = coord2[0] + const lat2 = coord2[1] + var a = 6378137, b = 6356752.314245, f = 1/298.257223563; // WGS-84 ellipsoid params + var L = (lon2-lon1).toRad() + var U1 = Math.atan((1-f) * Math.tan(lat1.toRad())); + var U2 = Math.atan((1-f) * Math.tan(lat2.toRad())); + var sinU1 = Math.sin(U1), cosU1 = Math.cos(U1); + var sinU2 = Math.sin(U2), cosU2 = Math.cos(U2); + + var lambda = L, lambdaP, iterLimit = 100; + do { + var sinLambda = Math.sin(lambda), cosLambda = Math.cos(lambda); + var sinSigma = Math.sqrt((cosU2*sinLambda) * (cosU2*sinLambda) + + (cosU1*sinU2-sinU1*cosU2*cosLambda) * (cosU1*sinU2-sinU1*cosU2*cosLambda)); + if (sinSigma===0) return 0; // co-incident points + var cosSigma = sinU1*sinU2 + cosU1*cosU2*cosLambda; + var sigma = Math.atan2(sinSigma, cosSigma); + var sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma; + var cosSqAlpha = 1 - sinAlpha*sinAlpha; + var cos2SigmaM = cosSigma - 2*sinU1*sinU2/cosSqAlpha; + if (isNaN(cos2SigmaM)) cos2SigmaM = 0; // equatorial line: cosSqAlpha=0 (§6) + var C = f/16*cosSqAlpha*(4+f*(4-3*cosSqAlpha)); + lambdaP = lambda; + lambda = L + (1-C) * f * sinAlpha * + (sigma + C*sinSigma*(cos2SigmaM+C*cosSigma*(-1+2*cos2SigmaM*cos2SigmaM))); + } while (Math.abs(lambda-lambdaP) > 1e-12 && --iterLimit>0); + + if (iterLimit===0) return NaN // formula failed to converge + + var uSq = cosSqAlpha * (a*a - b*b) / (b*b); + var A = 1 + uSq/16384*(4096+uSq*(-768+uSq*(320-175*uSq))); + var B = uSq/1024 * (256+uSq*(-128+uSq*(74-47*uSq))); + var deltaSigma = B*sinSigma*(cos2SigmaM+B/4*(cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)- + B/6*cos2SigmaM*(-3+4*sinSigma*sinSigma)*(-3+4*cos2SigmaM*cos2SigmaM))); + var s = b*A*(sigma-deltaSigma); + + s = s.toFixed(3); // round to 1mm precision + return s; +} diff --git a/static/scripts/cubes.js b/static/scripts/indicators.js similarity index 96% rename from static/scripts/cubes.js rename to static/scripts/indicators.js index 77596e4..57b9732 100644 --- a/static/scripts/cubes.js +++ b/static/scripts/indicators.js @@ -54,7 +54,7 @@ cube.position.x = 0 cube2.position.x = 0 // calibration globals -let manCalibration = new THREE.Euler( 0, 0, 0, 'YXZ' ) +let manCalibration = new THREE.Euler( 0, 0, 0, 'XYZ' ) let calibrationRot = new THREE.Quaternion() let calPitch = 0 let calRoll = 0 @@ -100,7 +100,7 @@ rollRange.oninput = () => { function delCalibration(evt) { calibrationRot = new THREE.Quaternion() - manCalibration = new THREE.Euler( 0, 0, 0, 'YXZ' ) + manCalibration = new THREE.Euler( 0, 0, 0, 'XYZ' ) pitchRange.value = 0 yawRange.value = 0 rollRange.value = 0 @@ -122,7 +122,7 @@ function manualCalibration(evt) { calState = !calState console.log("mancal OFF") con.style.display = "none" - manCalibration = new THREE.Euler( 0, 0, 0, 'YXZ' ) + manCalibration = new THREE.Euler( 0, 0, 0, 'XYZ' ) pitchRange.value = 0 yawRange.value = 0 rollRange.value = 0 @@ -147,7 +147,7 @@ document.getElementById("manualCalibration").onclick = manualCalibration document.getElementById("calibrate").onclick = calibrate -// indicators // +// indicators from https://github.com/sebmatton/jQuery-Flight-Indicators.git let options = { size : 200, // Sets the size in pixels of the indicator (square) @@ -202,4 +202,4 @@ function setIndicatorsSer(sensordata) { altimeter.setAltitude((sensordata.Position[2] * 10).toFixed()) altitudeLabel.innerHTML = `HMSL: ${(sensordata.Position[2]).toFixed(2)} m` } -} \ No newline at end of file +} diff --git a/static/scripts/websocket.js b/static/scripts/websocket.js index 51a2c44..cd4898f 100644 --- a/static/scripts/websocket.js +++ b/static/scripts/websocket.js @@ -1,6 +1,5 @@ let tempTCPCoords = null; let tempSERIALCoords = null; -let tempDist = 0; window.addEventListener("load", function(evt) { let output = document.getElementById("output"); @@ -41,7 +40,7 @@ window.addEventListener("load", function(evt) { document.getElementById("TCPlat").innerHTML = "Smartphone lat: " + dat.SOURCE_TCP.Position[0] updateMapTCP(dat.SOURCE_TCP.Position[1], dat.SOURCE_TCP.Position[0]) map.panTo([dat.SOURCE_TCP.Position[1], dat.SOURCE_TCP.Position[0]]) - tempTCPCoords = [dat.SOURCE_TCP.Position[1], dat.SOURCE_TCP.Position[0]] + tempTCPCoords = dat.SOURCE_TCP } if(dat.SOURCE_TCP.Speed !== 0){ addSpeedTcp(dat.SOURCE_TCP.Speed); @@ -69,37 +68,35 @@ window.addEventListener("load", function(evt) { document.getElementById("SERIALlat").innerHTML = "Ublox lat: " + dat.SOURCE_SERIAL.Position[0] updateMapSERIAL(dat.SOURCE_SERIAL.Position[1], dat.SOURCE_SERIAL.Position[0]) map.panTo([dat.SOURCE_SERIAL.Position[1], dat.SOURCE_SERIAL.Position[0]]) - tempSERIALCoords = [dat.SOURCE_SERIAL.Position[1], dat.SOURCE_SERIAL.Position[0]] + tempSERIALCoords = dat.SOURCE_SERIAL if (tempTCPCoords !== null) { - let long1 = tempTCPCoords[0] - let lat1 = tempTCPCoords[1] - let long2 = tempSERIALCoords[0] - let lat2 = tempSERIALCoords[1] - let earthRadiusM = 6371000 + // calculate distance between coordinates from phone and m8u + let tempDist = distVincenty(tempTCPCoords.Position, tempSERIALCoords.Position) + // calculate time difference between currently cached measurements from phone and m8u + let timeDiff = Date.parse(tempTCPCoords.Timestamp) - Date.parse(tempSERIALCoords.Timestamp) + // calculate error estimation: traveled distance in time difference + let distError = Math.abs(dat.SOURCE_SERIAL.Speed / 1000 * timeDiff) + // remove invalid negative values + let distClean = Math.max(tempDist - distError, 0) // set to zero if error greater than diff as this is only an estimation - let phi1 = lat1 * Math.PI / 180 - let phi2 = lat2 * Math.PI / 180 + addDistanceToBarChart(distClean) + document.getElementById("distance").innerHTML = "Distance 2D: " + distClean.toFixed(3) + " m" - let dlat = (lat2-lat1) * Math.PI / 180 - let dlong = (long2 - long1) * Math.PI / 180 - - let a = Math.sin(dlat/2) * Math.sin(dlat/2) + - Math.cos(phi1) * Math.cos(phi2) * - Math.sin(dlong/2) * Math.sin(dlong/2) - let c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a)) - tempDist = (earthRadiusM * c).toFixed(3) - - addDistanceToBarChart(tempDist) - document.getElementById("distance").innerHTML = "Distance: " + tempDist - - if(tempDist <= dat.SOURCE_SERIAL.HAcc){ + if(distClean <= dat.SOURCE_SERIAL.HAcc){ document.getElementById("greenlamp").style.backgroundColor = 'rgba(0, 230, 64, 1)' + document.getElementById("yellow").style.backgroundColor = 'rgb(157,117,25)' + document.getElementById("redlamp").style.backgroundColor = 'rgba(139, 0, 0, 1)' + } + else if(distClean <= tempTCPCoords.HAcc){ + document.getElementById("greenlamp").style.backgroundColor = 'rgba(0, 100, 0, 1)' + document.getElementById("yellow").style.backgroundColor = 'rgb(255,199,66)' document.getElementById("redlamp").style.backgroundColor = 'rgba(139, 0, 0, 1)' } else{ document.getElementById("greenlamp").style.backgroundColor = 'rgba(0, 100, 0, 1)' - document.getElementById("redlamp").style.backgroundColor = 'rgba(242, 38, 19, 1)' + document.getElementById("yellow").style.backgroundColor = 'rgb(157,117,25)' + document.getElementById("redlamp").style.backgroundColor = 'rgb(255,14,14)' } } diff --git a/templates/index.html b/templates/index.html index fd87af9..cbcbf19 100644 --- a/templates/index.html +++ b/templates/index.html @@ -7,6 +7,7 @@ + @@ -87,10 +88,13 @@
+
+
+
- + @@ -99,13 +103,17 @@
-
+

+ HAcc:
+ +

-
@@ -166,7 +171,7 @@ style="margin: 10px 0px"> - + diff --git a/templates/replayFull.html b/templates/replayFull.html index fd230e8..0e49541 100644 --- a/templates/replayFull.html +++ b/templates/replayFull.html @@ -6,6 +6,7 @@ + diff --git a/ublox/messages.go b/ublox/messages.go index 610f7b2..08830f5 100644 --- a/ublox/messages.go +++ b/ublox/messages.go @@ -4,12 +4,6 @@ type Message interface { ClassID() uint16 } -//type UbxMessage interface { -// Timestamp() (time.Time, error) -// Position() ([3]float64, error) -// Orientation() ([3]float64, error) -//} - type RawMessage struct { classID uint16 Data []byte