bdqh-course-platform-master/src/render/views/BDSatellite3DPostureControl/SatelliteModelView/index.vue

<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>