GeekTrack/4.Software/GeekTrackSDK/Project/GeekTrack.cpp

#include"stdafx.h"
#include"GeekTrackSDK.h"
#include<stdio.h>
#include<thread>
#include<winsock2.h>
#include<windows.h>

#pragma comment(lib,"ws2_32.lib")

/* max suppport diffrent device id. */
#define MAX_SUPPORT_DEV_NUM   9

struct GeekTrackNode_t {
	EulerAngles_t Pose;
	bool devOnline;
	GeekTrackNode_t() {
		Pose.x = 0.0f;
		Pose.y = 0.0f;
		Pose.z = 0.0f;
		devOnline = false;
	}
};

GeekTrackNode_t TrackNode[MAX_SUPPORT_DEV_NUM];

SOCKET        sock;
sockaddr_in   from, a;
WSADATA       wsdata;
int           fromlength = 0;
std::thread*  t;

void GeekTrackUpdate() {
	char buf[256];
	int i = 0;
	while (1)
	{
		//<2F>ӹ㲥<D3B9><E3B2A5>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD><CFA2>ע<EFBFBD><D7A2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>󶨵ĵ<F3B6A8B5>ַ<EFBFBD>ͽ<EFBFBD><CDBD><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD>ĵ<EFBFBD>ַ<EFBFBD>Dz<EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
		recvfrom(sock, buf, 256, 0, (struct sockaddr FAR*) &from, (int FAR*) & fromlength);
		Sleep(10);
		float yaw = 0.0f, roll = 0.0f, pitch = 0.0f;
		int id = 0;
		int ret = sscanf_s(buf, "%d %f %f %f\n", &id, &yaw, &roll, &pitch);
		if (ret > 0 && (id >= 0 && id < MAX_SUPPORT_DEV_NUM)) {
			TrackNode[id].Pose.x = pitch;
			TrackNode[id].Pose.y = roll;
			TrackNode[id].Pose.z = yaw;
			TrackNode[id].devOnline = true;
		}
		ZeroMemory(buf, 256);
	}
}


void GeekTrack_Init() {

	static bool optval = false;
	//<2F><><EFBFBD><EFBFBD>SOCKET<45>⣬<EFBFBD>汾Ϊ2.0
	if (optval == false) {
		return;
	}
	WSAStartup(0x0202, &wsdata);
	optval = true;
	//Ȼ<><C8BB><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ַ<EFBFBD><D6B7>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>׽<EFBFBD><D7BD>֣<EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵĹ㲥<C4B9><E3B2A5>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD><CFA2>
	a.sin_family = AF_INET;
	a.sin_addr.s_addr = 0;
	a.sin_port = htons(9000);

	//from.sin_family = AF_INET;
	//from.sin_addr.s_addr = INADDR_BROADCAST;
	//from.sin_port = htons(5050);

	fromlength = sizeof(SOCKADDR);
	//<2F><>UDP<44><50>ʼ<EFBFBD><CABC><EFBFBD>׽<EFBFBD><D7BD><EFBFBD>
	sock = socket(AF_INET, SOCK_DGRAM, 0);
	// <20><><EFBFBD>ø<EFBFBD><C3B8>׽<EFBFBD><D7BD><EFBFBD>Ϊ<EFBFBD>㲥<EFBFBD><E3B2A5><EFBFBD>ͣ<EFBFBD>
	//setsockopt(sock, SOL_SOCKET, SO_BROADCAST, (char FAR *)&optval, sizeof(optval));
	// <20>Ѹ<EFBFBD><D1B8>׽<EFBFBD><D7BD>ְ<EFBFBD><D6B0><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>ַ<EFBFBD><D6B7>
	bind(sock, (sockaddr*)&a, sizeof(sockaddr_in));

	t = new std::thread(GeekTrackUpdate);
	
}

EulerAngles_t GetEulerAngles(int devId) {
	if (devId >= 0 && devId < MAX_SUPPORT_DEV_NUM) {
		return TrackNode[devId].Pose;
	}
	EulerAngles_t Pose = {0.0f, 0.0f, 0.0f};
	return Pose;
}

int main()
{
	GeekTrack_Init();
	int devId = 0;
	while (1) {
		Sleep(50);
		printf("%d,%f,%f,%f\n", devId,TrackNode[devId].Pose.x, TrackNode[devId].Pose.y,
			TrackNode[devId].Pose.z);
	}
	system("pause");
	return 0;
}