1810 lines
46 KiB
C++
1810 lines
46 KiB
C++
#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(¬ificationFilter, 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,
|
||
¬ificationFilter,
|
||
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();
|
||
}
|