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);
}
}
void wiiuse_handshake_expansion(struct wiimote_t *wm,ubyte *data,uword len)
{
int id;
ubyte val;
ubyte *buf = NULL;
switch(wm->expansion_state) {
/* These two initialization writes disable the encryption */
case 0:
wm->expansion_state = 1;
val = 0x55;
wiiuse_write_data(wm,WM_EXP_MEM_ENABLE1,&val,1,wiiuse_handshake_expansion);
break;
case 1:
wm->expansion_state = 2;
val = 0x00;
wiiuse_write_data(wm,WM_EXP_MEM_ENABLE2,&val,1,wiiuse_handshake_expansion);
break;
case 2:
wm->expansion_state = 3;
buf = __lwp_wkspace_allocate(sizeof(ubyte)*EXP_HANDSHAKE_LEN);
wiiuse_read_data(wm,buf,WM_EXP_MEM_CALIBR,EXP_HANDSHAKE_LEN,wiiuse_handshake_expansion);
break;
case 3:
if(!data || !len) return;
id = BIG_ENDIAN_LONG(*(int*)(&data[220]));
switch(id) {
case EXP_ID_CODE_NUNCHUK:
if(!nunchuk_handshake(wm,&wm->exp.nunchuk,data,len)) return;
break;
case EXP_ID_CODE_CLASSIC_CONTROLLER:
if(!classic_ctrl_handshake(wm,&wm->exp.classic,data,len)) return;
break;
case EXP_ID_CODE_GUITAR:
if(!guitar_hero_3_handshake(wm,&wm->exp.gh3,data,len)) return;
break;
case EXP_ID_CODE_WIIBOARD:
if(!wii_board_handshake(wm,&wm->exp.wb,data,len)) return;
break;
default:
WIIMOTE_DISABLE_STATE(wm,WIIMOTE_STATE_EXP_HANDSHAKE);
WIIMOTE_ENABLE_STATE(wm,WIIMOTE_STATE_EXP_FAILED);
__lwp_wkspace_free(data);
wiiuse_status(wm,NULL);
return;
}
__lwp_wkspace_free(data);
WIIMOTE_DISABLE_STATE(wm,WIIMOTE_STATE_EXP_HANDSHAKE);
WIIMOTE_ENABLE_STATE(wm,WIIMOTE_STATE_EXP);
wiiuse_set_ir_mode(wm);
wiiuse_status(wm,NULL);
break;
}
}
void wiiuse_handshake(struct wiimote_t *wm,ubyte *data,uword len)
{
ubyte *buf = NULL;
struct accel_t *accel = &wm->accel_calib;
//printf("wiiuse_handshake(%d,%p,%d)\n",wm->handshake_state,data,len);
switch(wm->handshake_state) {
case 0:
wm->handshake_state++;
wiiuse_set_leds(wm,WIIMOTE_LED_NONE,NULL);
wiiuse_status(wm,wiiuse_handshake);
return;
case 1:
wm->handshake_state++;
buf = __lwp_wkspace_allocate(sizeof(ubyte)*8);
if (len > 2 && data[2]&WM_CTRL_STATUS_BYTE1_ATTACHMENT) {
wiiuse_read_data(wm,buf,WM_EXP_ID,6,wiiuse_handshake);
return;
case 2:
if (BIG_ENDIAN_LONG(*(int*)(&data[2])) == EXP_ID_CODE_CLASSIC_WIIU_PRO) {
memset(data, 0, 8);
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_WIIU_PRO);
break;
}
buf = data;
}
wm->handshake_state++;
wiiuse_read_data(wm,buf,WM_MEM_OFFSET_CALIBRATION,7,wiiuse_handshake);
return;
}
accel->cal_zero.x = ((data[0]<<2)|((data[3]>>4)&3));
accel->cal_zero.y = ((data[1]<<2)|((data[3]>>2)&3));
accel->cal_zero.z = ((data[2]<<2)|(data[3]&3));
accel->cal_g.x = (((data[4]<<2)|((data[7]>>4)&3)) - accel->cal_zero.x);
accel->cal_g.y = (((data[5]<<2)|((data[7]>>2)&3)) - accel->cal_zero.y);
accel->cal_g.z = (((data[6]<<2)|(data[7]&3)) - accel->cal_zero.z);
__lwp_wkspace_free(data);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE_COMPLETE);
wm->event = WIIUSE_CONNECT;
wiiuse_status(wm,NULL);
}
/**
* @brief Enable or disable the rumble.
*
* @param wm Pointer to a wiimote_t structure.
* @param status 1 to enable, 0 to disable.
*/
void wiiuse_rumble(struct wiimote_t* wm, int status) {
byte buf;
if (!wm || !WIIMOTE_IS_CONNECTED(wm)) {
return;
}
/* make sure to keep the current lit leds */
buf = wm->leds;
if (status) {
WIIUSE_DEBUG("Starting rumble...");
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_RUMBLE);
buf |= 0x01;
} else {
WIIUSE_DEBUG("Stopping rumble...");
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_RUMBLE);
buf &= ~(0x01);
}
/* preserve IR state */
if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR)) {
buf |= 0x04;
}
wiiuse_send(wm, WM_CMD_RUMBLE, &buf, 1);
}
/**
* @brief Find a wiimote or wiimotes.
*
* @param wm An array of wiimote_t structures.
* @param max_wiimotes The number of wiimote structures in \a wm.
* @param timeout The number of seconds before the search times out.
*
* @return The number of wiimotes found.
*
* @see wiimote_connect()
*
* This function will only look for wiimote devices. \n
* When a device is found the address in the structures will be set. \n
* You can then call wiimote_connect() to connect to the found \n
* devices.
*/
int wiic_find(struct wiimote_t** wm, int max_wiimotes, int timeout) {
int device_id;
int device_sock;
int found_devices;
int found_wiimotes;
/* reset all wiimote bluetooth device addresses */
for (found_wiimotes = 0; found_wiimotes < max_wiimotes; ++found_wiimotes)
wm[found_wiimotes]->bdaddr = *BDADDR_ANY;
found_wiimotes = 0;
/* get the id of the first bluetooth device. */
device_id = hci_get_route(NULL);
if (device_id < 0) {
perror("hci_get_route");
return 0;
}
/* create a socket to the device */
device_sock = hci_open_dev(device_id);
if (device_sock < 0) {
perror("hci_open_dev");
return 0;
}
inquiry_info scan_info_arr[128];
inquiry_info* scan_info = scan_info_arr;
memset(&scan_info_arr, 0, sizeof(scan_info_arr));
/* scan for bluetooth devices for 'timeout' seconds */
found_devices = hci_inquiry(device_id, timeout, 128, NULL, &scan_info, IREQ_CACHE_FLUSH);
if (found_devices < 0) {
perror("hci_inquiry");
return 0;
}
WIIC_INFO("Found %i bluetooth device(s).", found_devices);
int i = 0;
/* display discovered devices */
for (; (i < found_devices) && (found_wiimotes < max_wiimotes); ++i) {
if ((scan_info[i].dev_class[0] == WM_DEV_CLASS_0) &&
(scan_info[i].dev_class[1] == WM_DEV_CLASS_1) &&
(scan_info[i].dev_class[2] == WM_DEV_CLASS_2))
{
/* found a device */
ba2str(&scan_info[i].bdaddr, wm[found_wiimotes]->bdaddr_str);
WIIC_INFO("Found wiimote (%s) [id %i].", wm[found_wiimotes]->bdaddr_str, wm[found_wiimotes]->unid);
wm[found_wiimotes]->bdaddr = scan_info[i].bdaddr;
WIIMOTE_ENABLE_STATE(wm[found_wiimotes], WIIMOTE_STATE_DEV_FOUND);
++found_wiimotes;
}
}
close(device_sock);
return found_wiimotes;
}
void wiiuse_handshake_expansion_start(struct wiimote_t *wm)
{
if(WIIMOTE_IS_SET(wm,WIIMOTE_STATE_EXP) || WIIMOTE_IS_SET(wm,WIIMOTE_STATE_EXP_FAILED) || WIIMOTE_IS_SET(wm,WIIMOTE_STATE_EXP_HANDSHAKE))
return;
wm->expansion_state = 0;
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);
wiiuse_handshake_expansion(wm, NULL, 0);
}
/**
* @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 Set the IR sensitivity.
*
* @param wm Pointer to a wiimote_t structure.
* @param level 1-5, same as Wii system sensitivity setting.
*
* If the level is < 1, then level will be set to 1.
* If the level is > 5, then level will be set to 5.
*/
void wiiuse_set_ir_sensitivity(struct wiimote_t* wm, int level) {
char* block1 = NULL;
char* block2 = NULL;
if (!wm) return;
if (level > 5) level = 5;
if (level < 1) level = 1;
WIIMOTE_DISABLE_STATE(wm, (WIIMOTE_STATE_IR_SENS_LVL1 |
WIIMOTE_STATE_IR_SENS_LVL2 |
WIIMOTE_STATE_IR_SENS_LVL3 |
WIIMOTE_STATE_IR_SENS_LVL4 |
WIIMOTE_STATE_IR_SENS_LVL5));
switch (level) {
case 1:
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR_SENS_LVL1);
break;
case 2:
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR_SENS_LVL2);
break;
case 3:
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR_SENS_LVL3);
break;
case 4:
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR_SENS_LVL4);
break;
case 5:
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR_SENS_LVL5);
break;
default:
return;
}
/* set the new sensitivity */
get_ir_sens(wm, &block1, &block2);
wiiuse_write_data(wm, WM_REG_IR_BLOCK1, (byte*)block1, 9);
wiiuse_write_data(wm, WM_REG_IR_BLOCK2, (byte*)block2, 2);
WIIUSE_DEBUG("Set IR sensitivity to level %i (unid %i)", level, wm->unid);
}
/**
* @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) {
if(WIIMOTE_IS_SET(wm,WIIMOTE_STATE_ACC)) return;
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_ACC);
} else {
if(!WIIMOTE_IS_SET(wm,WIIMOTE_STATE_ACC)) return;
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_ACC);
}
if(!WIIMOTE_IS_SET(wm,WIIMOTE_STATE_HANDSHAKE_COMPLETE)) return;
wiiuse_status(wm,NULL);
}
void wiic_mute_speaker( struct wiimote_t* wm, int status ) {
if ( status ) {
// if already enabled then stop
if ( WIIMOTE_IS_SET(wm, WIIMOTE_STATE_SPEAKER_MUTE) ) return;
WIIMOTE_ENABLE_STATE( wm, WIIMOTE_STATE_SPEAKER_MUTE );
WIIC_DEBUG("Speaker unmuted");
byte buf = 0x00;
wiic_send( wm, WM_CMD_SPEAKER_MUTE, &buf, 1 );
} else {
// if already disabled then stop
if ( !WIIMOTE_IS_SET(wm, WIIMOTE_STATE_SPEAKER_MUTE) ) return;
WIIMOTE_DISABLE_STATE( wm, WIIMOTE_STATE_SPEAKER_MUTE );
WIIC_DEBUG("Speaker muted");
byte buf = 0x04;
wiic_send( wm, WM_CMD_SPEAKER_MUTE, &buf, 1 );
}
}
void wiiuse_set_motion_plus(struct wiimote_t *wm, int status)
{
ubyte val;
if(WIIMOTE_IS_SET(wm,WIIMOTE_STATE_EXP_HANDSHAKE))
return;
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);
if(status)
{
val = 0x04;
wiiuse_write_data(wm,WM_EXP_MOTION_PLUS_ENABLE,&val,1,wiiuse_motion_plus_check);
}
else
{
wiiuse_disable_expansion(wm);
val = 0x55;
wiiuse_write_data(wm,WM_EXP_MEM_ENABLE1,&val,1,wiiuse_set_motion_plus_clear1);
}
}
/**
* @brief Load Wii devices registered in the wiimotes.config file.
*
* @param wm An array of wiimote_t structures.
*
* @return The number of wiimotes successfully loaded.
*
* @see wiic_find()
* @see wiic_connect()
* @see wiic_connect_single()
* @see wiic_disconnect()
*
* Up to version 0.53, it is possible to register the MAC address of your
* Wii devices. This allows to automatically load them, without waiting for any
* search timeout. To register a new device, go to: <HOME_DIR>/.wiic/ and
* edit the file wiimotes.config, by adding the MAC address of the device
* you want to register (one line per MAC address).
*/
int wiic_load(struct wiimote_t** wm)
{
int loaded = 0;
int i = 0;
char str[200];
char* str_ptr = 0;
char configPath[100];
char* tmp = 0;
// Retrieve the HOME environment variable
tmp = getenv("HOME");
strcpy(configPath,tmp);
strncat(configPath,"/.wiic/wiimotes.config",22);
// Open the config file
FILE* fd = 0;
fd = fopen(configPath,"r");
if(!fd)
return loaded;
// Read line by line
while(fgets(str,sizeof(str),fd) != NULL && loaded < 1) {
int len = strlen(str)-1;
if(str[len] == '\n')
str[len] = 0;
loaded++;
}
// We initialize the device structure
for (; i < loaded; ++i) {
/* found a device */
strncpy(wm[i]->bdaddr_str,str,18);
str_ptr = str;
str2ba(str_ptr,&(wm[i]->bdaddr));
WIIMOTE_ENABLE_STATE(wm[i], WIIMOTE_STATE_DEV_FOUND);
WIIC_INFO("Loaded Wiimote (%s) [id %i].",wm[i]->bdaddr_str,wm[i]->unid);
}
return loaded;
}
void wiiuse_handshake(struct wiimote_t *wm,ubyte *data,uword len)
{
ubyte *buf = NULL;
struct accel_t *accel = &wm->accel_calib;
//printf("wiiuse_handshake(%d,%p,%d)\n",wm->handshake_state,data,len);
switch(wm->handshake_state) {
case 0:
wm->handshake_state++;
wiiuse_set_leds(wm,WIIMOTE_LED_NONE,NULL);
buf = __lwp_wkspace_allocate(sizeof(ubyte)*8);
wiiuse_read_data(wm,buf,WM_MEM_OFFSET_CALIBRATION,7,wiiuse_handshake);
break;
case 1:
wm->handshake_state++;
accel->cal_zero.x = ((data[0]<<2)|((data[3]>>4)&3));
accel->cal_zero.y = ((data[1]<<2)|((data[3]>>2)&3));
accel->cal_zero.z = ((data[2]<<2)|(data[3]&3));
accel->cal_g.x = (((data[4]<<2)|((data[7]>>4)&3)) - accel->cal_zero.x);
accel->cal_g.y = (((data[5]<<2)|((data[7]>>2)&3)) - accel->cal_zero.y);
accel->cal_g.z = (((data[6]<<2)|(data[7]&3)) - accel->cal_zero.z);
__lwp_wkspace_free(data);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE_COMPLETE);
wm->event = WIIUSE_CONNECT;
wiiuse_status(wm,NULL);
break;
default:
break;
}
}
void wiiuse_motion_plus_check(struct wiimote_t *wm,ubyte *data,uword len)
{
u32 val;
if(data == NULL)
{
wiiuse_read_data(wm, wm->motion_plus_id, WM_EXP_ID, 6, wiiuse_motion_plus_check);
}
else
{
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_FAILED);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);
val = (data[3] << 16) | (data[2] << 24) | (data[4] << 8) | data[5];
if(val == EXP_ID_CODE_MOTION_PLUS)
{
/* handshake done */
wm->event = WIIUSE_MOTION_PLUS_ACTIVATED;
wm->exp.type = EXP_MOTION_PLUS;
WIIMOTE_ENABLE_STATE(wm,WIIMOTE_STATE_EXP);
wiiuse_set_ir_mode(wm);
}
}
}
void wiic_set_speaker( struct wiimote_t* wm, int status ) {
if ( status ) {
// if already enabled then stop
if ( WIIMOTE_IS_SET(wm, WIIMOTE_STATE_SPEAKER) ) return;
WIIMOTE_ENABLE_STATE( wm, WIIMOTE_STATE_SPEAKER );
WIIMOTE_ENABLE_STATE( wm, WIIMOTE_STATE_SPEAKER_MUTE );
byte buf;
/* Initialization Protocol */
// Enable Speaker
buf = 0x04;
wiic_send( wm, WM_CMD_SPEAKER_ENABLE, &buf, 1 );
// Mute Speaker
buf = 0x04;
wiic_send( wm, WM_CMD_SPEAKER_MUTE, &buf, 1 );
// Write 0x01 to register 0xa20009
buf = 0x01;
wiic_write_data( wm, 0x04a20009, &buf, 1 );
// Write 0x08 to register 0xa20001
buf = 0x08;
wiic_write_data( wm, 0x04a20001, &buf, 1 );
// 1st byte for configuration
buf = 0x00;
wiic_write_data( wm, 0x04a20001, &buf, 1 );
// 2nd byte for configuration
buf = 0x00;
wiic_write_data( wm, 0x04a20002, &buf, 1 );
// 3rd byte for configuration
buf = 0xD0;
wiic_write_data( wm, 0x04a20003, &buf, 1 );
// 4th byte for configuration
buf = 0x07;
wiic_write_data( wm, 0x04a20004, &buf, 1 );
// 5th byte for configuration
buf = 40;
wiic_write_data( wm, 0x04a20005, &buf, 1 );
// 6th byte for configuration
buf = 0x00;
wiic_write_data( wm, 0x04a20006, &buf, 1 );
// 7th byte for configuration
buf = 0x00;
wiic_write_data( wm, 0x04a20007, &buf, 1 );
// Write 0x01 to register 0xa20008
buf = 0x01;
wiic_write_data( wm, 0x04a20008, &buf, 1 );
// Unmute Speaker
buf = 0x00;
wiic_send( wm, WM_CMD_SPEAKER_MUTE, &buf, 1 );
WIIC_DEBUG("Speaker enabled");
} else {
// if already disabled then stop
if ( !WIIMOTE_IS_SET(wm, WIIMOTE_STATE_SPEAKER) ) return;
WIIMOTE_DISABLE_STATE( wm, WIIMOTE_STATE_SPEAKER );
WIIMOTE_DISABLE_STATE( wm, WIIMOTE_STATE_SPEAKER_MUTE );
WIIC_DEBUG("Speaker disabled");
byte buf = 0x00;
wiic_send( wm, WM_CMD_SPEAKER_ENABLE, &buf, 1 );
}
/* set the wiimote report type */
wiic_set_report_type( wm );
/* wait for the wiimote to catch up */
usleep( 50000 );
}
/**
* @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 wiic_find().
*
* @return 1 on success, 0 on failure
*/
static int wiic_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;
}
WIIC_INFO("Connected to wiimote [id %i].", wm->unid);
/* do the handshake */
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_CONNECTED);
wiic_handshake(wm, NULL, 0);
wiic_set_report_type(wm);
return 1;
}
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;
}
int wiiuse_os_find(struct wiimote_t** wm, int max_wiimotes, int timeout) {
int device_id;
int device_sock;
inquiry_info scan_info_arr[128];
inquiry_info* scan_info = scan_info_arr;
int found_devices;
int found_wiimotes;
int i = 0;
/* reset all wiimote bluetooth device addresses */
for (found_wiimotes = 0; found_wiimotes < max_wiimotes; ++found_wiimotes) {
/* bacpy(&(wm[found_wiimotes]->bdaddr), BDADDR_ANY); */
memset(&(wm[found_wiimotes]->bdaddr), 0, sizeof(bdaddr_t));
}
found_wiimotes = 0;
/* get the id of the first bluetooth device. */
device_id = hci_get_route(NULL);
if (device_id < 0) {
if (errno == ENODEV) {
WIIUSE_ERROR("Could not detect a Bluetooth adapter!");
} else {
perror("hci_get_route");
}
return 0;
}
/* create a socket to the device */
device_sock = hci_open_dev(device_id);
if (device_sock < 0) {
perror("hci_open_dev");
return 0;
}
memset(&scan_info_arr, 0, sizeof(scan_info_arr));
/* scan for bluetooth devices for 'timeout' seconds */
found_devices = hci_inquiry(device_id, timeout, 128, NULL, &scan_info, IREQ_CACHE_FLUSH);
if (found_devices < 0) {
perror("hci_inquiry");
close(device_sock);
return 0;
}
WIIUSE_INFO("Found %i bluetooth device(s).", found_devices);
/* display discovered devices */
for (i = 0; (i < found_devices) && (found_wiimotes < max_wiimotes); ++i) {
bool is_wiimote_regular = (scan_info[i].dev_class[0] == WM_DEV_CLASS_0) &&
(scan_info[i].dev_class[1] == WM_DEV_CLASS_1) &&
(scan_info[i].dev_class[2] == WM_DEV_CLASS_2);
bool is_wiimote_plus = (scan_info[i].dev_class[0] == WM_PLUS_DEV_CLASS_0) &&
(scan_info[i].dev_class[1] == WM_PLUS_DEV_CLASS_1) &&
(scan_info[i].dev_class[2] == WM_PLUS_DEV_CLASS_2);
if (is_wiimote_regular || is_wiimote_plus) {
/* found a device */
ba2str(&scan_info[i].bdaddr, wm[found_wiimotes]->bdaddr_str);
const char* str_type;
if(is_wiimote_regular)
{
wm[found_wiimotes]->type = WIIUSE_WIIMOTE_REGULAR;
str_type = " (regular wiimote)";
}
else if(is_wiimote_plus)
{
wm[found_wiimotes]->type = WIIUSE_WIIMOTE_MOTION_PLUS_INSIDE;
str_type = " (motion plus inside)";
}
WIIUSE_INFO("Found wiimote (type: %s) (%s) [id %i].", str_type, wm[found_wiimotes]->bdaddr_str, wm[found_wiimotes]->unid);
wm[found_wiimotes]->bdaddr = scan_info[i].bdaddr;
WIIMOTE_ENABLE_STATE(wm[found_wiimotes], WIIMOTE_STATE_DEV_FOUND);
++found_wiimotes;
}
}
close(device_sock);
return found_wiimotes;
}
/**
* @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);
}
/**
* @brief Get initialization data from the wiimote.
*
* @param wm Pointer to a wiimote_t structure.
* @param data unused
* @param len unused
*
* When first called for a wiimote_t structure, a request
* is sent to the wiimote for initialization information.
* This includes factory set accelerometer data.
* The handshake will be concluded when the wiimote responds
* with this data.
*/
void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len) {
if (!wm) return;
switch (wm->handshake_state) {
case 0:
{
/* send request to wiimote for accelerometer calibration */
byte* buf;
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
wiiuse_set_leds(wm, WIIMOTE_LED_NONE);
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;
/* 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);
/* request the status of the wiimote to see if there is an expansion */
wiiuse_status(wm);
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);
}
break;
}
default:
{
break;
}
}
}
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;
}
}
}
/**
* @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={0} ;
//char dest[18]="00:25:A0:B0:96:8A" ;
if (!wm || WIIMOTE_IS_CONNECTED(wm))
return 0;
wm->out_sock = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
if (wm->out_sock == -1)
return 0;
addr.l2_family = AF_BLUETOOTH;
addr.l2_psm = htobs(WM_OUTPUT_CHANNEL); // =0x11, le port -> Wiimote.
//str2ba(dest, &addr.l2_bdaddr) ;
if( address )
// Use provided address.
str2ba(address, &addr.l2_bdaddr) ;
else
// Use address of device discovered.
str2ba(wm->bdaddr_str, &addr.l2_bdaddr) ;
/*
* 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;
}
