logzhan
/
GeekIMU
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
GeekIMU/4.Software/GeekIMUDriver 1.1/Src/main.cpp

1810 lines
46 KiB
C++
Raw Normal View History

上传GeekIMU资料
2024-11-09 21:39:20 +08:00
#include <stdio.h>
#include <windows.h>
#include <tchar.h>
#include "USB_Win32_HIDDevice.h"
#include "MagCalibrate.h"
#include "SensorImpl.h"
#include "IMU_CAPI.h"
#include "UsbHid_CAPI.h"
#include "quaternion.h"
#include <setupapi.h>
#include "hidsdi.h"
#include <iostream>
#include <sddl.h>
#include "PipeHMD.h"
#include<conio.h>
#include <windows.h>
#include <stdio.h>
#include <tchar.h>
#include <strsafe.h>
#include "openvr_driver.h"
#include "OVR_CAPI.h"
#include "MagCalibrate.h"
int global_calibration_flag = 0;//0none 1 gyro 2 acc 3 mag
int global_Show = 0;//0 hide 1 gyro 2acc 3mag 4angle 5Hex
std::string sCurrent_Device_str("null");
std::string strGlobalDevicePath("null");
extern "C" int globalpacklen;
extern "C" BOOL device_is_found;
extern "C" PSP_DEVICE_INTERFACE_DETAIL_DATA pData;
extern "C" HANDLE DM_HID_Read_HANDLE;
extern "C" HANDLE DM_HID_Write_HANDLE;
extern float gx0, gy0, gz0;
extern "C" OVERLAPPED usb_read_over_lapped;
int global_pipeserver = 0;
//extern Quaternion Qcorr;
bool global_debug = false;
#include <fstream>
#include <time.h>
FILE *fpdata;
#define BUFSIZE 512
#define COMMAND_DEVICE_IS_OPEN 1
#define COMMAND_GET_LEFT_POSE 2
#define COMMAND_GET_RIGHT_POSE 3
#define COMMAND_GET_LEFT_BAT 10
#define COMMAND_GET_RIGHT_BAT 11
#define COMMAND_SET_RUMBLE 20
#define LEFT_HAND 0
#define RIGHT_HAND 1
#define BUFSIZE 512
#define COMMAND_DEVICE_IS_OPEN 1
#define COMMAND_GET_DATA 2
#define COMMAND_DEVICE_CALIBRATION 3
#define COMMAND_DEVICE_GET_PURE_DATA 4
#define COMMAND_DEVICE_ENTER_DFU_MODE 11
#define ENTER_DFU_MODE_SUCCESS 1
#define ENTER_DFU_MODE_FAILED 0
#define CALIBRATION_SUCCESS 1
#define CALIBRATION_FAILED 0
#define FEATURE_OFFSET_SET_GYRO 1
#define FEATURE_OFFSET_SET_ACC 2
#define FEATURE_OFFSET_SET_MAG 3
#define FEATURE_OFFSET_SET_LED 4
#define FEATURE_OFFSET_GET_GYRO 0x11
#define FEATURE_OFFSET_GET_ACC 0x12
#define FEATURE_OFFSET_GET_MAG 0x13
#define LED_COLOR_BLACK 0x00
#define LED_COLOR_WHITE 0x07
#define LED_COLOR_RED 0x04
#define LED_COLOR_GREEN 0x02
#define LED_COLOR_BLUE 0x01
#define LED_COLOR_CYAN 0x03
#define LED_COLOR_MAGENTA 0x06
#define LED_COLOR_YELLOW 0x05
UINT8 LED_COLOR_LIST[8] = {LED_COLOR_BLACK,LED_COLOR_WHITE,LED_COLOR_RED,LED_COLOR_GREEN,
LED_COLOR_BLUE,LED_COLOR_CYAN,LED_COLOR_MAGENTA,LED_COLOR_YELLOW,};
Quaternion CQGamePad = { 1, 0, 0, 0 };
Quaternion CQHMD = { 1, 0, 0, 0 };
#define FEATURE_OFFSET_SIZE 52//3+1+4*12
DWORD WINAPI InstanceThread(LPVOID);
VOID GetAnswerToRequest(LPCSTR, LPCSTR, LPDWORD);
BOOL CreatePipeSecurity(PSECURITY_ATTRIBUTES *ppSa)
{
BOOL fSucceeded = TRUE;
DWORD dwError = ERROR_SUCCESS;
PSECURITY_DESCRIPTOR pSd = NULL;
PSECURITY_ATTRIBUTES pSa = NULL;
// Define the SDDL for the security descriptor.
LPCSTR szSDDL = "D:" // Discretionary ACL
"(A;OICI;GRGW;;;AU)" // Allow read/write to authenticated users
"(A;OICI;GA;;;BA)"; // Allow full control to administrators
if (!ConvertStringSecurityDescriptorToSecurityDescriptor(szSDDL,
SDDL_REVISION_1, &pSd, NULL))
{
fSucceeded = FALSE;
dwError = GetLastError();
goto Cleanup;
}
// Allocate the memory of SECURITY_ATTRIBUTES.
pSa = (PSECURITY_ATTRIBUTES)LocalAlloc(LPTR, sizeof(*pSa));
if (pSa == NULL)
{
fSucceeded = FALSE;
dwError = GetLastError();
goto Cleanup;
}
pSa->nLength = sizeof(*pSa);
pSa->lpSecurityDescriptor = pSd;
pSa->bInheritHandle = FALSE;
*ppSa = pSa;
Cleanup:
// Clean up the allocated resources if something is wrong.
if (!fSucceeded)
{
if (pSd)
{
LocalFree(pSd);
pSd = NULL;
}
if (pSa)
{
LocalFree(pSa);
pSa = NULL;
}
SetLastError(dwError);
}
return fSucceeded;
}
void FreePipeSecurity(PSECURITY_ATTRIBUTES pSa)
{
if (pSa)
{
if (pSa->lpSecurityDescriptor)
{
LocalFree(pSa->lpSecurityDescriptor);
}
LocalFree(pSa);
}
}
int pipeservermain(VOID)
{
if (global_pipeserver != 1)
{
return 0;
}
global_pipeserver = 2;
BOOL fConnected = FALSE;
DWORD dwThreadId = 0;
HANDLE hPipe = INVALID_HANDLE_VALUE, hThread = NULL;
//LPCSTR lpszPipename = TEXT("\\\\.\\pipe\\Dematrix_SteamVR");
LPCSTR lpszPipename = TEXT("\\\\.\\pipe\\Dematrix_CTL");
PSECURITY_ATTRIBUTES pSa;
if (!CreatePipeSecurity(&pSa))
{
printf("CreatePipeSecurity pSa FALSE\n");
FreePipeSecurity(pSa);
return 0;
}
// The main loop creates an instance of the named pipe and
// then waits for a client to connect to it. When the client
// connects, a thread is created to handle communications
// with that client, and this loop is free to wait for the
// next client connect request. It is an infinite loop.
for (;;)
{
if (global_pipeserver == -1)
{
global_pipeserver = 0;
break;
}
//_tprintf(TEXT("\nPipe Server: Main thread awaiting client connection on %s\n"), lpszPipename);
hPipe = CreateNamedPipe(
lpszPipename, // pipe name
PIPE_ACCESS_DUPLEX, // read/write access
PIPE_TYPE_MESSAGE | // message type pipe
PIPE_READMODE_MESSAGE | // message-read mode
PIPE_WAIT, // blocking mode
PIPE_UNLIMITED_INSTANCES, // max. instances
BUFSIZE, // output buffer size
BUFSIZE, // input buffer size
0, // client time-out
pSa); // default security attribute
if (hPipe == INVALID_HANDLE_VALUE)
{
_tprintf(TEXT("CreateNamedPipe failed, GLE=%d.\n"), GetLastError());
return -1;
}
// Wait for the client to connect; if it succeeds,
// the function returns a nonzero value. If the function
// returns zero, GetLastError returns ERROR_PIPE_CONNECTED.
fConnected = ConnectNamedPipe(hPipe, NULL) ?
TRUE : (GetLastError() == ERROR_PIPE_CONNECTED);
if (fConnected)
{
//printf("Client connected, creating a processing thread.\n");
// Create a thread for this client.
hThread = CreateThread(
NULL, // no security attribute
0, // default stack size
InstanceThread, // thread proc
(LPVOID)hPipe, // thread parameter
0, // not suspended
&dwThreadId); // returns thread ID
if (hThread == NULL)
{
_tprintf(TEXT("CreateThread failed, GLE=%d.\n"), GetLastError());
return -1;
}
else CloseHandle(hThread);
}
else
// The client could not connect, so close the pipe.
CloseHandle(hPipe);
}
printf("<EFBFBD>ܵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹ....\n");
return 0;
}
DWORD WINAPI InstanceThread(LPVOID lpvParam)
// This routine is a thread processing function to read from and reply to a client
// via the open pipe connection passed from the main loop. Note this allows
// the main loop to continue executing, potentially creating more threads of
// of this procedure to run concurrently, depending on the number of incoming
// client connections.
{
HANDLE hHeap = GetProcessHeap();
CHAR* pchRequest = (CHAR*)HeapAlloc(hHeap, 0, BUFSIZE*sizeof(CHAR));
CHAR* pchReply = (CHAR*)HeapAlloc(hHeap, 0, BUFSIZE*sizeof(CHAR));
DWORD cbBytesRead = 0, cbReplyBytes = 0, cbWritten = 0;
BOOL fSuccess = FALSE;
HANDLE hPipe = NULL;
// Do some extra error checking since the app will keep running even if this
// thread fails.
if (lpvParam == NULL)
{
// printf("\nERROR - Pipe Server Failure:\n");
// printf(" InstanceThread got an unexpected NULL value in lpvParam.\n");
// printf(" InstanceThread exitting.\n");
if (pchReply != NULL) HeapFree(hHeap, 0, pchReply);
if (pchRequest != NULL) HeapFree(hHeap, 0, pchRequest);
return (DWORD)-1;
}
if (pchRequest == NULL)
{
// printf("\nERROR - Pipe Server Failure:\n");
// printf(" InstanceThread got an unexpected NULL heap allocation.\n");
// printf(" InstanceThread exitting.\n");
if (pchReply != NULL) HeapFree(hHeap, 0, pchReply);
return (DWORD)-1;
}
if (pchReply == NULL)
{
// printf("\nERROR - Pipe Server Failure:\n");
// printf(" InstanceThread got an unexpected NULL heap allocation.\n");
// printf(" InstanceThread exitting.\n");
if (pchRequest != NULL) HeapFree(hHeap, 0, pchRequest);
return (DWORD)-1;
}
// Print verbose messages. In production code, this should be for debugging only.
//printf("InstanceThread created, receiving and processing messages.\n");
// The thread's parameter is a handle to a pipe object instance.
hPipe = (HANDLE)lpvParam;
// Loop until done reading
while (1)
{
// Read client requests from the pipe. This simplistic code only allows messages
// up to BUFSIZE characters in length.
fSuccess = ReadFile(
hPipe, // handle to pipe
pchRequest, // buffer to receive data
BUFSIZE*sizeof(TCHAR), // size of buffer
&cbBytesRead, // number of bytes read
NULL); // not overlapped I/O
if (!fSuccess || cbBytesRead == 0)
{
if (GetLastError() == ERROR_BROKEN_PIPE)
{
//_tprintf(TEXT("InstanceThread: client disconnected.\n"), GetLastError());
}
else
{
_tprintf(TEXT("InstanceThread ReadFile failed, GLE=%d.\n"), GetLastError());
}
break;
}
// Process the incoming message.
GetAnswerToRequest(pchRequest, pchReply, &cbReplyBytes);
// Write the reply to the pipe.
fSuccess = WriteFile(
hPipe, // handle to pipe
pchReply, // buffer to write from
cbReplyBytes, // number of bytes to write
&cbWritten, // number of bytes written
NULL); // not overlapped I/O
if (!fSuccess || cbReplyBytes != cbWritten)
{
_tprintf(TEXT("InstanceThread WriteFile failed, GLE=%d.\n"), GetLastError());
break;
}
}
// Flush the pipe to allow the client to read the pipe's contents
// before disconnecting. Then disconnect the pipe, and close the
// handle to this pipe instance.
FlushFileBuffers(hPipe);
DisconnectNamedPipe(hPipe);
CloseHandle(hPipe);
HeapFree(hHeap, 0, pchRequest);
HeapFree(hHeap, 0, pchReply);
//printf("InstanceThread exitting.\n");
return 1;
}
void ProcessHMDData(void *read_buffer, void* write_buffer, LPDWORD pchBytes){
unsigned char* writeBuffer = (unsigned char*)write_buffer;
BOOL status = FALSE;
unsigned int *int_value;
int_value = (unsigned int *)read_buffer;
memset(writeBuffer, 0, 512);
switch (int_value[0]){
case COMMAND_DEVICE_IS_OPEN:{
BOOL *result = (BOOL*)&writeBuffer[0];
result[0] = TRUE;
if (!device_is_found)
{
result[0] = FALSE;
}
*pchBytes = sizeof(BOOL);
break;
}
case COMMAND_GET_DATA:{
ovrSensorState *result = (ovrSensorState*)&writeBuffer[0];
Quaternion q = GetPoseQuat();
result[0].Recorded.Pose.Orientation.w = q.w;
result[0].Recorded.Pose.Orientation.x = q.x;
result[0].Recorded.Pose.Orientation.y = q.y;
result[0].Recorded.Pose.Orientation.z = q.z;
*pchBytes = sizeof(ovrSensorState);
break;
}
case COMMAND_DEVICE_CALIBRATION:{
BOOL *result = (BOOL*)&writeBuffer[0];
result[0] = TRUE;
if (!device_is_found)
{
result[0] = FALSE;
}
*pchBytes = sizeof(BOOL);
break;
}
// case COMMAND_DEVICE_GET_PURE_DATA:{
// float *result = (float*)&writeBuffer[0];
// result[0] = usbDEV.fBatteryLevel[LEFT_HAND];
// if (!device_is_found)
// {
// result[0] = 0.0f;
// }
// *pchBytes = sizeof(float);
// break;
// }
case COMMAND_DEVICE_ENTER_DFU_MODE:{
BOOL *result = (BOOL*)&writeBuffer[0];
result[0] = TRUE;
if (!device_is_found)
{
result[0] = FALSE;
}
*pchBytes = sizeof(BOOL);
break;
}
}
}
extern UINT8 global_key[4];
float global_ctlposition[3] = {0,0,0};
void ProcessCtlData(void *read_buffer, void* write_buffer, LPDWORD pchBytes){
unsigned char* writeBuffer = (unsigned char*)write_buffer;
BOOL status = FALSE;
unsigned int *int_value;
int_value = (unsigned int *)read_buffer;
memset(writeBuffer, 0, 512);
switch (int_value[0]){
case COMMAND_DEVICE_IS_OPEN:{
BOOL *result = (BOOL*)&writeBuffer[0];
*result = device_is_found;
*pchBytes = sizeof(BOOL);
break;
}
case COMMAND_GET_LEFT_POSE:{
vr::DriverPose_t *result = (vr::DriverPose_t*)&writeBuffer[0];
//Quaternion q = DM_GetQ();
result[0].qRotation.w = 1;
result[0].qRotation.x = 0;
result[0].qRotation.y = 0;
result[0].qRotation.z = 0;
*pchBytes = sizeof(vr::DriverPose_t);
break;
}
case COMMAND_GET_RIGHT_POSE:{
vr::DriverPose_t *result = (vr::DriverPose_t*)&writeBuffer[0];
Quaternion q = QuaternionMulti(GetPoseQuat(),CQGamePad);
result[0].qRotation.w = q.w;
result[0].qRotation.x = q.x;
result[0].qRotation.y = q.y;
result[0].qRotation.z = q.z;
result[0].vecPosition[0]=global_ctlposition[0];
result[0].vecPosition[1]=global_ctlposition[1];
result[0].vecPosition[2]=global_ctlposition[2];
unsigned char *aAxis = (unsigned char *)&result[0].vecVelocity[3];
aAxis[0] = global_key[0]<<1;//unsigned char K[4] ;//<2F><><EFBFBD><EFBFBD>Ϊ 8<><38><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(8bit<69><74><EFBFBD>Ρ<EFBFBD>,<2C><>,RB,<2C><>(<28>ᰴ<EFBFBD><E1B0B4>), Y,B,A,X ),x<><78>,y<><79>,<2C><>trig
//aAxis[1] = usbDEV.K[hand][1];
//aAxis[2] = usbDEV.K[hand][2];
aAxis[3] = global_key[3];//<2F><>͵<EFBFBD><CDB5><EFBFBD><EFBFBD><EFBFBD>⴫<EFBFBD>İ<EFBFBD><C4B0><EFBFBD>״̬......<2E><>˵<EFBFBD><CBB5><EFBFBD><EFBFBD>ҪvecVelocity<74><79><EFBFBD><EFBFBD>....
*pchBytes = sizeof(vr::DriverPose_t);
break;
}
case COMMAND_GET_LEFT_BAT:{
float *result = (float*)&writeBuffer[0];
result[0] = 0;
if (!device_is_found)
{
result[0] = 0.0f;
}
*pchBytes = sizeof(float);
break;
}
case COMMAND_GET_RIGHT_BAT:{
float *result = (float*)&writeBuffer[0];
result[0] = 0.7f;
if (!device_is_found)
{
result[0] = 0.0f;
}
*pchBytes = sizeof(float);
break;
}
case COMMAND_SET_RUMBLE:{
BOOL *result = (BOOL*)&writeBuffer[0];
*result = true;// TriggerHaptic(*(unsigned int*)a_command.data / 255, 50);
*pchBytes = sizeof(BOOL);
break;
}
}
}
VOID GetAnswerToRequest(LPCSTR pchRequest,
LPCSTR pchReply,
LPDWORD pchBytes)
{
// This routine is a simple function to print the client request to the console
// and populate the reply buffer with a default data string. This is where you
// would put the actual client request processing code that runs in the context
// of an instance thread. Keep in mind the main thread will continue to wait for
// and receive other client connections while the instance thread is working.
ProcessCtlData((void*)pchRequest, (void*)pchReply, pchBytes);
}
FILE* Getfpdata()
{
return fpdata;
}
Quaternion CorrectQuaterinonY(Quaternion q)
{
Quaternion qcorr;
qcorr.w = q.w; qcorr.x = 0.0f;
qcorr.y = q.y; qcorr.z = 0.0f;
qcorr = QuaternionInversion(qcorr);
float mod = QuaternionMod(qcorr);
qcorr.w = qcorr.w / mod;
qcorr.x = qcorr.x / mod;
qcorr.y = qcorr.y / mod;
qcorr.z = qcorr.z / mod;
return qcorr;
}
Quaternion CorrectQuaterinon(Quaternion q)
{
Quaternion qcorr;
qcorr.w = q.w; qcorr.x = 0.0f;
qcorr.y = q.y; qcorr.z = 0.0f;
qcorr = QuaternionInversion(qcorr);
float mod = QuaternionMod(qcorr);
qcorr.w = qcorr.w / mod;
qcorr.x = qcorr.x / mod;
qcorr.y = qcorr.y / mod;
qcorr.z = qcorr.z / mod;
return qcorr;
}
void ShowHelp()
{
printf("============================HELP===========================\n");
printf("h - <20><>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD>\n");
printf("c - <20><><EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD><E8B1B8><EFBFBD><EFBFBD>\n");
printf("d - <20><><EFBFBD><EFBFBD>DFUģʽ\n");
printf("l - LED <20><>ɫ<EFBFBD><C9AB><EFBFBD><EFBFBD>\n");
printf("t - У׼<D0A3><D7BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>\n");
// printf(" - b - ȡ<><C8A1>У׼\n");
// printf(" - s - <20><>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>\n");
// printf(" - - b - <20><><EFBFBD><EFBFBD>\n");
printf("p - <20><><EFBFBD><EFBFBD><EFBFBD>ܵ<EFBFBD><DCB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>\n");
// printf(" - r - <20><><EFBFBD><EFBFBD><EFBFBD>ܵ<EFBFBD><DCB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>\n");
// printf(" - s - <20>رչܵ<D5B9><DCB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>\n");
printf("g - <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У׼\n");
// printf(" - s - <20><>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>\n");
// printf(" - - b - <20><><EFBFBD><EFBFBD>\n");
// printf(" - c - У׼<D0A3><D7BC><EFBFBD><EFBFBD>\n");
// printf(" - r - <20><><EFBFBD><EFBFBD>Ϊ0\n");
// printf(" - g - <20><>ȡ<EFBFBD><C8A1>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>Ǽ<EFBFBD>У׼<D0A3><D7BC><EFBFBD><EFBFBD>\n");
// printf(" - b - <20><><EFBFBD><EFBFBD>\n");
printf("m - <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У׼\n");
// printf(" - g - <20><>ȡ<EFBFBD><C8A1>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У׼<D0A3><D7BC><EFBFBD><EFBFBD>\n");
// printf(" - s - д<><D0B4>Ĭ<EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD>Ʋ<EFBFBD><C6B2><EFBFBD>\n");
// printf(" - c - <20><>ʼ<EFBFBD><CABC>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD>д<EFBFBD><D0B4>data.txt\n");
// printf(" - - f - <20><><EFBFBD>ݻ<EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD> <20><>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD>µ<EFBFBD>У׼<D0A3><D7BC><EFBFBD><EFBFBD>\n");
// printf(" - - c - ȡ<><C8A1>У׼\n");
// printf(" - - q - <20>˳<EFBFBD>У׼\n");
printf("a - <20><>ʾŷ<CABE><C5B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>\n");
// printf(" - b - <20><><EFBFBD><EFBFBD>\n");
printf("o - <20><>ʾԭʼ<D4AD><CABC><EFBFBD><EFBFBD>\n");
// printf(" - b - <20><><EFBFBD><EFBFBD>\n");
printf("M - <20>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>\n");
printf(" cУ׼<D0A3><D7BC><EFBFBD><EFBFBD> b<><62><EFBFBD><EFBFBD> <20>ո<EFBFBD><D5B8><EFBFBD><EFBFBD><EFBFBD> \n");
printf(" ws ad qe<71><65><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB> \n");
printf(" WS AD QE<51><45><EFBFBD><EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB> \n");
printf("============================<3D><><EFBFBD><EFBFBD>===========================\n");
}
int main()
{
// printf("DMGamePad<61><64><EFBFBD>Թ<EFBFBD><D4B9><EFBFBD> 2016-12-13 02:11:20\n");
// /**
// * <20><>ʼ<EFBFBD><CABC>USB_HID
// * <20><><EFBFBD>س<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// * <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD>߳<EFBFBD>
// **/globalpacklen = 64;
// USB_HID_Init();
// find_device:
// /*<2A><><EFBFBD><EFBFBD>PID=0x0001<30><31>VIDΪ0x2833<33><33>USB<53>豸*/
// //while (!DM_HIDDeviceEnumerate(0x5750, 0x0483))
// while (!DM_HIDDeviceEnumerate(0x0002, 0x2833))
// {
// printf("<22>Ҳ<EFBFBD><D2B2><EFBFBD>0x0001, 0x2833 USB<53>豸!\n");
// Sleep(1000);
// // if (_getche()=='q')
// // {
// // return 0;
// // }
// }
Init("VID_2833&PID_0002");
//system("cls");
ShowHelp();
DMPipe::PipeHMD p("\\\\.\\Pipe\\Dematrix_SteamVR");
// UINT8 MAG_Info[FEATURE_OFFSET_SIZE] = { 0x0b };
// UINT8 MAG_Set[FEATURE_OFFSET_SIZE] = { 0x0b};
UINT8 FEATURE_OFFSET_Get[FEATURE_OFFSET_SIZE] = { 0x0b };
UINT8 FEATURE_OFFSET_Set[FEATURE_OFFSET_SIZE] = { 0x0b };
UINT8* offset_get_dev = (UINT8*)(FEATURE_OFFSET_Get + 3);
float* offset_get_data = (float*)(FEATURE_OFFSET_Get + 4);
UINT8* offset_set_dev = (UINT8*)(FEATURE_OFFSET_Set + 3);
float* offset_set_data = (float*)(FEATURE_OFFSET_Set + 4);
while (1)
{
//printf(">>");
*offset_set_dev = *offset_get_dev = 0;
memset(offset_get_data, 0, FEATURE_OFFSET_SIZE - 4);
memset(offset_set_data, 0, FEATURE_OFFSET_SIZE - 4);
char s;
printf(">>"); s = _getche(); printf("\n");
if (s == 'c')//<2F><><EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>Ч
{
if (CheckDevice()==0)
{
printf("<EFBFBD>豸<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>...\n");
}
else
{
printf("<EFBFBD>豸<EFBFBD>Ͽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>...\n");
device_is_found = FALSE;
Sleep(1000);
//goto find_device;
}
}
else if (s == 'h')
{
ShowHelp();
}
else if (s == 'a')
{
global_Show = 4;
while (global_Show != 0)
{
printf(">>"); s = _getche(); printf("\n");
if (s == 'b')
{
global_Show = 0;
}
}
}
else if (s == 'M')
{
int _flag = 1;
float step = 0.005f;
//clrscr();
//global_Show = 4;
while (_flag!=0)
{
printf(">>"); s = _getch(); printf("\n");
MVector3 tmppos = {0,0,0};
Quaternion qhmd = { 1, 0, 0, 0 };
if (s == 'b')
{
_flag = 0;
}
else if (s == ' ')
{
global_ctlposition[0] = 0;
global_ctlposition[1] = 0;
global_ctlposition[2] = 0;
}
else if (s == 'r')
{
CQHMD = {1,0,0,0};
CQGamePad = { 1, 0, 0, 0 };
}
else if (s == 'c')
{
if (p.IsOpen())
{
ovrSensorState ss = p.GetSensorData();
qhmd = { ss.Recorded.Pose.Orientation.w, 0, ss.Recorded.Pose.Orientation.y, 0 };
CQHMD = CorrectQuaterinonY(qhmd);
CQGamePad = QuaternionMulti(CorrectQuaterinonY(GetPoseQuat()), qhmd);
printf("Success!\n");
}
}
else if ((s == 'w') || (s == 'W'))
{
tmppos = { 0, 0, -1 };//<2F><>ǰ -z<><7A><EFBFBD><EFBFBD>
}
else if ((s == 's') || (s == 'S'))
{
tmppos = { 0, 0, 1 };//<2F><><EFBFBD><EFBFBD> z<><7A><EFBFBD><EFBFBD>
}
else if ((s == 'a') || (s == 'A'))
{
tmppos = { 0, 0, -1 };//<2F><><EFBFBD><EFBFBD> -x<><78><EFBFBD><EFBFBD>
}
else if ((s == 'd') || (s == 'D'))
{
tmppos = { 0, 0, 1 };//<2F><><EFBFBD><EFBFBD> x<><78><EFBFBD><EFBFBD>
}
else if ((s == 'q') || (s == 'Q'))
{
global_ctlposition[1] -= step;
}
else if ((s == 'e') || (s == 'E'))
{
global_ctlposition[1] += step;
}
Quaternion cq;// = CorrectQuaterinonY(DM_GetQ());
if ((s == 'W') || (s == 'A') || (s == 'S') || (s == 'D'))
{
if (p.IsOpen())
{
ovrSensorState ss = p.GetSensorData();
qhmd = { ss.Recorded.Pose.Orientation.w, ss.Recorded.Pose.Orientation.x, ss.Recorded.Pose.Orientation.y, ss.Recorded.Pose.Orientation.z };
CQHMD = CorrectQuaterinonY(qhmd);
}
cq = CQHMD;
}
else if ((s == 'w') || (s == 'a') || (s == 's') || (s == 'd'))
{
cq = QuaternionMulti(CorrectQuaterinonY(GetPoseQuat()), CQGamePad);
}
if ((s == 'a') || (s == 'd') || (s == 'A') || (s == 'D'))
{
cq = QuaternionMulti(cq, CreateQuaternion(-90, { 0, 1, 0 }));
}
Matrix3 rm3 = QuaternionToMatrix(cq);
MVector3 posv3 = ComputeRotate(rm3, tmppos);
global_ctlposition[2] += posv3.v[2] * step;
global_ctlposition[0] += posv3.v[0] * step;
printf("POS:%f %f %f\n", global_ctlposition[0], global_ctlposition[1], global_ctlposition[2]);
}
}
else if (s == 'o')//ԭʼ<D4AD><CABC><EFBFBD><EFBFBD>
{
system("cls");
global_Show = 5;
while (global_Show != 0)
{
printf(">>"); s = _getche(); printf("\n");
if (s == 'b')
{
global_Show = 0;
}
}
}
else if (s=='l')
{
printf("\nLED <20><>ɫ<EFBFBD><C9AB><EFBFBD><EFBFBD>\n");
printf("K - BLACK\n");
printf("W - WHITE\n");
printf("R - RED\n");
printf("G - GREEN\n");
printf("B - BLUE\n");
printf("C - CYAN\n");
printf("M - MAGENTA\n");
printf("Y - YELLOW\n");
printf("L - LOOP\n");
printf(">>"); s = _getche(); printf("\n");
if (s=='L'||s=='l')
{
for (int i = 0; i < 8; i++)
{
*offset_set_dev = FEATURE_OFFSET_SET_LED;
FEATURE_OFFSET_Set[4] = LED_COLOR_LIST[i];
if (HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE))
{
//printf("%x ", FEATURE_OFFSET_Set[4]);
}
Sleep(250);
}
*offset_set_dev = FEATURE_OFFSET_SET_LED;
FEATURE_OFFSET_Set[4] = LED_COLOR_LIST[5];
if (HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE))
{
//printf("%x ", FEATURE_OFFSET_Set[4]);
}
}
else
{
char colorlist[8]{'K', 'W', 'R', 'G', 'B', 'C', 'M', 'Y'};
for (int i = 0; i < 8; i++)
{
if (colorlist[i] == s || colorlist[i] == (s - 32))//<2F><>Сд
{
*offset_set_dev = FEATURE_OFFSET_SET_LED;
FEATURE_OFFSET_Set[4] = LED_COLOR_LIST[i];
if (HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE))
{
printf("Color Set\n");
//printf("%x ", FEATURE_OFFSET_Set[4]);
}
else
{
printf("FAILED\n");
}
break;
}
}
}
}
else if (s == 'g')
{
printf("\n<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У׼\n");
printf(" - s - <20><>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>\n");
printf(" - - b - <20><><EFBFBD><EFBFBD>\n");
printf(" - c - У׼<D0A3><D7BC><EFBFBD><EFBFBD>\n");
printf(" - r - <20><><EFBFBD><EFBFBD>Ϊ0\n");
printf(" - g - <20><>ȡ<EFBFBD><C8A1>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>Ǽ<EFBFBD>У׼<D0A3><D7BC><EFBFBD><EFBFBD>\n");
printf(" - b - <20><><EFBFBD><EFBFBD>\n");
printf(">>"); s = _getche(); printf("\n");
if (s == 'c')
{
//printf("<22><>λ<EFBFBD><CEBB>У׼<D0A3><D7BC>δʵ<CEB4><CAB5>\n");
*offset_set_dev = FEATURE_OFFSET_GET_GYRO;
HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE);
UINT8 GYRO_Cali_Last[FEATURE_OFFSET_SIZE] = { 0x0B };
GYRO_Cali_Last[3] = FEATURE_OFFSET_GET_GYRO;
if (HidD_GetFeature(DM_HID_Read_HANDLE, GYRO_Cali_Last, FEATURE_OFFSET_SIZE))
{
float gyrodefault[3] = { 0.0f, 0.0f, 0.0f};
memcpy(offset_set_data, gyrodefault, sizeof(float) * 3);
*offset_set_dev = FEATURE_OFFSET_SET_GYRO;
HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE);
global_Show = 1;
global_calibration_flag = 1;
while (global_calibration_flag == 1)//<2F><>ʼ<EFBFBD><CABC>ȡgyro<72><6F><EFBFBD><EFBFBD>
{
}
global_Show = 0;
Sleep(500);
*offset_set_dev = FEATURE_OFFSET_SET_GYRO;
float gyronew[3] = { -gx0,-gz0,-gy0 };//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
memcpy(offset_set_data, gyronew, sizeof(float) * 3);
if (HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE))
{
for (int i = 0; i < FEATURE_OFFSET_SIZE; i++)
{
if(global_debug) printf("%2x ", FEATURE_OFFSET_Set[i]);
}
printf("\n");
printf("SetGyroOffset\n");
}
memset(offset_get_data, 0, FEATURE_OFFSET_SIZE - 4);
*offset_set_dev = FEATURE_OFFSET_GET_GYRO;
HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE);
*offset_get_dev = FEATURE_OFFSET_GET_GYRO;
if (HidD_GetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Get, FEATURE_OFFSET_SIZE))
{
//<2F><>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD>
for (int i = 0; i < 3; i++)
{
printf("%f ", offset_get_data[i]);
}
printf("\n");
}
}
}
else if (s == 'g')//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>
{
*offset_set_dev = FEATURE_OFFSET_GET_GYRO;
HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE);
*offset_get_dev = FEATURE_OFFSET_GET_GYRO;
if (HidD_GetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Get, FEATURE_OFFSET_SIZE))
{
printf("GetGyro\n");
for (int i = 0; i < FEATURE_OFFSET_SIZE; i++)
{
if(global_debug) printf("%2x ", FEATURE_OFFSET_Get[i]);
}
printf("\n");
//<2F><>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD>
//float* f= (float*)(FEATURE_OFFSET_Get + 6);
for (int i = 0; i < 3; i++)
{
printf("%f ",offset_get_data[i]);
}
printf("\n");
}
}
else if (s=='r')
{
*offset_set_dev = FEATURE_OFFSET_SET_GYRO;
float gyronew[3] = { 0, 0, 0 };
memcpy(offset_set_data, gyronew, sizeof(float) * 3);
if (HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE))
{
printf("ResetGyroOffset\n");
}
for (int i = 0; i < 3; i++)
{
printf("%f ", offset_set_data[i]);
}
printf("\n");
}
else if (s == 's')
{
global_Show = 1;
while (global_Show != 0)
{
printf(">>"); s = _getche(); printf("\n");
if (s == 'b')
{
global_Show = 0;
}
}
}
else if (s == 'b')
{
}
}
else if (s == 't')
{
printf("\n<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У׼\n<EFBFBD>뾲<EFBFBD><EFBFBD>ͷ<EFBFBD><EFBFBD> \n <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼУ׼<D0A3><D7BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>\n");
printf(" - b - ȡ<><C8A1>У׼\n");
printf(" - s - <20><>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>\n");
printf(" - - b - <20><><EFBFBD><EFBFBD>\n");
printf(">>"); s = _getche(); printf("\n");
if (s == 'b')
{
printf("У׼ȡ<EFBFBD><EFBFBD>\n");
}
else if (s == 's')
{
global_Show = 1;
while (global_Show != 0)
{
printf(">>"); s = _getche(); printf("\n");
if (s == 'b')
{
global_Show = 0;
}
}
}
else
{
Sleep(1000);
UINT8 gyrocali[5] = { 0x08, 0x00, 0x00, 0x10, 0x26 };
if (HidD_SetFeature(DM_HID_Read_HANDLE, gyrocali, 5))
{
printf("<EFBFBD><EFBFBD><EFBFBD><EFBFBD>У׼<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>...<2E>뱣<EFBFBD><EBB1A3>ͷ<EFBFBD><CDB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>!\n");
Sleep(5000);
printf("У׼<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>dzɹ<EFBFBD>\n");
}
else
{
printf("У׼<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD><EFBFBD>\n");
}
}
}
else if (s == 'd')
{
printf("enter dfu\n");
UINT8 dfu[5] = { 0x08, 0x00, 0x00, 0x10, 0x25 };
HidD_SetFeature(DM_HID_Read_HANDLE, dfu, 5);
}
else if (s == 'i')
{
printf("chipid\n");
UINT8 chipid[15] = { 0x0A };
HidD_GetFeature(DM_HID_Read_HANDLE, chipid, 15);
for (int i = 0; i < 15; i++)
{
if(global_debug) printf("%2x ", chipid[i]);
}
printf("\n");
}
else if (s == 'm')
{
printf("\n<EFBFBD><EFBFBD><EFBFBD><EFBFBD>У׼\n");
printf(" - l - <20><><EFBFBD><EFBFBD>2data.txt");
printf(" - r - <20><><EFBFBD><EFBFBD>ΪĬ<CEAA><C4AC>ֵ\n");
printf(" - g - <20><>ȡ<EFBFBD><C8A1>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У׼<D0A3><D7BC><EFBFBD><EFBFBD>\n");
printf(" - s - д<><D0B4>Ĭ<EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD>Ʋ<EFBFBD><C6B2><EFBFBD>\n");
printf(" - c - <20><>ʼ<EFBFBD><CABC>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD>д<EFBFBD><D0B4>data.txt\n");
printf(" - - f - <20><><EFBFBD>ݻ<EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD> <20><>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD>µ<EFBFBD>У׼<D0A3><D7BC><EFBFBD><EFBFBD>\n");
printf(" - - c - ȡ<><C8A1>У׼\n");
printf(" - - q - <20>˳<EFBFBD>У׼\n");
printf(">>"); s = _getche(); printf("\n");
if (s=='l')
{
GetMagDataFormFile();
}
else if (s == 's')
{
printf("setreport\n");
float magcali[6] = { -2.018673f, -2.303837f, 1.989453f, 4.899964f, 4.839235f, 5.541566f };
memcpy(offset_set_data, magcali, sizeof(float) * 6);
*offset_set_dev = FEATURE_OFFSET_SET_MAG;
for (int i = 0; i < FEATURE_OFFSET_SIZE; i++)
{
if(global_debug) printf("%2x ", FEATURE_OFFSET_Set[i]);
}
if (HidD_SetFeature(DM_HID_Read_HANDLE,FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE))
{
printf("success\n");
}
}
else if (s == 'r')
{
*offset_set_dev = FEATURE_OFFSET_SET_MAG;
float magdefault[6] = { 0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f };
memcpy(offset_set_data, magdefault, sizeof(float) * 6);
if (HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE))
{
printf("ResetMagOffset\n");
}
for (int i = 0; i < 6; i++)
{
printf("%f ", offset_set_data[i]);
}
printf("\n");
}
else if (s == 'g')
{
// printf("GetMag\n");
//
// for (int i = 0; i < FEATURE_OFFSET_SIZE; i++)
// {
// if(global_debug) printf("%2x ", FEATURE_OFFSET_Get[i]);
// }
// printf("\nin float\n");
// for (int i = 0; i < 6; i++)
// {
// printf("%f ", offset_get_data[i]);
// }
// printf("\n");
*offset_set_dev = FEATURE_OFFSET_GET_MAG;
HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE);
*offset_get_dev = FEATURE_OFFSET_GET_MAG;
if (HidD_GetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Get, FEATURE_OFFSET_SIZE))
{
for (int i = 0; i < FEATURE_OFFSET_SIZE; i++)
{
if(global_debug) printf("%2x ", FEATURE_OFFSET_Get[i]);
}
printf("\nin float\n");
for (int i = 0; i < 6; i++)
{
printf("%f ", offset_get_data[i]);
}
printf("\n");
}
}
else if (s == 'c')
{
global_calibration_flag = 3;
if (global_calibration_flag == 3)//MagConfig
{
*offset_set_dev = FEATURE_OFFSET_GET_MAG;
HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE);
UINT8 MAG_Cali_Last[FEATURE_OFFSET_SIZE] = { 0x0B };
MAG_Cali_Last[3] = FEATURE_OFFSET_GET_MAG;
//if (HidD_GetFeature(DM_HID_Read_HANDLE, MAG_Cali_Last, FEATURE_OFFSET_SIZE))
{
*offset_set_dev= FEATURE_OFFSET_SET_MAG;
float magdefault[6] = { 0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f };
memcpy(offset_set_data, magdefault, sizeof(float) * 6);
//if (HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE))
{
remove("data.txt");
fopen_s(&fpdata, "data.txt", "w");//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC>Խ<EFBFBD><D4BD><EFBFBD>д<EFBFBD>롣<EFBFBD><EBA1A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD>ڣ<EFBFBD><DAA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>١<EFBFBD>
if (fpdata != NULL)
{
global_Show = 3;
printf("<EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD>!\n");
//Sleep(100);
}
else
{
printf("<EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>ʧ<EFBFBD><EFBFBD>!\n");
global_calibration_flag = 0;
}
while (global_calibration_flag == 3)//<2F><>ʼ<EFBFBD><CABC>ȡMag<61><67><EFBFBD><EFBFBD>
{
printf(">>"); s = _getche(); printf("\n");
if (s == 'f' || s == 'c' || s == 'q')//<2F><><EFBFBD><EFBFBD>/ȡ<><C8A1>/<2F>˳<EFBFBD>
{
break;
}
}
}
global_Show = 0;
}
global_calibration_flag = 0;
if (fpdata != NULL)
{
CancelIo(fpdata);
fclose(fpdata);
}
if (s == 'f')//Finished
{
printf("д<EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽ<EFBFBD><EFBFBD><EFBFBD>,<2C>ȴ<EFBFBD><C8B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:\n");
float mag_new_cali[6];
system("pause");
MagCaliParam magcalitmp = GetMagDataFormFile();
mag_new_cali[0] = magcalitmp.xOffset;
mag_new_cali[1] = magcalitmp.yOffset;
mag_new_cali[2] = magcalitmp.zOffset;
mag_new_cali[3] = magcalitmp.xScale;
mag_new_cali[4] = magcalitmp.yScale;
mag_new_cali[5] = magcalitmp.zScale;
// for (int i = 0; i < 6; i++)
// {
// printf("<22><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>%d<><64><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:", i + 1);
// std::cin >> mag_str_tmp;
// mag_new_cali[i] = atof(mag_str_tmp);
// }
memcpy(offset_set_data, mag_new_cali, sizeof(float) * 6);
}
else
{
if (s == 'c')
printf("У׼ȡ<EFBFBD><EFBFBD>");
memcpy(FEATURE_OFFSET_Set, MAG_Cali_Last, FEATURE_OFFSET_SIZE);
}
printf("\n");
*offset_set_dev = FEATURE_OFFSET_SET_MAG;
if (HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE))
{
if (s == 'c')
printf("<EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD>\n");
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡһ<C8A1><D2BB>
printf("Get\n");
Sleep(100);
*offset_set_dev = FEATURE_OFFSET_GET_MAG;
HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE);
*offset_get_dev = FEATURE_OFFSET_GET_MAG;
if (HidD_GetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Get, FEATURE_OFFSET_SIZE))
{
printf("<EFBFBD><EFBFBD>ǰУ׼<EFBFBD><EFBFBD><EFBFBD><EFBFBD>:\n");
for (int i = 0; i < 6; i++)
{
printf("%f ", offset_get_data[i]);
}
printf("\n");
}
}
}
}
else if (s == 'p')
{
printf("<EFBFBD>ܵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>\n");
printf(" - r - <20><><EFBFBD><EFBFBD><EFBFBD>ܵ<EFBFBD><DCB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>\n");
printf(" - s - <20>رչܵ<D5B9><DCB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>\n");
printf(">>"); s = _getche(); printf("\n");
if (s == 'r')
{
if (global_pipeserver == 0)
{
global_pipeserver = 1;
printf("<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.....\n");
start_thread(pipeservermain, NULL);
}
else
{
printf("<EFBFBD>ܵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>...\n");
}
}
else if (s == 's')
{
if (global_pipeserver == 0)
{
printf("<EFBFBD>ܵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>δ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>...\n");
}
else
{
global_pipeserver = -1;
}
}
}
}
printf("<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ⰴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼУ׼...\n");
getchar();
printf("<EFBFBD><EFBFBD>ʼд<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>...\n");
while (1)
{
}
}
//win<69><6E>Ϣ
#include <Dbt.h>
BOOL bIsRunning = TRUE;
//transform
#include <algorithm>
#pragma region HID
HDEVINFO device_info_set;
SP_DEVICE_INTERFACE_DATA device_interface_data;
HIDP_CAPS dematrix_device_capabilities;
GUID HidGuid;
BOOL InitPathFromInterfaceData(HDEVINFO hdevInfoSet, SP_DEVICE_INTERFACE_DATA* pidata)
{
DWORD detailSize = 0;
BOOL Result;
Result = SetupDiGetDeviceInterfaceDetail(hdevInfoSet, pidata, NULL, (DWORD)NULL, &detailSize, NULL);
pData = (PSP_DEVICE_INTERFACE_DETAIL_DATA)malloc(detailSize);
if (pData == NULL){
printf("<EFBFBD>ڴ治<EFBFBD>㣡\n");
SetupDiDestroyDeviceInfoList(hdevInfoSet);
return FALSE;
}
pData->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA);
Result = SetupDiGetDeviceInterfaceDetail(hdevInfoSet, pidata, pData, detailSize, NULL, NULL);
if (Result == TRUE)
{
//printf("Path = %s\n", (char *)(pData->DevicePath));
return TRUE;
}
return FALSE;
}
BOOL DM_HIDDeviceEnumerate(void)
{
HDEVINFO hdevInfoSet;
SP_DEVICE_INTERFACE_DATA interfaceData;
int gamepadcount = 0;
int deviceIndex = 0;
interfaceData.cbSize = sizeof(interfaceData);
/* <20><>ȡ<EFBFBD><C8A1>GUID */
HidD_GetHidGuid(&HidGuid);
hdevInfoSet = SetupDiGetClassDevs(&HidGuid, NULL, NULL, DIGCF_INTERFACEDEVICE | DIGCF_PRESENT);
if (hdevInfoSet == INVALID_HANDLE_VALUE) return FALSE;
for (deviceIndex = 0;
SetupDiEnumDeviceInterfaces(hdevInfoSet, NULL, &HidGuid, deviceIndex, &interfaceData);
deviceIndex++)
{
if (!InitPathFromInterfaceData(hdevInfoSet, &interfaceData))
continue;
//printf("index = %d\n",deviceIndex);
string pathstr(pData->DevicePath);
transform(pathstr.begin(), pathstr.end(), pathstr.begin(), toupper);
std::size_t found = pathstr.find(strGlobalDevicePath);
if (found != std::string::npos)
{
// printf("Path = %s\n", pathstr.c_str());
sCurrent_Device_str = pathstr;
//printf("sCurrent_Device_str:\n %s\n", sCurrent_Device_str.c_str());
DM_HID_Read_HANDLE = CreateFileA((char *)(pData->DevicePath), (DWORD)GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ |
FILE_SHARE_WRITE, NULL, OPEN_EXISTING,
FILE_ATTRIBUTE_NORMAL | FILE_FLAG_OVERLAPPED, NULL);
DM_HID_Write_HANDLE = CreateFileA((char *)(pData->DevicePath), GENERIC_WRITE, FILE_SHARE_READ | FILE_SHARE_WRITE,
(LPSECURITY_ATTRIBUTES)NULL, OPEN_EXISTING, 0, NULL);
if (DM_HID_Read_HANDLE != INVALID_HANDLE_VALUE)
{
device_is_found = TRUE;
SetEvent(usb_read_over_lapped.hEvent);
printf("read<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD>ɹ<EFBFBD><EFBFBD><EFBFBD>\n>>");
}
else
{
printf("read<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>豸ʧ<EFBFBD>ܣ<EFBFBD>\n>>");
}
if (DM_HID_Write_HANDLE != INVALID_HANDLE_VALUE)
{
printf("write<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD>ɹ<EFBFBD><EFBFBD><EFBFBD>\n>>");
}
else
{
printf("write<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>豸ʧ<EFBFBD>ܣ<EFBFBD>\n>>");
}
break;
}
else
continue;
}
SetupDiDestroyDeviceInfoList(hdevInfoSet);
if (device_is_found == TRUE)return TRUE;
else
return FALSE;
}
#pragma endregion
#pragma region WindowsMessage
HWND hMessageWindow;
HDEVNOTIFY hDeviceNotify;
bool MessageCallback(WORD messageType, const std::string& devicePath)
{
bool rv = true;
if (messageType == DBT_DEVICEARRIVAL)
{
//rv = pNotificationClient->OnMessage(Notifier::DeviceAdded, devicePath);
if (!device_is_found)
{
printf("<EFBFBD><EFBFBD><EFBFBD>⵽<EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD>ڳ<EFBFBD>ʼ<EFBFBD><CABC>!\n>>");
DM_HIDDeviceEnumerate();
rv = device_is_found;
//TriggerHaptic(1, 50);
//TriggerHaptic(1, 100);
}
else
{
printf("<EFBFBD><EFBFBD><EFBFBD>⵽<EFBFBD><EFBFBD><EFBFBD><EFBFBD>VID PID <20><>ͬ<EFBFBD><CDAC><EFBFBD>豸,<2C><><EFBFBD>豸<EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD>ʼ<EFBFBD><CABC>!\n>>");
}
}
else if (messageType == DBT_DEVICEREMOVECOMPLETE)
{
//pNotificationClient->OnMessage(Notifier::DeviceRemoved, devicePath);
//printf("%s\n%s\n", devicePath.c_str(), sCurrent_Device_str.c_str());
if (device_is_found)
{
//found = devicePath.find(sCurrent_Device_str);
if (devicePath.find(sCurrent_Device_str) != std::string::npos)
{
ResetAHRS();
device_is_found = FALSE;
sCurrent_Device_str = "NULL";
if (DM_HID_Read_HANDLE != INVALID_HANDLE_VALUE)
{
CloseHandle(DM_HID_Read_HANDLE);
DM_HID_Read_HANDLE = INVALID_HANDLE_VALUE;
}
if (DM_HID_Write_HANDLE != INVALID_HANDLE_VALUE)
{
CloseHandle(DM_HID_Write_HANDLE);
DM_HID_Write_HANDLE = INVALID_HANDLE_VALUE;
}
printf("<EFBFBD>豸<EFBFBD>Ѱγ<EFBFBD>!\n>>");
}
else
{
printf("<EFBFBD>γ<EFBFBD><EFBFBD>˾<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬ<EFBFBD><EFBFBD>vid pid<69><64><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE>򿪵<EFBFBD><F2BFAAB5>豸!\n>>");
}
}
}
else
{
}
return rv;
}
LRESULT CALLBACK WindowsMessageCallback(HWND hwnd,
UINT message,
WPARAM wParam,
LPARAM lParam)
{
switch (message)
{
case WM_CREATE:
{
// Setup window user data with device status object pointer.
LPCREATESTRUCT create_struct = reinterpret_cast<LPCREATESTRUCT>(lParam);
void *lpCreateParam = create_struct->lpCreateParams;
//DeviceStatus *pDeviceStatus = reinterpret_cast<DeviceStatus*>(lpCreateParam);
//printf("create\n");
bIsRunning = TRUE;
//SetWindowLongPtr(hwnd, GWLP_USERDATA, reinterpret_cast<LONG_PTR>(pDeviceStatus));
}
return 0; // Return 0 for successfully handled WM_CREATE.
case WM_DEVICECHANGE:
{
WORD loword = LOWORD(wParam);
if (loword != DBT_DEVICEARRIVAL &&
loword != DBT_DEVICEREMOVECOMPLETE)
{
// Ignore messages other than device arrive and remove complete
// (we're not handling intermediate ones).
return TRUE; // Grant WM_DEVICECHANGE request.
}
DEV_BROADCAST_DEVICEINTERFACE* hdr;
hdr = (DEV_BROADCAST_DEVICEINTERFACE*)lParam;
if (hdr->dbcc_devicetype != DBT_DEVTYP_DEVICEINTERFACE)
{
// Ignore non interface device messages.
return TRUE; // Grant WM_DEVICECHANGE request.
}
//LONG_PTR userData = GetWindowLongPtr(hwnd, GWLP_USERDATA);
//DeviceStatus* pDeviceStatus = (DeviceStatus*)userData;
string devicePath(hdr->dbcc_name);
if (/*pDeviceStatus->*/HidGuid == hdr->dbcc_classguid)
{
transform(devicePath.begin(), devicePath.end(), devicePath.begin(), toupper);
std::string str2(strGlobalDevicePath);
//printf("devicePath:%s\n", devicePath.c_str());
std::size_t found = devicePath.find(str2);
if (found != std::string::npos)
{
if (!MessageCallback(loword, devicePath))
{
}
break;
}
}
}
return TRUE; // Grant WM_DEVICECHANGE request.
case WM_CLOSE:
{
//LONG_PTR userData = GetWindowLongPtr(hwnd, GWLP_USERDATA);
bIsRunning = FALSE;
DestroyWindow(hwnd);
}
return 0; // We processed the WM_CLOSE message.
case WM_DESTROY:
PostQuitMessage(0);
return 0; // We processed the WM_DESTROY message.
}
return DefWindowProc(hwnd, message, wParam, lParam);
}
bool WinProcInitialize()
{
TCHAR special_name[] = TEXT("wnd_class_name_usbDMCali_JustFeng_2017");
WNDCLASS wndClass;
wndClass.style = CS_HREDRAW | CS_VREDRAW;
wndClass.lpfnWndProc = WindowsMessageCallback;
wndClass.cbClsExtra = 0;
wndClass.cbWndExtra = 0;
wndClass.hInstance = 0;
wndClass.hIcon = LoadIcon(NULL, IDI_APPLICATION);
wndClass.hCursor = LoadCursor(NULL, IDC_ARROW);
wndClass.hbrBackground = (HBRUSH)(COLOR_WINDOW + 1);
wndClass.lpszMenuName = NULL;
wndClass.lpszClassName = special_name;
if (!RegisterClass(&wndClass))
{
printf("Failed to register window class.");
return false;
}
// We're going to create a 'message-only' window. This will be hidden, can't be enumerated etc.
// To do this we supply 'HWND_MESSAGE' as the hWndParent.
// http://msdn.microsoft.com/en-us/library/ms632599%28VS.85%29.aspx#message_only
hMessageWindow = CreateWindow(special_name,
special_name,
WS_OVERLAPPEDWINDOW,
CW_USEDEFAULT,
CW_USEDEFAULT,
CW_USEDEFAULT,
CW_USEDEFAULT,
HWND_MESSAGE,
NULL,
0,
NULL); // Pass this object via the CREATESTRUCT mechanism
// so that we can attach it to the window user data.
if (hMessageWindow == NULL)
{
printf("Failed to create window.");
return false;
}
// According to MS, topmost windows receive WM_DEVICECHANGE faster.
::SetWindowPos(hMessageWindow, HWND_TOPMOST, 0, 0, 0, 0, SWP_NOMOVE | SWP_NOSIZE);
UpdateWindow(hMessageWindow);
// Register notification for additional HID messages.
HidD_GetHidGuid(&HidGuid);
DEV_BROADCAST_DEVICEINTERFACE notificationFilter;
ZeroMemory(&notificationFilter, sizeof(notificationFilter));
notificationFilter.dbcc_size = sizeof(DEV_BROADCAST_DEVICEINTERFACE);
notificationFilter.dbcc_devicetype = DBT_DEVTYP_DEVICEINTERFACE;
//notificationFilter.dbcc_classguid = hidguid;
// We need DEVICE_NOTIFY_ALL_INTERFACE_CLASSES to detect
// HDMI plug/unplug events.
hDeviceNotify = RegisterDeviceNotification(
hMessageWindow,
&notificationFilter,
DEVICE_NOTIFY_ALL_INTERFACE_CLASSES | DEVICE_NOTIFY_WINDOW_HANDLE);
if (hDeviceNotify == NULL)
{
printf("Failed to register for device notifications.");
return false;
}
return true;
}
void ProcessMessages()
{
//printf("MSG\n");
if (hMessageWindow == NULL)
printf("Need to call 'Initialize' before first use.");
MSG msg;
// Note WM_DEVICECHANGED messages are dispatched but not retrieved by PeekMessage.
// I think this is because they are pending, non-queued messages.
while (PeekMessage(&msg, NULL, 0, 0, PM_REMOVE))
{
TranslateMessage(&msg);
DispatchMessage(&msg);
}
}
#pragma endregion
DWORD Thread_Win_MSG()
{
if (!WinProcInitialize())
{
bIsRunning = FALSE;
}
while (bIsRunning)
{
Sleep(1);
ProcessMessages();
}
return 0;
}
/* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
__declspec(dllexport) int Init(char* devpath)
{
USB_HID_Init();
strGlobalDevicePath = devpath;//"VID_2833&PID_0002";
globalpacklen = 64;
start_thread(Thread_Win_MSG, 0);
DM_HIDDeviceEnumerate();
return 1;
}
__declspec(dllexport) int CheckDevice(void)
{
UINT8 FEATURE_OFFSET_Get[FEATURE_OFFSET_SIZE] = { 0x0b };
if (HidD_GetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Get, FEATURE_OFFSET_SIZE))
{
printf("<EFBFBD>豸<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>...\n");
}
else
{
printf("<EFBFBD>豸<EFBFBD>Ͽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>...\n");
device_is_found = FALSE;
}
return device_is_found?1:0;
}
__declspec(dllexport) int GyroCali(void)
{
if (device_is_found)
{
UINT8 gyrocali[5] = { 0x08, 0x00, 0x00, 0x10, 0x26 };
if (HidD_SetFeature(DM_HID_Read_HANDLE, gyrocali, 5))
{
//printf("<22><><EFBFBD><EFBFBD>У׼<D0A3><D7BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>...<2E>뱣<EFBFBD><EBB1A3>ͷ<EFBFBD><CDB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>!\n");
//Sleep(1000);
return 1;
//printf("У׼<D0A3><D7BC><EFBFBD><EFBFBD><EFBFBD>dzɹ<C7B3>\n");
}
}
return 0;
}
__declspec(dllexport) Vector3 GetGyroCali(void)
{
Vector3 v;
v.x = v.y = v.z = -99999.0f;
if (!device_is_found)
return v;
UINT8 FEATURE_OFFSET_Get[FEATURE_OFFSET_SIZE] = { 0x0b };
UINT8 FEATURE_OFFSET_Set[FEATURE_OFFSET_SIZE] = { 0x0b };
UINT8* offset_get_dev = (UINT8*)(FEATURE_OFFSET_Get + 3);
float* offset_get_data = (float*)(FEATURE_OFFSET_Get + 4);
UINT8* offset_set_dev = (UINT8*)(FEATURE_OFFSET_Set + 3);
float* offset_set_data = (float*)(FEATURE_OFFSET_Set + 4);
*offset_set_dev = *offset_get_dev = 0;
memset(offset_get_data, 0, FEATURE_OFFSET_SIZE - 4);
memset(offset_set_data, 0, FEATURE_OFFSET_SIZE - 4);
*offset_set_dev = FEATURE_OFFSET_GET_GYRO;
HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE);
*offset_get_dev = FEATURE_OFFSET_GET_GYRO;
if (HidD_GetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Get, FEATURE_OFFSET_SIZE))
{
v.x = offset_get_data[0];
v.y = offset_get_data[1];
v.z = offset_get_data[2];
}
return v;
}
__declspec(dllexport) MagCaliParam GetMagCali()
{
MagCaliParam mcp;
UINT8 FEATURE_OFFSET_Get[FEATURE_OFFSET_SIZE] = { 0x0b };
UINT8 FEATURE_OFFSET_Set[FEATURE_OFFSET_SIZE] = { 0x0b };
UINT8* offset_get_dev = (UINT8*)(FEATURE_OFFSET_Get + 3);
float* offset_get_data = (float*)(FEATURE_OFFSET_Get + 4);
UINT8* offset_set_dev = (UINT8*)(FEATURE_OFFSET_Set + 3);
float* offset_set_data = (float*)(FEATURE_OFFSET_Set + 4);
*offset_set_dev = FEATURE_OFFSET_GET_MAG;
HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE);
*offset_get_dev = FEATURE_OFFSET_GET_MAG;
if (HidD_GetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Get, FEATURE_OFFSET_SIZE))
{
mcp.xOffset = offset_get_data[0];
mcp.yOffset = offset_get_data[1];
mcp.zOffset = offset_get_data[2];
mcp.xScale = offset_get_data[3];
mcp.yScale = offset_get_data[4];
mcp.zScale = offset_get_data[5];
printf("<EFBFBD><EFBFBD>ǰУ׼<EFBFBD><EFBFBD><EFBFBD><EFBFBD>:\n");
for (int i = 0; i < 6; i++)
{
printf("%f ", offset_get_data[i]);
}
printf("\n");
}
return mcp;
}
__declspec(dllexport) int SetMagCali(float x, float y, float z, float x0, float y0, float z0)
{
UINT8 FEATURE_OFFSET_Set[FEATURE_OFFSET_SIZE] = { 0x0b };
UINT8* offset_set_dev = (UINT8*)(FEATURE_OFFSET_Set + 3);
float* offset_set_data = (float*)(FEATURE_OFFSET_Set + 4);
*offset_set_dev = FEATURE_OFFSET_SET_MAG;
float magdefault[6] = { x, y, z, x0, y0, z0 };
memcpy(offset_set_data, magdefault, sizeof(float) * 6);
if (HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE))
{
printf("SetMagOffset\n");
return 1;
}
return 0;
}
int Global_MagCali = 0;
__declspec(dllexport) int MagCalibrate(void)
{
if (global_calibration_flag == 3)
{
return 0;
}
UINT8 FEATURE_OFFSET_Get[FEATURE_OFFSET_SIZE] = { 0x0b };
UINT8 FEATURE_OFFSET_Set[FEATURE_OFFSET_SIZE] = { 0x0b };
UINT8* offset_get_dev = (UINT8*)(FEATURE_OFFSET_Get + 3);
float* offset_get_data = (float*)(FEATURE_OFFSET_Get + 4);
UINT8* offset_set_dev = (UINT8*)(FEATURE_OFFSET_Set + 3);
float* offset_set_data = (float*)(FEATURE_OFFSET_Set + 4);
DWORD checkhandle;
global_calibration_flag = 3;
if (global_calibration_flag == 3)//MagConfig
{
*offset_set_dev = FEATURE_OFFSET_GET_MAG;
HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE);
UINT8 MAG_Cali_Last[FEATURE_OFFSET_SIZE] = { 0x0B };
MAG_Cali_Last[3] = FEATURE_OFFSET_GET_MAG;
if (HidD_GetFeature(DM_HID_Read_HANDLE, MAG_Cali_Last, FEATURE_OFFSET_SIZE))
{
*offset_set_dev = FEATURE_OFFSET_SET_MAG;
if (SetMagCali(0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f))
{
return 1;
}
global_Show = 0;
}
global_calibration_flag = 0;
return 0;
}
}
__declspec(dllexport) int FinishMagCali(void)
{
global_calibration_flag = 0;
global_Show = 0;
return 1;
}
__declspec(dllexport) MagCaliParam CalculateMagParam()
{
return GetMagDataFormFile();
}
/* <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ */
__declspec(dllexport) Vector3 GetPoint(void)
{
if (!device_is_found){
Vector3 v;
v.x = v.y = v.z = -99999.0f;
return v;
}
return GetMagData();
}
__declspec(dllexport) Vector3 GetEulerianAngle(void)
{
Vector3 v;
v.x = v.y = v.z = -99999.0f;
if (device_is_found)
{
v.z = DM_GetYaw();
v.x = DM_GetPitch();
v.y = DM_GetRoll();
}
return v;
}
__declspec(dllexport) int Correction(void)
{
return ResetHeading();
}
__declspec(dllexport) Quaternion GetQuaternion(void)
{
return GetPoseQuat();
}