GeekTrcak采用四元数代替欧拉角传输，更新基于四元数的GeekTrackSDK

2.Firmware/GeekTrack/components/GeekHAL/HAL/HAL_Network.cpp

@@ -45,8 +45,10 @@ void Network_UdpSendData()
     while (1) 
     {
         float yaw = 0.0f, roll = 0.0f, pitch = 0.0f;
+        float quat[4] = {0.0f};
         MPU9250_GetEulerAngles(&yaw, &roll, &pitch);
-		sprintf(buff, "%s %f %f %f\n",ConfigString, yaw, roll, pitch);
+        MPU9250_GetQuaternion(quat);
+		sprintf(buff, "%s %f %f %f %f\n",ConfigString, quat[0],quat[1],quat[2],quat[3]);
         err = sendto(sock, buff, 256, 0, (struct sockaddr *)&saddr, 
                 sizeof(struct sockaddr_in));
         if (err < 0) {

2.Firmware/GeekTrack/components/Libraries/include/mpu9250.h

@@ -185,6 +185,7 @@ void AHRSUpdate(float gx, float gy, float gz,
 
 void MPU9250_GetEulerAngles(float* yaw,float* roll, float* pitch);
 
+void MPU9250_GetQuaternion(float quat[4]);
 
 #ifdef __cplusplus
 }

2.Firmware/GeekTrack/components/Libraries/src/mpu9250.c

@@ -116,6 +116,14 @@ void MPU9250_GetEulerAngles(float* yaw,float* roll, float* pitch)
 	*pitch = mpu9250.pitch; 
 }
 
+void MPU9250_GetQuaternion(float quat[4]){
+	quat[0] = q0;
+	quat[1] = q1;
+	quat[2] = q2;
+	quat[3] = q3;
+}
+
+
 void AHRSUpdate(float gx, float gy, float gz, float ax, float ay, float az) 
 {	
 	float halfT = 0.5 * (10.0f / 1000.0f);

4.Software/GeekTrackSDK/Project/GeekTrack.cpp

@@ -12,11 +12,11 @@
 
 struct GeekTrackNode_t {
 	EulerAngles_t Pose;
+	Quaternion_t  Quat;
 	bool devOnline;
 	GeekTrackNode_t() {
-		Pose.x = 0.0f;
-		Pose.y = 0.0f;
-		Pose.z = 0.0f;
+		memset(&Pose, 0, sizeof(EulerAngles_t));
+		memset(&Quat, 0, sizeof(Quaternion_t));
 		devOnline = false;
 	}
 };
@@ -34,13 +34,31 @@ void GeekTrackUpdate() {
 	int i = 0;
 	while (1)
 	{
+		//printf("...\n");
 		//从广播地址接收消息，注意用来绑定的地址和接收消息的地址是不一样的
 		recvfrom(sock, buf, 256, 0, (struct sockaddr FAR*) &from, (int FAR*) & fromlength);
 		Sleep(10);
-		float yaw = 0.0f, roll = 0.0f, pitch = 0.0f;
+		float q[4] = { 0.0f };
 		int id = 0;
-		int ret = sscanf_s(buf, "%d %f %f %f\n", &id, &yaw, &roll, &pitch);
+		//printf("%s\n", buf);
+		int ret = sscanf_s(buf, "%d %f %f %f %f\n", &id, &q[0], &q[1], &q[2], &q[3]);
 		if (ret > 0 && (id >= 0 && id < MAX_SUPPORT_DEV_NUM)) {
+
+			float yaw = 0.0f, pitch = 0.0f, roll = 0.0f;
+
+			pitch = asin((-2.0 * ((double)q[3]*q[1] - (double)q[0]*q[2]))); // * (180.0f / 3.141592f);							                                            */
+			yaw   = atan2((double)q[2]*q[1] + (double)q[0]*q[3], 0.5 - (double)q[2]*q[2] - (double)q[3]*q[3]); // * (180.0f /3.141592f);
+			roll  = atan2((double)q[2]*q[3] + (double)q[0]*q[1], 0.5 - (double)q[2]*q[2] - (double)q[1]*q[1]); //* (180.0f /3.141592f);
+
+			yaw *= (180.0f / 3.141592f);
+			roll *= (180.0f / 3.141592f);
+			pitch *= (180.0f / 3.141592f);
+
+			TrackNode[id].Quat.w = q[0];
+			TrackNode[id].Quat.x = q[1];
+			TrackNode[id].Quat.y = q[2];
+			TrackNode[id].Quat.z = q[3];
+
 			TrackNode[id].Pose.x = pitch;
 			TrackNode[id].Pose.y = roll;
 			TrackNode[id].Pose.z = yaw;
@@ -55,7 +73,7 @@ void GeekTrack_Init() {
 
 	static bool optval = false;
 	//启动SOCKET库，版本为2.0
-	if (optval == false) {
+	if (optval == true) {
 		return;
 	}
 	WSAStartup(0x0202, &wsdata);
@@ -89,12 +107,20 @@ EulerAngles_t GetEulerAngles(int devId) {
 	return Pose;
 }
 
+Quaternion_t GetQuaternion(int devId) {
+	if (devId >= 0 && devId < MAX_SUPPORT_DEV_NUM) {
+		return TrackNode[devId].Quat;
+	}
+	Quaternion_t Quat = { 0.0f, 0.0f, 0.0f, 0.0f };
+	return Quat;
+}
+
 int main()
 {
 	GeekTrack_Init();
 	int devId = 0;
 	while (1) {
-		Sleep(50);
+		Sleep(30);
 		printf("%d,%f,%f,%f\n", devId,TrackNode[devId].Pose.x, TrackNode[devId].Pose.y,
 			TrackNode[devId].Pose.z);
 	}

4.Software/GeekTrackSDK/Src/GeekTrackSDK.h

@@ -7,10 +7,15 @@ typedef struct {
 	float x, y, z;
 }EulerAngles_t;
 
+typedef struct {
+	float w, x, y, z;
+}Quaternion_t;
 
 __declspec(dllexport) void GeekTrack_Init();
 
 __declspec(dllexport) EulerAngles_t GetEulerAngles(int devId);
+
+__declspec(dllexport) Quaternion_t GetQuaternion(int devId);
 }
 
 #endif

4.Software/TestGeekTrackSDK/TestGeekTrack/Program.cs

@@ -11,20 +11,29 @@ namespace TestGeekTrack
             public float x, y, z;
         };
 
+        public struct GeekQuaternion
+        {
+            public float w, x, y, z;
+        };
+
         [DllImport("GeekTrackSDK.dll", EntryPoint = "GeekTrack_Init", CallingConvention = CallingConvention.Cdecl)]
         public extern static void GeekTrack_Init();
 
         [DllImport("GeekTrackSDK.dll", EntryPoint = "GetEulerAngles", CallingConvention = CallingConvention.Cdecl)]
         public extern static GeekEulerAngles GetEulerAngles(int devId);
 
+        [DllImport("GeekTrackSDK.dll", EntryPoint = "GetQuaternion", CallingConvention = CallingConvention.Cdecl)]
+        public extern static GeekQuaternion GetQuaternion(int devId);
+
         static void Main(string[] args)
         {
             GeekTrack_Init();
             while (true) {
                 GeekEulerAngles angles = GetEulerAngles(0);
+                GeekQuaternion quat = GetQuaternion(0);
                 Thread.Sleep(100);
                 //Debug.Log("<color=#9400D3>" + + "</color>");
-                Console.WriteLine(angles.x + "," + angles.y + "," + angles.z);
+                Console.WriteLine(quat.w + "," + quat.x + "," + quat.y + "," + quat.z);
             }
             
         }

4.Software/TestGeekTrackSDK/TestGeekTrack/obj/TestGeekTrack.csproj.nuget.dgspec.json

@@ -1,17 +1,17 @@
 {
   "format": 1,
   "restore": {
-    "E:\\Hardware Project\\GeekTrack\\4.Software\\TestGeekTrackSDK_V1.0\\TestGeekTrack\\TestGeekTrack.csproj": {}
+    "E:\\Hardware Project\\GeekTrack\\4.Software\\TestGeekTrackSDK\\TestGeekTrack\\TestGeekTrack.csproj": {}
   },
   "projects": {
-    "E:\\Hardware Project\\GeekTrack\\4.Software\\TestGeekTrackSDK_V1.0\\TestGeekTrack\\TestGeekTrack.csproj": {
+    "E:\\Hardware Project\\GeekTrack\\4.Software\\TestGeekTrackSDK\\TestGeekTrack\\TestGeekTrack.csproj": {
       "version": "1.0.0",
       "restore": {
-        "projectUniqueName": "E:\\Hardware Project\\GeekTrack\\4.Software\\TestGeekTrackSDK_V1.0\\TestGeekTrack\\TestGeekTrack.csproj",
+        "projectUniqueName": "E:\\Hardware Project\\GeekTrack\\4.Software\\TestGeekTrackSDK\\TestGeekTrack\\TestGeekTrack.csproj",
         "projectName": "TestGeekTrack",
-        "projectPath": "E:\\Hardware Project\\GeekTrack\\4.Software\\TestGeekTrackSDK_V1.0\\TestGeekTrack\\TestGeekTrack.csproj",
+        "projectPath": "E:\\Hardware Project\\GeekTrack\\4.Software\\TestGeekTrackSDK\\TestGeekTrack\\TestGeekTrack.csproj",
         "packagesPath": "C:\\Users\\Administrator\\.nuget\\packages\\",
-        "outputPath": "E:\\Hardware Project\\GeekTrack\\4.Software\\TestGeekTrackSDK_V1.0\\TestGeekTrack\\obj\\",
+        "outputPath": "E:\\Hardware Project\\GeekTrack\\4.Software\\TestGeekTrackSDK\\TestGeekTrack\\obj\\",
         "projectStyle": "PackageReference",
         "fallbackFolders": [
           "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages",

4.Software/TestGeekTrackSDK/TestGeekTrack/obj/project.assets.json

@@ -15,11 +15,11 @@
   "project": {
     "version": "1.0.0",
     "restore": {
-      "projectUniqueName": "E:\\Hardware Project\\GeekTrack\\4.Software\\TestGeekTrackSDK_V1.0\\TestGeekTrack\\TestGeekTrack.csproj",
+      "projectUniqueName": "E:\\Hardware Project\\GeekTrack\\4.Software\\TestGeekTrackSDK\\TestGeekTrack\\TestGeekTrack.csproj",
       "projectName": "TestGeekTrack",
-      "projectPath": "E:\\Hardware Project\\GeekTrack\\4.Software\\TestGeekTrackSDK_V1.0\\TestGeekTrack\\TestGeekTrack.csproj",
+      "projectPath": "E:\\Hardware Project\\GeekTrack\\4.Software\\TestGeekTrackSDK\\TestGeekTrack\\TestGeekTrack.csproj",
       "packagesPath": "C:\\Users\\Administrator\\.nuget\\packages\\",
-      "outputPath": "E:\\Hardware Project\\GeekTrack\\4.Software\\TestGeekTrackSDK_V1.0\\TestGeekTrack\\obj\\",
+      "outputPath": "E:\\Hardware Project\\GeekTrack\\4.Software\\TestGeekTrackSDK\\TestGeekTrack\\obj\\",
       "projectStyle": "PackageReference",
       "fallbackFolders": [
         "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages",

4.Software/TestGeekTrackSDK/TestGeekTrack/obj/project.nuget.cache

@@ -1,8 +1,8 @@
 {
   "version": 2,
-  "dgSpecHash": "9JxXv/tIWM0ENP1AlM+SwchvhgqL8V/ELkvxXz7e1HCbpr1e0lic0NCd+Exl+hwSZQkLYUjftNhgZYaAKodQGQ==",
+  "dgSpecHash": "6OhT4/s2fKivJ76Rl050qvNGtjBukr/ejkwO8BeIFOyKHlhB9Jggz5KO5hpJvO3UvBnCSAADMzUQvxXBUM7G9A==",
   "success": true,
-  "projectFilePath": "E:\\Hardware Project\\GeekTrack\\4.Software\\TestGeekTrackSDK_V1.0\\TestGeekTrack\\TestGeekTrack.csproj",
+  "projectFilePath": "E:\\Hardware Project\\GeekTrack\\4.Software\\TestGeekTrackSDK\\TestGeekTrack\\TestGeekTrack.csproj",
   "expectedPackageFiles": [],
   "logs": []
 }

4.Software/TestGeekTrackSDK/TestGeekTrack/obj/x64/Debug/netcoreapp3.1/TestGeekTrack.GeneratedMSBuildEditorConfig.editorconfig

@@ -1,3 +1,3 @@
 is_global = true
 build_property.RootNamespace = TestGeekTrack
-build_property.ProjectDir = E:\Hardware Project\GeekTrack\4.Software\TestGeekTrackSDK_V1.0\TestGeekTrack\
+build_property.ProjectDir = E:\Hardware Project\GeekTrack\4.Software\TestGeekTrackSDK\TestGeekTrack\

4.Software/TestGeekTrackSDK/TestGeekTrack/obj/x64/Debug/netcoreapp3.1/TestGeekTrack.csproj.FileListAbsolute.txt

@@ -25,3 +25,17 @@ E:\Hardware Project\GeekTrack\4.Software\TestGeekTrackSDK_V1.0\TestGeekTrack\obj
 E:\Hardware Project\GeekTrack\4.Software\TestGeekTrackSDK_V1.0\TestGeekTrack\obj\x64\Debug\netcoreapp3.1\TestGeekTrack.dll
 E:\Hardware Project\GeekTrack\4.Software\TestGeekTrackSDK_V1.0\TestGeekTrack\obj\x64\Debug\netcoreapp3.1\TestGeekTrack.pdb
 E:\Hardware Project\GeekTrack\4.Software\TestGeekTrackSDK_V1.0\TestGeekTrack\obj\x64\Debug\netcoreapp3.1\TestGeekTrack.genruntimeconfig.cache
+E:\Hardware Project\GeekTrack\4.Software\TestGeekTrackSDK\TestGeekTrack\bin\x64\Debug\netcoreapp3.1\TestGeekTrack.exe
+E:\Hardware Project\GeekTrack\4.Software\TestGeekTrackSDK\TestGeekTrack\bin\x64\Debug\netcoreapp3.1\TestGeekTrack.deps.json
+E:\Hardware Project\GeekTrack\4.Software\TestGeekTrackSDK\TestGeekTrack\bin\x64\Debug\netcoreapp3.1\TestGeekTrack.runtimeconfig.json
+E:\Hardware Project\GeekTrack\4.Software\TestGeekTrackSDK\TestGeekTrack\bin\x64\Debug\netcoreapp3.1\TestGeekTrack.runtimeconfig.dev.json
+E:\Hardware Project\GeekTrack\4.Software\TestGeekTrackSDK\TestGeekTrack\bin\x64\Debug\netcoreapp3.1\TestGeekTrack.dll
+E:\Hardware Project\GeekTrack\4.Software\TestGeekTrackSDK\TestGeekTrack\bin\x64\Debug\netcoreapp3.1\TestGeekTrack.pdb
+E:\Hardware Project\GeekTrack\4.Software\TestGeekTrackSDK\TestGeekTrack\obj\x64\Debug\netcoreapp3.1\TestGeekTrack.csproj.AssemblyReference.cache
+E:\Hardware Project\GeekTrack\4.Software\TestGeekTrackSDK\TestGeekTrack\obj\x64\Debug\netcoreapp3.1\TestGeekTrack.GeneratedMSBuildEditorConfig.editorconfig
+E:\Hardware Project\GeekTrack\4.Software\TestGeekTrackSDK\TestGeekTrack\obj\x64\Debug\netcoreapp3.1\TestGeekTrack.AssemblyInfoInputs.cache
+E:\Hardware Project\GeekTrack\4.Software\TestGeekTrackSDK\TestGeekTrack\obj\x64\Debug\netcoreapp3.1\TestGeekTrack.AssemblyInfo.cs
+E:\Hardware Project\GeekTrack\4.Software\TestGeekTrackSDK\TestGeekTrack\obj\x64\Debug\netcoreapp3.1\TestGeekTrack.csproj.CoreCompileInputs.cache
+E:\Hardware Project\GeekTrack\4.Software\TestGeekTrackSDK\TestGeekTrack\obj\x64\Debug\netcoreapp3.1\TestGeekTrack.dll
+E:\Hardware Project\GeekTrack\4.Software\TestGeekTrackSDK\TestGeekTrack\obj\x64\Debug\netcoreapp3.1\TestGeekTrack.pdb
+E:\Hardware Project\GeekTrack\4.Software\TestGeekTrackSDK\TestGeekTrack\obj\x64\Debug\netcoreapp3.1\TestGeekTrack.genruntimeconfig.cache

4.Software/TestGeekTrackSDK/TestGeekTrack/obj/x64/Debug/netcoreapp3.1/TestGeekTrack.genruntimeconfig.cache

@@ -1 +1 @@
-99993d063c3f93a631daf010ae6d9c95462d21a7
+6ce667a04e91fed21d0709417c88ad041d8af50e