You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
102 lines
2.8 KiB
102 lines
2.8 KiB
3 years ago
|
using System;
|
||
|
|
using System.Collections;
|
||
|
|
using System.Collections.Generic;
|
||
|
|
using System.IO.Ports;
|
||
|
|
using System.Threading;
|
||
|
|
using UnityEngine;
|
||
|
|
using LitJson;
|
||
|
|
|
||
|
|
public class MyUnitySerialPort : MonoBehaviour {
|
||
|
|
private SerialPort serialPort;
|
||
|
|
private Thread readPortData;
|
||
|
|
//public List<byte> listReceive = new List<byte>();
|
||
|
|
public string jsonData = "";
|
||
|
|
public Transform MpuBoardTrans;
|
||
|
|
|
||
|
|
|
||
|
|
// Use this for initialization
|
||
|
|
void Start () {
|
||
|
|
//Application.targetFrameRate = 120;
|
||
|
|
try
|
||
|
|
{
|
||
|
|
serialPort = new SerialPort("COM4", 115200);
|
||
|
|
serialPort.ReadTimeout = 500;
|
||
|
|
if (!serialPort.IsOpen)
|
||
|
|
{
|
||
|
|
serialPort.Open();
|
||
|
|
}
|
||
|
|
readPortData = new Thread(new ThreadStart(readData));
|
||
|
|
readPortData.IsBackground = true;
|
||
|
|
readPortData.Start();
|
||
|
|
}
|
||
|
|
catch (Exception)
|
||
|
|
{
|
||
|
|
throw;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Update is called once per frame
|
||
|
|
void Update () {
|
||
|
|
if (jsonData != "") {
|
||
|
|
MPUQuaternion mpudata_ = JsonMapper.ToObject<MPUQuaternion>(jsonData);
|
||
|
|
MpuBoardTrans.transform.localEulerAngles = TransQuaternionToEularAngles((float)mpudata_.q0, (float)mpudata_.q1, (float)mpudata_.q2, (float)mpudata_.q3);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
//结束后关闭串口
|
||
|
|
private void OnDestroy()
|
||
|
|
{
|
||
|
|
if (serialPort.IsOpen)
|
||
|
|
serialPort.Close();
|
||
|
|
readPortData.Abort();
|
||
|
|
}
|
||
|
|
|
||
|
|
//弧度转角度
|
||
|
|
private float TransRadToAngle(double rad)
|
||
|
|
{
|
||
|
|
//return -(float)rad * 180 / Mathf.PI;
|
||
|
|
return (float)rad * 90;
|
||
|
|
}
|
||
|
|
//Double转Float
|
||
|
|
private float TransDoubleToFloat(double angle)
|
||
|
|
{
|
||
|
|
//return -(float)rad * 180 / Mathf.PI;
|
||
|
|
return (float)angle;
|
||
|
|
}
|
||
|
|
/// <summary>
|
||
|
|
/// 读取数据
|
||
|
|
/// </summary>
|
||
|
|
private void readData()
|
||
|
|
{
|
||
|
|
try
|
||
|
|
{
|
||
|
|
while (serialPort != null && serialPort.IsOpen)
|
||
|
|
{
|
||
|
|
byte addr = Convert.ToByte(serialPort.ReadByte());
|
||
|
|
serialPort.DiscardInBuffer();
|
||
|
|
//listReceive.Add(addr);
|
||
|
|
//print("{"+serialPort.ReadLine());
|
||
|
|
jsonData = "{" + serialPort.ReadLine();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
catch (Exception)
|
||
|
|
{
|
||
|
|
throw;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
//将四元数转换为欧拉角(pitch,yaw取负数的情况)
|
||
|
|
public Vector3 TransQuaternionToEularAngles(float q0, float q1, float q2, float q3)
|
||
|
|
{
|
||
|
|
float pitch = -Mathf.Asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3f;
|
||
|
|
float roll = Mathf.Atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3f;
|
||
|
|
float yaw = -Mathf.Atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3f;
|
||
|
|
Vector3 eularAngles = new Vector3(pitch, yaw, roll);
|
||
|
|
return eularAngles;
|
||
|
|
}
|
||
|
|
}
|