ESP32獲取WT901C-TTL/232四元數

阿新 • • 發佈：2021-12-09

文章《ESP32讀取串列埠感測器資料》介紹瞭如何獲取WT901C-TTL/232串列埠資料，但官方提供的JY901庫沒有獲取四元數的功能。我修改了一下JY901庫：

JY901.h程式碼：

#ifndef JY901_h
#define JY901_h

#define SAVE 			0x00
#define CALSW 		0x01
#define RSW 			0x02
#define RRATE			0x03
#define BAUD 			0x04
#define AXOFFSET	0x05
#define AYOFFSET	0x06
#define AZOFFSET	0x07
#define GXOFFSET	0x08
#define GYOFFSET	0x09
#define GZOFFSET	0x0a
#define HXOFFSET	0x0b
#define HYOFFSET	0x0c
#define HZOFFSET	0x0d
#define D0MODE		0x0e
#define D1MODE		0x0f
#define D2MODE		0x10
#define D3MODE		0x11
#define D0PWMH		0x12
#define D1PWMH		0x13
#define D2PWMH		0x14
#define D3PWMH		0x15
#define D0PWMT		0x16
#define D1PWMT		0x17
#define D2PWMT		0x18
#define D3PWMT		0x19
#define IICADDR		0x1a
#define LEDOFF 		0x1b
#define GPSBAUD		0x1c

#define YYMM				0x30
#define DDHH				0x31
#define MMSS				0x32
#define MS					0x33
#define AX					0x34
#define AY					0x35
#define AZ					0x36
#define GX					0x37
#define GY					0x38
#define GZ					0x39
#define HX					0x3a
#define HY					0x3b
#define HZ					0x3c			
#define Roll				0x3d
#define Pitch				0x3e
#define Yaw					0x3f
#define TEMP				0x40
#define D0Status		0x41
#define D1Status		0x42
#define D2Status		0x43
#define D3Status		0x44
#define PressureL		0x45
#define PressureH		0x46
#define HeightL			0x47
#define HeightH			0x48
#define LonL				0x49
#define LonH				0x4a
#define LatL				0x4b
#define LatH				0x4c
#define GPSHeight   0x4d
#define GPSYAW      0x4e
#define GPSVL				0x4f
#define GPSVH				0x50

#define DIO_MODE_AIN 0
#define DIO_MODE_DIN 1
#define DIO_MODE_DOH 2
#define DIO_MODE_DOL 3
#define DIO_MODE_DOPWM 4
#define DIO_MODE_GPS 5		

struct STime
{
	unsigned char ucYear;
	unsigned char ucMonth;
	unsigned char ucDay;
	unsigned char ucHour;
	unsigned char ucMinute;
	unsigned char ucSecond;
	unsigned short usMiliSecond;
};
struct SAcc
{
	short a[3];
	short T;
};
struct SGyro
{
	short w[3];
	short T;
};
struct SAngle
{
	short Angle[3];
	short T;
};
struct SMag
{
	short h[3];
	short T;
};

struct SDStatus
{
	short sDStatus[4];
};

struct SPress
{
	long lPressure;
	long lAltitude;
};

struct SLonLat
{
	long lLon;
	long lLat;
};

struct SGPSV
{
	short sGPSHeight;
	short sGPSYaw;
	long lGPSVelocity;
};
struct SQ
{ short q[4];
}; 

struct SSN
{
	short sSVNum;
	short sPDOP;
	short sHDOP;
	short sVDOP;
};
class CJY901 
{
  public: 
	struct STime		stcTime;
	struct SAcc 		stcAcc;
	struct SGyro 		stcGyro;
	struct SAngle 		stcAngle;
	struct SMag 		stcMag;
	struct SDStatus 	stcDStatus;
	struct SPress 		stcPress;
	struct SLonLat 		stcLonLat;
	struct SGPSV 		stcGPSV;
	struct SQ       　　　　 stcQ; 

	struct SSN 		stcSN;
	
    CJY901 (); 
	void StartIIC();
	void StartIIC(unsigned char ucAddr);
    void CopeSerialData(unsigned char ucData);
	short ReadWord(unsigned char ucAddr);
	void WriteWord(unsigned char ucAddr,short sData);
	void ReadData(unsigned char ucAddr,unsigned char ucLength,char chrData[]);
	void GetTime();
	void GetAcc();
	void GetGyro();
	void GetAngle();
	void GetMag();
	void GetPress();
	void GetDStatus();
	void GetLonLat();
	void GetGPSV();

  private: 
	unsigned char ucDevAddr; 
	void readRegisters(unsigned char deviceAddr,unsigned char addressToRead, unsigned char bytesToRead, char * dest);
	void writeRegister(unsigned char deviceAddr,unsigned char addressToWrite,unsigned char bytesToRead, char *dataToWrite);
};
extern CJY901 JY901;
#include <Wire.h>
#endif

JY901.cpp程式碼：

#include "JY901.h"
#include "string.h"

CJY901 ::CJY901 ()
{
	ucDevAddr =0x50;
}
void CJY901::StartIIC()
{
	ucDevAddr = 0x50;
	Wire.begin();
}
void CJY901::StartIIC(unsigned char ucAddr)
{
	ucDevAddr = ucAddr;
	Wire.begin();
}
void CJY901 ::CopeSerialData(unsigned char ucData)
{
	static unsigned char ucRxBuffer[250];
	static unsigned char ucRxCnt = 0;	
	
	ucRxBuffer[ucRxCnt++]=ucData;
	if (ucRxBuffer[0]!=0x55) 
	{
		ucRxCnt=0;
		return;
	}
	if (ucRxCnt<11) {return;}
	else
	{
		switch(ucRxBuffer[1])
		{
			case 0x50:	memcpy(&stcTime,&ucRxBuffer[2],8);break;
			case 0x51:	memcpy(&stcAcc,&ucRxBuffer[2],8);break;
			case 0x52:	memcpy(&stcGyro,&ucRxBuffer[2],8);break;
			case 0x53:	memcpy(&stcAngle,&ucRxBuffer[2],8);break;
			case 0x54:	memcpy(&stcMag,&ucRxBuffer[2],8);break;
			case 0x55:	memcpy(&stcDStatus,&ucRxBuffer[2],8);break;
			case 0x56:	memcpy(&stcPress,&ucRxBuffer[2],8);break;
			case 0x57:	memcpy(&stcLonLat,&ucRxBuffer[2],8);break;
			case 0x58:	memcpy(&stcGPSV,&ucRxBuffer[2],8);break;
			case 0x59:	memcpy(&stcQ,&ucRxBuffer[2],8);break;
		}
		ucRxCnt=0;
	}
}
void CJY901::readRegisters(unsigned char deviceAddr,unsigned char addressToRead, unsigned char bytesToRead, char * dest)
{
  Wire.beginTransmission(deviceAddr);
  Wire.write(addressToRead);
  Wire.endTransmission(false); //endTransmission but keep the connection active

  Wire.requestFrom(deviceAddr, bytesToRead); //Ask for bytes, once done, bus is released by default

  while(Wire.available() < bytesToRead); //Hang out until we get the # of bytes we expect

  for(int x = 0 ; x < bytesToRead ; x++)
    dest[x] = Wire.read();    
}
void CJY901::writeRegister(unsigned char deviceAddr,unsigned char addressToWrite,unsigned char bytesToRead, char *dataToWrite)
{
  Wire.beginTransmission(deviceAddr);
  Wire.write(addressToWrite);
  for(int i = 0 ; i < bytesToRead ; i++)
  Wire.write(dataToWrite[i]);
  Wire.endTransmission(); //Stop transmitting
}

short CJY901::ReadWord(unsigned char ucAddr)
{
	short sResult;
	readRegisters(ucDevAddr, ucAddr, 2, (char *)&sResult);
	return sResult;
}
void CJY901::WriteWord(unsigned char ucAddr,short sData)
{	
	writeRegister(ucDevAddr, ucAddr, 2, (char *)&sData);
}
void CJY901::ReadData(unsigned char ucAddr,unsigned char ucLength,char chrData[])
{
	readRegisters(ucDevAddr, ucAddr, ucLength, chrData);
}

void CJY901::GetTime()
{
	readRegisters(ucDevAddr, 0x30, 8, (char*)&stcTime);	
}
void CJY901::GetAcc()
{
	readRegisters(ucDevAddr, AX, 6, (char *)&stcAcc);
}
void CJY901::GetGyro()
{
	readRegisters(ucDevAddr, GX, 6, (char *)&stcGyro);
}

void CJY901::GetAngle()
{
	readRegisters(ucDevAddr, Roll, 6, (char *)&stcAngle);
}
void CJY901::GetMag()
{
	readRegisters(ucDevAddr, HX, 6, (char *)&stcMag);
}
void CJY901::GetPress()
{
	readRegisters(ucDevAddr, PressureL, 8, (char *)&stcPress);
}
void CJY901::GetDStatus()
{
	readRegisters(ucDevAddr, D0Status, 8, (char *)&stcDStatus);
}
void CJY901::GetLonLat()
{
	readRegisters(ucDevAddr, LonL, 8, (char *)&stcLonLat);
}
void CJY901::GetGPSV()
{
	readRegisters(ucDevAddr, GPSHeight, 8, (char *)&stcGPSV);
}
CJY901 JY901 = CJY901();

實際上這個庫程式碼很冗餘，關鍵位置紅色標記。

然後在loop中如下呼叫：

Serial.print("加速度Acc:");Serial.print((float)JY901.stcAcc.a[0]/32768*16);Serial.print(" ");Serial.print((float)JY901.stcAcc.a[1]/32768*16);Serial.print(" ");Serial.println((float)JY901.stcAcc.a[2]/32768*16);
  Serial.print("角速度Gyro:");Serial.print((float)JY901.stcGyro.w[0]/32768*2000);Serial.print(" ");Serial.print((float)JY901.stcGyro.w[1]/32768*2000);Serial.print(" ");Serial.println((float)JY901.stcGyro.w[2]/32768*2000);
  Serial.print("角度Angle:");Serial.print((float)JY901.stcAngle.Angle[0]/32768*180);Serial.print(" ");Serial.print((float)JY901.stcAngle.Angle[1]/32768*180);Serial.print(" ");Serial.println((float)JY901.stcAngle.Angle[2]/32768*180);
  Serial.print("磁場Mag:");Serial.print(JY901.stcMag.h[0]);Serial.print(" ");Serial.print(JY901.stcMag.h[1]);Serial.print(" ");Serial.println(JY901.stcMag.h[2]);
  Serial.print("四元數:");Serial.print((float)JY901.stcQ.q[0]/32768);Serial.print(" ");Serial.print((float)JY901.stcQ.q[1]/32768);Serial.print(" ");Serial.print((float)JY901.stcQ.q[2]/32768);Serial.print(" ");Serial.print((float)JY901.stcQ.q[3]/32768);Serial.print(" ");

ESP32獲取WT901C-TTL/232四元數

文章《ESP32讀取串列埠感測器資料》介紹瞭如何獲取WT901C-TTL/232串列埠資料，但官方提供的JY901庫沒有獲取四元數的功能。我修改了一下JY901庫：

python將四元數變換為旋轉矩陣的例項

如下所示： import numpy as np from autolab_core import RigidTransform # 寫上用四元數表示的orientation和xyz表示的position

四元數壓縮

https://blog.codingnow.com/2017/11/quaternion_compress.html https://bitbucket.org/Nabril/unitynetworking/commits/702a387656c6ee54311345a1c29f286767b59a22?at=5.4

three.js尤拉角和四元數的使用方法

前言這篇郭先生就來說說尤拉角和四元數，尤拉角和四元數的優缺點是老生常談的話題了，使用條件我就不多說了，我只說一下使用方法。

Eigen 旋轉矩陣，旋轉向量和四元數的初始化和相互轉換的實現

部落格轉自：https://blog.csdn.net/u011092188/article/details/77430988 Eigen庫是一個開源的C++線性代數庫，它提供了快速的有關矩陣的線性代數運算，還包括解方程等功能。Eigen是一個用純標頭檔案搭建起來的庫，

尤拉角轉換成四元數的

資料結構： struct vec3 { vec3(){} vec3(float x, float y, float z) { mx = x; my = y; mz = z; } float mx;

MATLAB 求向量間的旋轉矩陣與四元數

問題是這樣，如果我們知道兩個向量v1和v2，計算從v1轉到v2的旋轉矩陣和四元數，由於旋轉矩陣和四元數可以互轉，所以我們先計算四元數。

MATLAB 對應點集配準的四元數法

這個算是ICP演算法中的一個關鍵步驟，單獨拿出來看一下。演算法流程如下：

Unity 四元數、尤拉角、軸向角之間的互相轉換

using UnityEngine; public class RotateTest : MonoBehaviour { public Transform a; public Transform b; public Transform c;

unity基礎之常用類(transform,vector3,time,四元數,Maths)

在Unity的工程中,資源層級管理是這樣的,從scene開始,scene裡面有許多遊戲物件,每個遊戲物件都有各種元件,元件可分為系統元件和C# 指令碼元件

python_四元數q轉旋轉矩陣R(已驗證)

技術標籤：python 方法1 使用公式:https://doc.rc-visard.com/latest/de/pose_formats.html?highlight=format

3D數學：尤拉角、萬向鎖、四元數

左手系、右手系尤拉角尤拉角用來在3D世界中表示物體的朝向，通常我們將朝向定義為將某一個正朝向旋轉至當前朝向所進行的變換。當我們表示物體的朝向時，實際上指的是對物體所進行的旋轉變換。

三維旋轉筆記:尤拉角/四元數/旋轉矩陣/軸角-記憶點整理

在看《尤拉角、旋轉矩陣、四元數合輯》，就之前所學做點筆記，以便以後再次複習。

Unity中的四元數

一.簡介: 　　在遊戲中常常需要對物體進行旋轉操作,或者瞭解物體的旋轉朝向,所以需要對物體的旋轉進行描述.對物體的旋轉的描述有以下4種常用方式:

Quaternion Knowledge Graph Embeddings —— 基於四元數的知識圖譜嵌入

一、論文概覽在本篇論文中，作者將基於複數的知識圖譜嵌入拓展到超複數空間——四元數，每個四元數\\(Q\\)由一個實數\\(r\\)和三個虛數單位\\(\\textbf{i}\\)，\\(\\textbf{j}\\)，\\(\\textbf{k}\\)組成，即\\(Q=

【ABB】機械臂尤拉角轉四元數

背景我們最近搞機械臂+機器視覺實現機械臂的引導、定位和抓取操作。我們的機械臂是ABB的，ABB的機械臂預設採用四元數的方式表示空間座標，但是這個東西非常不便於理解，其他廠家的機械臂一般使用尤拉角表示空間位

python 尤拉角，旋轉矩陣，四元數之間轉換

import numpy as np import math from scipy.spatial.transform import Rotation as R Rq=[-0.71934025092983234, 1.876085535681999e-06, 3.274841213980097e-08, 0.69465790385533299]

Python獲取二維陣列的行列數的2種方法

這篇文章主要介紹了Python獲取二維陣列的行列數的2種方法,文中通過示例程式碼介紹的非常詳細，對大家的學習或者工作具有一定的參考學習價值,需要的朋友可以參考下

C#獲取本地IP的四種方式示例詳解

1.第一種方式採用System.Net.Dns的GetHostAddress的方式，具體請看程式碼： /// <summary>

獲取類物件的四種方法

對於每一個類在載入時都會有一個唯一的Class物件,Class物件中包含了所有需要動態獲取的類的資訊, 所以Class類是Reflection API 中的核心類。

ESP32獲取WT901C-TTL/232四元數

相關推薦