init commit

src/render/views/BDSatellite3DPostureControl/SatelliteModelView/index.vue

@@ -0,0 +1,218 @@
+<template>
+  <div ref="satelliteRef" id="satellite" />
+</template>
+
+<script setup lang="ts">
+import { ref, onMounted } from 'vue'
+import * as THREE from 'three'
+
+import { GLTFLoader } from 'three/addons/loaders/GLTFLoader.js'
+import { RGBELoader } from 'three/addons/loaders/RGBELoader.js'
+
+import { useSerialPortStore, SerialPortEventType } from '@/stores/serialPort'
+import { LineBreakParser } from '@/utils/parser'
+
+const accRate = 16384.0
+const gyrRate = 131.072
+
+const satelliteRef = ref(null)
+
+let renderer  = null
+let scene     = null
+let camera    = null
+let satellite = null
+const initSatellite = async () => {
+  // 创建场景对象Scene
+  scene = new THREE.Scene()
+
+  // 添加相机
+  const w = satelliteRef.value.clientWidth, h = satelliteRef.value.clientHeight
+  const aspect = w / h
+  const frustumSize = 15
+
+  camera = new THREE.OrthographicCamera(
+    frustumSize * aspect / -2, frustumSize * aspect / 2,
+    frustumSize / 2, frustumSize / -2)
+  camera.position.z = 100
+  scene.add(camera)
+
+
+  // 添加renderer
+  renderer = new THREE.WebGLRenderer()
+  renderer.setPixelRatio(window.devicePixelRatio)
+  renderer.setSize(w, h)
+  
+
+  // 加载卫星模型
+  const modelDirPath = '../models/geo/'
+
+  const rgbeLoader    = new RGBELoader().setPath(modelDirPath)
+  const environment   = await rgbeLoader.loadAsync('env.hdr')
+  environment.mapping = THREE.EquirectangularReflectionMapping
+  scene.environment   = environment
+
+  const gltfLoader = new GLTFLoader().setPath(modelDirPath)
+  const geoModel   = await gltfLoader.loadAsync('geo.gltf')
+  geoModel.scene.traverse( function ( child ) {
+    if ( child.isMesh ) {
+      child.material.emissive    = child.material.color
+      child.material.emissiveMap = child.material.map 
+    }
+  })
+  satellite = geoModel.scene
+
+
+  scene.add(satellite)
+  camera.lookAt(satellite.position)
+  
+  satelliteRef.value.appendChild(renderer.domElement)
+}
+
+let acc = new Array(3), gyr = new Array(3)
+let firstSample = true
+let lastTime = 0
+let RwGyro = [0, 0, 0]  // 重新读取陀螺仪
+let Awz    = [0, 0]     // XZ/ YZ平面和Z轴（度）R的投影之间的角度
+let RwEst  = [0, 0, 0]
+
+const wGyro = 10.0
+const HALF_PI = Math.PI / 2
+
+const render = () => {
+  if (acc && gyr) {
+    updateRwExt(acc, gyr)
+  }
+  
+  if (satellite) {
+    satellite.rotation.x = HALF_PI * -RwEst[0]
+    satellite.rotation.z = HALF_PI * RwEst[1]
+    // satellite.rotation.y = HALF_PI * RwEst[2]
+  }
+  renderer?.render(scene, camera)
+}
+
+const updateRwExt = (acc, gyr) => {
+  normalize3DVec(acc)
+  
+  let currentTime = Date.now()
+  let interval    = currentTime - lastTime
+  lastTime        = currentTime
+
+  if (firstSample || isNaN(RwEst[0])) {
+    for (let index = 0; index < 3; index++) {
+      RwEst[index] = acc[index]// 初始化加速度传感器读数
+    }
+  } else {
+    // 对RwGyro进行评估
+    if (Math.abs(RwEst[2]) < 0.1) {
+      // Rz值非常的小，它的作用是作为Axz与Ayz的计算参照值，防止放大的波动产生错误的结果。
+      // 这种情况下就跳过当前的陀螺仪数据，使用以前的。
+      for (let index = 0; index < 3; index++) {
+        RwGyro[index] = RwEst[index]
+      }
+    } else {
+      // ZX/ZY平面和Z轴R的投影之间的角度，基于最近一次的RwEst值
+      for (let index = 0; index < 2; index++) {
+        let tmpf = gyr[index] // 获取当前陀螺仪的deg/s
+        tmpf *= interval / 1000.0 // 得到角度变化值
+        Awz[index] = Math.atan2(RwEst[index], RwEst[2]) * 180 / Math.PI // 得到角度并转换为度
+        Awz[index] += tmpf // 根据陀螺仪的运动得到更新后的角度
+      }
+
+      // 判断RzGyro是多少，主要看Axz的弧度是多少
+      // 当Axz在-90 ..90 => cos(Awz) >= 0这个范围内的时候RzGyro是准确的
+      let signRzGyro = (Math.cos(Awz[0] * Math.PI / 180) >= 0) ? 1 : -1
+
+      // 从Awz的角度值反向计算RwGyro的公式请查看网页 http://starlino.com/imu_guide.html
+      for (let index = 0; index < 2; index++) {
+        RwGyro[0] = Math.sin(Awz[0] * Math.PI / 180)
+        RwGyro[0] /= Math.sqrt(1 + squared(Math.cos(Awz[0] * Math.PI / 180)) * squared(Math.tan(Awz[1] * Math.PI / 180)))
+        RwGyro[1] = Math.sin(Awz[1] * Math.PI / 180)
+        RwGyro[1] /= Math.sqrt(1 + squared(Math.cos(Awz[1] * Math.PI / 180)) * squared(Math.tan(Awz[0] * Math.PI / 180)))
+      }
+      RwGyro[2] = signRzGyro * Math.sqrt(1 - squared(RwGyro[0]) - squared(RwGyro[1]))
+
+      // 把陀螺仪与加速度传感器的值进行结合
+      for (let index = 0; index < 3; index++) {
+        RwEst[index] = (acc[index] + wGyro * RwGyro[index]) / (1 + wGyro)
+      }
+
+      normalize3DVec(RwEst)
+    }
+  }
+}
+
+const normalize3DVec = (vector) => {
+  const R = Math.sqrt(squared(vector[0]) + squared(vector[1]) + squared(vector[2]))
+  vector[0] /= R
+  vector[1] /= R
+  vector[2] /= R
+}
+
+const squared = x => Math.pow(x, 2)
+
+const animate = () => {
+  render()
+  requestAnimationFrame(animate)
+}
+
+// 判断字符串是否满足协议格式
+const isValidFormat = data => {
+  if (!data) {
+    return false
+  }
+
+  if (data.indexOf('@POSE') < 0) {
+    return false
+  }
+
+  const splitIndex = data.indexOf('*') + 1
+  const dataStr = data.substring(0, splitIndex)
+  const csStr = data.substring(splitIndex)
+
+  if (!dataStr || !csStr) {
+    return false
+  }
+
+  if (dataStr.length !== parseInt(csStr.trim())) {
+    return false
+  }
+
+  return true
+}
+
+// 将data转换成加速度和角速度
+const data2AccGyr = data => {
+  const startIndex = data.indexOf(',') + 1
+  const endIndex   = data.indexOf('*')
+  const dataArr    = data.substring(startIndex, endIndex).split(',').map(parseFloat)
+
+  acc = new Array(3), gyr = new Array(3)
+
+  acc[0] = dataArr[0] / accRate
+  acc[1] = dataArr[1] / accRate
+  acc[2] = dataArr[2] / accRate
+  gyr[0] = dataArr[3] / gyrRate
+  gyr[1] = dataArr[4] / gyrRate
+  gyr[2] = dataArr[5] / gyrRate
+}
+
+const decoder    = new TextDecoder()
+const parser     = new LineBreakParser()
+const onSerialPortData = (data: Uint8Array) => {
+  const lineArr = parser.parse(decoder.decode(data))
+  lineArr.forEach(line => {
+    if (!isValidFormat(line)) return
+
+    data2AccGyr(line)
+  })
+}
+
+const serialPort = useSerialPortStore()
+serialPort.addEventListener(SerialPortEventType.data, onSerialPortData)
+
+onMounted(() => {
+  initSatellite()
+  animate()
+})
+</script>