cesium: fix missing segments, use std for range plotting

ArduPilot · Aug 26, 2023 · bb1c807 · bb1c807
1 parent 2539f71
commit bb1c807
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 24 deletions.
diff --git a/src/components/CesiumViewer.vue b/src/components/CesiumViewer.vue
@@ -867,14 +867,16 @@ export default {
             }
 
             for (const pos of this.points.slice(first, last)) {
-                this.position = Cartesian3.fromDegrees(
+                const position = Cartesian3.fromDegrees(
                     pos[0],
                     pos[1],
                     isBoat ? 0.1 : pos[2] + this.heightOffset
                 )
 
                 const newColor = this.getModeColor(pos[3])
+
                 if (!Color.equals(newColor, currentColor)) {
+                    currentSegment.push(position)
                     if (currentSegment.length > 1) {
                         geometryInstances.push(new GeometryInstance({
                             geometry: new PolylineGeometry({
@@ -885,18 +887,13 @@ export default {
                                 color: ColorGeometryInstanceAttribute.fromColor(currentColor)
                             }
                         }))
-                    } else if (geometryInstances.length > 0) {
-                        const lastSegment = geometryInstances[geometryInstances.length - 1].geometry.positions
-                        if (lastSegment) {
-                            lastSegment.push(this.position)
-                            currentSegment.push(this.position)
-                        }
                     }
 
                     currentColor = newColor
-                    currentSegment = []
+                    currentSegment = [position]
+                } else {
+                    currentSegment.push(position)
                 }
-                currentSegment.push(this.position)
             }
 
             if (currentSegment.length > 1) {
@@ -909,11 +906,6 @@ export default {
                         color: ColorGeometryInstanceAttribute.fromColor(currentColor)
                     }
                 }))
-            } else if (geometryInstances.length > 0) {
-                const lastSegment = geometryInstances[geometryInstances.length - 1].geometry.positions
-                if (lastSegment) {
-                    lastSegment.push(this.position)
-                }
             }
 
             // Remove old trajectory primitives

diff --git a/src/components/cesiumExtra/colorCoderRange.js b/src/components/cesiumExtra/colorCoderRange.js
@@ -4,36 +4,57 @@ export default class ColorCoderRange {
   requiredMessages = ['RFND']
   lastindex = 0 // lets track this so we are more efficient
   maxDist = 3
+  stats = null
   constructor (state) {
       this.state = state
   }
 
+  computeStats (arr) {
+      const n = arr.length
+      if (n === 0) return { min: null, max: null, avg: null, std: null }
+
+      let min = Infinity
+      let max = -Infinity
+      let sum = 0
+      let sumOfSquares = 0
+
+      for (let i = 0; i < n; i++) {
+          const value = arr[i]
+          if (value < min) min = value
+          if (value > max) max = value
+          sum += value
+          sumOfSquares += value * value
+      }
+
+      const avg = sum / n
+      // Calculate variance as: (sumOfSquares/n) - avg^2
+      // Then, standard deviation is the square root of variance
+      const std = Math.sqrt((sumOfSquares / n) - avg * avg)
+
+      return { min, max, avg, std }
+  }
+
   getLegend () {
       const legend = [
           {
               name: 'shallow',
-              color: 'rgb(0, 0, 255)'
+              color: 'rgb(255, 0, 0)'
           },
           {
               name: 'deep',
-              color: 'rgb(255, 0, 0)'
+              color: 'rgb(0, 0, 255)'
           }
       ]
       return legend
   }
 
   getMax () {
-      this.maxDist = 0
-      for (const dist of this.state.messages.RFND.Dist) {
-          if (dist > this.maxDist) {
-              this.maxDist = dist
-          }
-      }
+      return this.stats.avg + this.stats.std * 2
   }
 
   getColor (time) {
-      if (this.maxDist === 0) {
-          this.getMax()
+      if (!this.stats) {
+          this.stats = this.computeStats(this.state.messages.RFND.Dist)
       }
       if (this.state.messages.RFND.time_boot_ms[this.lastindex] - time > 1000) {
           this.lastindex = 0