void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len) {
/* send request to wiimote for accelerometer calibration */
byte buf[MAX_PAYLOAD];
/* step 0 - Reset wiimote */
{
//wiiuse_set_leds(wm, WIIMOTE_LED_NONE);
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_CONNECTED);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_ACC);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_RUMBLE);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP);
WIIMOTE_DISABLE_FLAG(wm, WIIUSE_CONTINUOUS);
wiiuse_set_report_type(wm);
wiiuse_millisleep(500);
WIIUSE_DEBUG("Wiimote reset!\n");
}
/* step 1 - calibration of accelerometers */
if(wm->type != WIIUSE_WIIMOTE_MOTION_PLUS_INSIDE)
{
struct accel_t* accel = &wm->accel_calib;
wiiuse_read_data_sync(wm, 1, WM_MEM_OFFSET_CALIBRATION, 8, buf);
/* received read data */
accel->cal_zero.x = buf[0];
accel->cal_zero.y = buf[1];
accel->cal_zero.z = buf[2];
accel->cal_g.x = buf[4] - accel->cal_zero.x;
accel->cal_g.y = buf[5] - accel->cal_zero.y;
accel->cal_g.z = buf[6] - accel->cal_zero.z;
WIIUSE_DEBUG("Calibrated wiimote acc\n");
}
/* step 2 - re-enable IR and ask for status */
{
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE_COMPLETE);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
/* now enable IR if it was set before the handshake completed */
if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR)) {
WIIUSE_DEBUG("Handshake finished, enabling IR.");
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR);
wiiuse_set_ir(wm, 1);
}
WIIUSE_DEBUG("Asking for status ...\n");
wm->event = WIIUSE_CONNECT;
wiiuse_status(wm);
}
}
/**
* @brief Set if the wiimote should report motion sensing.
*
* @param wm Pointer to a wiimote_t structure.
* @param status 1 to enable, 0 to disable.
*
* Since reporting motion sensing sends a lot of data,
* the wiimote saves power by not transmitting it
* by default.
*/
void wiiuse_motion_sensing(struct wiimote_t* wm, int status) {
if (status) {
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_ACC);
} else {
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_ACC);
}
wiiuse_set_report_type(wm);
}
/**
* @brief Switch the Balance Board to use report 0x34 instead of 0x32
*
* @param wm Pointer to a wiimote_t structure.
* @param enabled enabled != 0 -> use report 0x32
*/
void wiiuse_wiiboard_use_alternate_report(struct wiimote_t *wm, int enabled)
{
if(wm->exp.type == EXP_WII_BOARD)
{
wm->exp.wb.use_alternate_report = enabled;
wiiuse_set_report_type(wm);
}
else
printf("Alternate report can be set only on a Balance Board!\n");
}
int wiiuse_os_find(struct wiimote_t** wm, int max_wiimotes, int timeout) {
GUID device_id;
HANDLE dev;
HDEVINFO device_info;
int i, index;
DWORD len;
SP_DEVICE_INTERFACE_DATA device_data;
PSP_DEVICE_INTERFACE_DETAIL_DATA detail_data = NULL;
HIDD_ATTRIBUTES attr;
int found = 0;
(void) timeout; /* unused */
device_data.cbSize = sizeof(device_data);
index = 0;
/* get the device id */
HidD_GetHidGuid(&device_id);
/* get all hid devices connected */
device_info = SetupDiGetClassDevs(&device_id, NULL, NULL, (DIGCF_DEVICEINTERFACE | DIGCF_PRESENT));
for (;; ++index) {
if (detail_data) {
free(detail_data);
detail_data = NULL;
}
/* query the next hid device info */
if (!SetupDiEnumDeviceInterfaces(device_info, NULL, &device_id, index, &device_data)) {
break;
}
/* get the size of the data block required */
i = SetupDiGetDeviceInterfaceDetail(device_info, &device_data, NULL, 0, &len, NULL);
detail_data = (SP_DEVICE_INTERFACE_DETAIL_DATA_A*)malloc(len);
detail_data->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA);
/* query the data for this device */
if (!SetupDiGetDeviceInterfaceDetail(device_info, &device_data, detail_data, len, NULL, NULL)) {
continue;
}
/* open the device */
dev = CreateFile(detail_data->DevicePath,
(GENERIC_READ | GENERIC_WRITE),
(FILE_SHARE_READ | FILE_SHARE_WRITE),
NULL, OPEN_EXISTING, FILE_FLAG_OVERLAPPED, NULL);
if (dev == INVALID_HANDLE_VALUE) {
continue;
}
/* get device attributes */
attr.Size = sizeof(attr);
i = HidD_GetAttributes(dev, &attr);
if ((attr.VendorID == WM_VENDOR_ID) && (attr.ProductID == WM_PRODUCT_ID)) {
/* this is a wiimote */
wm[found]->dev_handle = dev;
wm[found]->hid_overlap.hEvent = CreateEvent(NULL, 1, 1, "");
wm[found]->hid_overlap.Offset = 0;
wm[found]->hid_overlap.OffsetHigh = 0;
WIIMOTE_ENABLE_STATE(wm[found], WIIMOTE_STATE_DEV_FOUND);
WIIMOTE_ENABLE_STATE(wm[found], WIIMOTE_STATE_CONNECTED);
/* try to set the output report to see if the device is actually connected */
if (!wiiuse_set_report_type(wm[found])) {
WIIMOTE_DISABLE_STATE(wm[found], WIIMOTE_STATE_CONNECTED);
continue;
}
/* do the handshake */
wiiuse_handshake(wm[found], NULL, 0);
WIIUSE_INFO("Connected to wiimote [id %i].", wm[found]->unid);
++found;
if (found >= max_wiimotes) {
break;
}
} else {
/* not a wiimote */
CloseHandle(dev);
}
}
if (detail_data) {
free(detail_data);
}
SetupDiDestroyDeviceInfoList(device_info);
return found;
}
/**
* @brief Connect to a wiimote with a known address.
*
* @param wm Pointer to a wiimote_t structure.
* @param address The address of the device to connect to.
* If NULL, use the address in the struct set by wiiuse_find().
*
* @return 1 on success, 0 on failure
*/
static int wiiuse_connect_single(struct wiimote_t* wm, char* address) {
struct sockaddr_l2 addr;
memset(&addr, 0, sizeof (addr));
if (!wm || WIIMOTE_IS_CONNECTED(wm))
return 0;
addr.l2_family = AF_BLUETOOTH;
if (address)
/* use provided address */
str2ba(address, &addr.l2_bdaddr);
else
/* use address of device discovered */
addr.l2_bdaddr = wm->bdaddr;
/*
* OUTPUT CHANNEL
*/
wm->out_sock = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
if (wm->out_sock == -1)
return 0;
addr.l2_psm = htobs(WM_OUTPUT_CHANNEL);
/* connect to wiimote */
if (connect(wm->out_sock, (struct sockaddr*)&addr, sizeof(addr)) < 0) {
perror("connect() output sock");
return 0;
}
/*
* INPUT CHANNEL
*/
wm->in_sock = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
if (wm->in_sock == -1) {
close(wm->out_sock);
wm->out_sock = -1;
return 0;
}
addr.l2_psm = htobs(WM_INPUT_CHANNEL);
/* connect to wiimote */
if (connect(wm->in_sock, (struct sockaddr*)&addr, sizeof(addr)) < 0) {
perror("connect() interrupt sock");
close(wm->out_sock);
wm->out_sock = -1;
return 0;
}
WIIUSE_INFO("Connected to wiimote [id %i].", wm->unid);
/* do the handshake */
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_CONNECTED);
wiiuse_handshake(wm, NULL, 0);
wiiuse_set_report_type(wm);
return 1;
}
/**
* @brief Set if the wiimote should track IR targets.
*
* @param wm Pointer to a wiimote_t structure.
* @param status 1 to enable, 0 to disable.
*/
void wiiuse_set_ir(struct wiimote_t* wm, int status) {
byte buf;
char* block1 = NULL;
char* block2 = NULL;
int ir_level;
if (!wm)
return;
/*
* Wait for the handshake to finish first.
* When it handshake finishes and sees that
* IR is enabled, it will call this function
* again to actually enable IR.
*/
if (!WIIMOTE_IS_SET(wm, WIIMOTE_STATE_HANDSHAKE_COMPLETE)) {
if(status) {
WIIUSE_DEBUG("Tried to enable IR, will wait until handshake finishes.");
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR);
} // else ignoring request to turn off, since it's turned off by default
return;
}
/*
* Check to make sure a sensitivity setting is selected.
*/
ir_level = get_ir_sens(wm, &block1, &block2);
if (!ir_level) {
WIIUSE_ERROR("No IR sensitivity setting selected.");
return;
}
if (status) {
/* if already enabled then stop */
if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR))
return;
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR);
} else {
/* if already disabled then stop */
if (!WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR))
return;
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR);
}
/* set camera 1 and 2 */
buf = (status ? 0x04 : 0x00);
wiiuse_send(wm, WM_CMD_IR, &buf, 1);
wiiuse_send(wm, WM_CMD_IR_2, &buf, 1);
if (!status) {
WIIUSE_DEBUG("Disabled IR cameras for wiimote id %i.", wm->unid);
wiiuse_set_report_type(wm);
return;
}
/* enable IR, set sensitivity */
buf = 0x08;
wiiuse_write_data(wm, WM_REG_IR, &buf, 1);
/* wait for the wiimote to catch up */
#ifndef WIN32
usleep(50000);
#else
Sleep(50);
#endif
/* write sensitivity blocks */
wiiuse_write_data(wm, WM_REG_IR_BLOCK1, (byte*)block1, 9);
wiiuse_write_data(wm, WM_REG_IR_BLOCK2, (byte*)block2, 2);
/* set the IR mode */
if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_EXP))
buf = WM_IR_TYPE_BASIC;
else
buf = WM_IR_TYPE_EXTENDED;
wiiuse_write_data(wm, WM_REG_IR_MODENUM, &buf, 1);
#ifndef WIN32
usleep(50000);
#else
Sleep(50);
#endif
/* set the wiimote report type */
wiiuse_set_report_type(wm);
WIIUSE_DEBUG("Enabled IR camera for wiimote id %i (sensitivity level %i).", wm->unid, ir_level);
}
void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len) {
if (!wm) {
return;
}
switch (wm->handshake_state) {
case 0: {
byte* buf;
/* continuous reporting off, report to buttons only */
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
wiiuse_set_leds(wm, WIIMOTE_LED_NONE);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_ACC);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_RUMBLE);
WIIMOTE_DISABLE_FLAG(wm, WIIUSE_CONTINUOUS);
wiiuse_set_report_type(wm);
/* send request to wiimote for accelerometer calibration */
buf = (byte*)malloc(sizeof(byte) * 8);
wiiuse_read_data_cb(wm, wiiuse_handshake, buf, WM_MEM_OFFSET_CALIBRATION, 7);
wm->handshake_state++;
wiiuse_set_leds(wm, WIIMOTE_LED_NONE);
break;
}
case 1: {
struct read_req_t* req = wm->read_req;
struct accel_t* accel = &wm->accel_calib;
byte val;
/* received read data */
accel->cal_zero.x = req->buf[0];
accel->cal_zero.y = req->buf[1];
accel->cal_zero.z = req->buf[2];
accel->cal_g.x = req->buf[4] - accel->cal_zero.x;
accel->cal_g.y = req->buf[5] - accel->cal_zero.y;
accel->cal_g.z = req->buf[6] - accel->cal_zero.z;
/* done with the buffer */
free(req->buf);
/* handshake is done */
WIIUSE_DEBUG("Handshake finished. Calibration: Idle: X=%x Y=%x Z=%x\t+1g: X=%x Y=%x Z=%x",
accel->cal_zero.x, accel->cal_zero.y, accel->cal_zero.z,
accel->cal_g.x, accel->cal_g.y, accel->cal_g.z);
/* M+ off */
val = 0x55;
wiiuse_write_data_cb(wm, WM_EXP_MEM_ENABLE1, &val, 1, wiiuse_disable_motion_plus1);
break;
}
case 2: {
/* request the status of the wiimote to check for any expansion */
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE_COMPLETE);
wm->handshake_state++;
/* now enable IR if it was set before the handshake completed */
if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR)) {
WIIUSE_DEBUG("Handshake finished, enabling IR.");
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR);
wiiuse_set_ir(wm, 1);
}
wm->event = WIIUSE_CONNECT;
wiiuse_status(wm);
break;
}
default: {
break;
}
}
}
