void Wiimote::StartReporting(uint8_t aReportMode)
{
ULOG_DEBUG_F();
if (cwiid_set_mesg_callback(mWiimote, &Wiimote::CWiidCallback)) {
ULOG_DEBUG_F("Error setting callback");
if (cwiid_close(mWiimote)) {
ULOG_DEBUG_F("Error on disconnect");
}
mWiimote = NULL;
NotifyConnectionStatus(IWiimoteObserver::EConnectionError);
NotifyConnectionStatus(IWiimoteObserver::ENotConnected);
} else {
ULOG_DEBUG_F("Connected");
if (cwiid_get_acc_cal(mWiimote, CWIID_EXT_NONE, &mWmCal)) {
ULOG_DEBUG_F("Unable to retrieve accelerometer calibration");
}
NotifyConnectionStatus(IWiimoteObserver::EConnected);
/*
set_gui_state();
set_report_mode();
cwiid_enable(wiimote, CWIID_FLAG_MOTIONPLUS);
*/
//uint8_t rpt_mode;
//rpt_mode = CWIID_RPT_STATUS | CWIID_RPT_BTN | CWIID_RPT_ACC;
if (cwiid_set_rpt_mode(mWiimote, aReportMode)) {
ULOG_DEBUG_F("Error setting report mode");
}
cwiid_request_status(mWiimote);
}
}
static PyObject *Wiimote_request_status(Wiimote *self)
{
if (cwiid_request_status(self->wiimote)) {
PyErr_SetString(PyExc_RuntimeError, "Error requesting wiimote status");
return NULL;
}
Py_RETURN_NONE;
}
static void wiimote_status(t_wiimote *x)
{
if(x->connected) {
if (cwiid_request_status(x->wiimote)) {
pd_error(x, "error requesting status message");
}
}
wiimote_out_status(x, x->connected);
}
int cwiid_set_passthrough_mode(cwiid_wiimote_t *wiimote)
{
unsigned char data1,data2;
data1 = 0x55;
cwiid_write(wiimote, CWIID_RW_REG, 0xA400F0, 1, &data1);
data2 = 0x05;
cwiid_write(wiimote, CWIID_RW_REG, 0xA600FE, 1, &data2);
cwiid_request_status(wiimote);
return 0;
}
int cwiid_enable(cwiid_wiimote_t *wiimote, int flags)
{
unsigned char data;
if ((flags & CWIID_FLAG_NONBLOCK) &&
!(wiimote->flags & CWIID_FLAG_NONBLOCK)) {
if (fcntl(wiimote->mesg_pipe[0], F_SETFL, O_NONBLOCK)) {
cwiid_err(wiimote, "File control error (mesg pipe)");
return -1;
}
}
if (flags & CWIID_FLAG_MOTIONPLUS) {
data = 0x04;
cwiid_write(wiimote, CWIID_RW_REG, 0xA600FE, 1, &data);
cwiid_request_status(wiimote);
}
wiimote->flags |= flags;
return 0;
}
void menuConnect_activate(void)
{
char reset_bdaddr = 0;
if (bacmp(&bdaddr, BDADDR_ANY) == 0) {
reset_bdaddr = 1;
}
message(GTK_MESSAGE_INFO,
"Put Wiimote in discoverable mode (press 1+2) and press OK",
GTK_WINDOW(winMain));
if ((wiimote = cwiid_open(&bdaddr, CWIID_FLAG_MESG_IFC)) == NULL) {
message(GTK_MESSAGE_ERROR, "Unable to connect", GTK_WINDOW(winMain));
status("No connection");
}
else if (cwiid_set_mesg_callback(wiimote, &cwiid_callback)) {
message(GTK_MESSAGE_ERROR, "Error setting callback",
GTK_WINDOW(winMain));
if (cwiid_close(wiimote)) {
message(GTK_MESSAGE_ERROR, "Error on disconnect",
GTK_WINDOW(winMain));
}
wiimote = NULL;
status("No connection");
}
else {
status("Connected");
if (cwiid_get_acc_cal(wiimote, CWIID_EXT_NONE, &wm_cal)) {
message(GTK_MESSAGE_ERROR, "Unable to retrieve accelerometer "
"calibration", GTK_WINDOW(winMain));
}
set_gui_state();
set_report_mode();
cwiid_enable(wiimote, CWIID_FLAG_MOTIONPLUS);
cwiid_request_status(wiimote);
}
if (reset_bdaddr) {
bdaddr = *BDADDR_ANY;
}
}
int cwiid_command(cwiid_wiimote_t *wiimote, enum cwiid_command command,
int flags) {
int ret;
switch (command) {
case CWIID_CMD_STATUS:
ret = cwiid_request_status(wiimote);
break;
case CWIID_CMD_LED:
ret = cwiid_set_led(wiimote, flags);
break;
case CWIID_CMD_RUMBLE:
ret = cwiid_set_rumble(wiimote, flags);
break;
case CWIID_CMD_RPT_MODE:
ret = cwiid_set_rpt_mode(wiimote, flags);
break;
default:
ret = -1;
break;
}
return ret;
}
/* read the usb, and process it into frames
* a return value < 0 indicates error */
int ltr_int_tracker_get_frame(struct camera_control_block *ccb,
struct frame_type *f, bool *frame_acquired)
{
(void) ccb;
struct cwiid_state state;
unsigned int required_blobnum = 3;
unsigned int valid;
int i;
//Otherwise the polling takes too much processor
ltr_int_usleep(8334);
if (!gStateCheckIn--) {
gStateCheckIn = STATE_CHECK_INTERVAL;
if (cwiid_request_status(gWiimote)) {
ltr_int_log_message("Requesting status failed, disconnecting\n");
cwiid_close(gWiimote);
gWiimote = NULL;
return -1;
}
}
if (cwiid_get_state(gWiimote, &state)) {
// Treat connection as disconnected on error
ltr_int_log_message("Error reading wiimote state\n");
cwiid_close(gWiimote);
gWiimote = NULL;
return -1;
}
valid = 0;
for (i=0; i<CWIID_IR_SRC_COUNT; i++) {
if (state.ir_src[i].valid) {
valid++;
}
}
f->width = WIIMOTE_HORIZONTAL_RESOLUTION / 2;
f->height = WIIMOTE_VERTICAL_RESOLUTION / 2;
f->bloblist.num_blobs = valid < required_blobnum ? valid : required_blobnum;
f->bloblist.blobs = (struct blob_type *)
ltr_int_my_malloc(f->bloblist.num_blobs*sizeof(struct blob_type));
assert(f->bloblist.blobs);
bool draw;
if(f->bitmap != NULL){
draw = true;
}else{
draw = false;
}
image_t img = {
.w = WIIMOTE_HORIZONTAL_RESOLUTION / 2,
.h = WIIMOTE_VERTICAL_RESOLUTION / 2,
.bitmap = f->bitmap,
.ratio = 1.0
};
valid = 0;
for (i=0; i<CWIID_IR_SRC_COUNT; i++) {
if (state.ir_src[i].valid) {
if (valid<required_blobnum) {
f->bloblist.blobs[valid].x = -1 * state.ir_src[i].pos[CWIID_X] + WIIMOTE_HORIZONTAL_RESOLUTION/2;
f->bloblist.blobs[valid].y = state.ir_src[i].pos[CWIID_Y] - WIIMOTE_VERTICAL_RESOLUTION/2;
f->bloblist.blobs[valid].score = state.ir_src[i].size;
if(draw){
ltr_int_draw_square(&img, state.ir_src[i].pos[CWIID_X] / 2, (WIIMOTE_VERTICAL_RESOLUTION - state.ir_src[i].pos[CWIID_Y]) / 2, 2*state.ir_src[i].size);
ltr_int_draw_cross(&img, state.ir_src[i].pos[CWIID_X] / 2, (WIIMOTE_VERTICAL_RESOLUTION - state.ir_src[i].pos[CWIID_Y]) / 2, (int)WIIMOTE_HORIZONTAL_RESOLUTION/100.0);
}
}
valid++;
}
}
*frame_acquired = true;
return 0;
}
int main(int argc, char *argv[])
{
cwiid_wiimote_t *wiimote; /* wiimote handle */
struct cwiid_state state; /* wiimote state */
bdaddr_t bdaddr; /* bluetooth device address */
unsigned char mesg = 0;
unsigned char led_state = 0;
unsigned char rpt_mode = 0;
unsigned char rumble = 0;
int exit = 0;
cwiid_set_err(err);
/* Connect to address given on command-line, if present */
if (argc > 1) {
str2ba(argv[1], &bdaddr);
}
else {
bdaddr = *BDADDR_ANY;
}
/* Connect to the wiimote */
printf("Put Wiimote in discoverable mode now (press 1+2)...\n");
if (!(wiimote = cwiid_open(&bdaddr, 0))) {
fprintf(stderr, "Unable to connect to wiimote\n");
return -1;
}
if (cwiid_set_mesg_callback(wiimote, cwiid_callback)) {
fprintf(stderr, "Unable to set message callback\n");
}
printf("Note: To demonstrate the new API interfaces, wmdemo no longer "
"enables messages by default.\n"
"Output can be gathered through the new state-based interface (s), "
"or by enabling the messages interface (c).\n");
/* Menu */
printf("%s", MENU);
while (!exit) {
switch (getchar()) {
case '1':
toggle_bit(led_state, CWIID_LED1_ON);
set_led_state(wiimote, led_state);
break;
case '2':
toggle_bit(led_state, CWIID_LED2_ON);
set_led_state(wiimote, led_state);
break;
case '3':
toggle_bit(led_state, CWIID_LED3_ON);
set_led_state(wiimote, led_state);
break;
case '4':
toggle_bit(led_state, CWIID_LED4_ON);
set_led_state(wiimote, led_state);
break;
case '5':
toggle_bit(rumble, 1);
if (cwiid_set_rumble(wiimote, rumble)) {
fprintf(stderr, "Error setting rumble\n");
}
break;
case 'a':
toggle_bit(rpt_mode, CWIID_RPT_ACC);
set_rpt_mode(wiimote, rpt_mode);
break;
case 'b':
toggle_bit(rpt_mode, CWIID_RPT_BTN);
set_rpt_mode(wiimote, rpt_mode);
break;
case 'e':
/* CWIID_RPT_EXT is actually
* CWIID_RPT_NUNCHUK | CWIID_RPT_CLASSIC */
toggle_bit(rpt_mode, CWIID_RPT_EXT);
set_rpt_mode(wiimote, rpt_mode);
break;
case 'i':
/* libwiimote picks the highest quality IR mode available with the
* other options selected (not including as-yet-undeciphered
* interleaved mode */
toggle_bit(rpt_mode, CWIID_RPT_IR);
set_rpt_mode(wiimote, rpt_mode);
break;
case 'm':
if (!mesg) {
if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC)) {
fprintf(stderr, "Error enabling messages\n");
}
else {
mesg = 1;
}
}
else {
if (cwiid_disable(wiimote, CWIID_FLAG_MESG_IFC)) {
fprintf(stderr, "Error disabling message\n");
}
else {
mesg = 0;
}
}
break;
case 'p':
printf("%s", MENU);
break;
case 'r':
if (cwiid_request_status(wiimote)) {
fprintf(stderr, "Error requesting status message\n");
}
break;
case 's':
if (cwiid_get_state(wiimote, &state)) {
fprintf(stderr, "Error getting state\n");
}
print_state(&state);
break;
case 't':
toggle_bit(rpt_mode, CWIID_RPT_STATUS);
set_rpt_mode(wiimote, rpt_mode);
break;
case 'x':
exit = -1;
break;
case '\n':
break;
default:
fprintf(stderr, "invalid option\n");
}
}
if (cwiid_close(wiimote)) {
fprintf(stderr, "Error on wiimote disconnect\n");
return -1;
}
return 0;
}
cwiid_wiimote_t *cwiid_new(int ctl_socket, int int_socket, int flags)
{
struct wiimote *wiimote = NULL;
char mesg_pipe_init = 0, status_pipe_init = 0, rw_pipe_init = 0,
state_mutex_init = 0, rw_mutex_init = 0, rpt_mutex_init = 0,
router_thread_init = 0, status_thread_init = 0;
void *pthread_ret;
int err;
/* Allocate wiimote */
if ((wiimote = malloc(sizeof *wiimote)) == NULL) {
cwiid_err(NULL, "Memory allocation error (cwiid_wiimote_t)");
goto ERR_HND;
}
/* set sockets and flags */
wiimote->ctl_socket = ctl_socket;
wiimote->int_socket = int_socket;
wiimote->flags = flags;
/* Global Lock, Store and Increment wiimote_id */
err = pthread_mutex_lock(&global_mutex);
if (err) {
cwiid_err(NULL, "Mutex lock error (global mutex): %s", strerror(err));
goto ERR_HND;
}
wiimote->id = wiimote_id++;
err = pthread_mutex_unlock(&global_mutex);
if (err) {
cwiid_err(wiimote, "Mutex unlock error (global mutex) - "
"deadlock warning: %s", strerror(err));
goto ERR_HND;
}
/* Create pipes */
if (pipe(wiimote->mesg_pipe)) {
cwiid_err(wiimote, "Pipe creation error (mesg pipe): %s", strerror(errno));
goto ERR_HND;
}
mesg_pipe_init = 1;
if (pipe(wiimote->status_pipe)) {
cwiid_err(wiimote, "Pipe creation error (status pipe): %s", strerror(errno));
goto ERR_HND;
}
status_pipe_init = 1;
if (pipe(wiimote->rw_pipe)) {
cwiid_err(wiimote, "Pipe creation error (rw pipe): %s", strerror(errno));
goto ERR_HND;
}
rw_pipe_init = 1;
/* Setup blocking */
if (fcntl(wiimote->mesg_pipe[1], F_SETFL, O_NONBLOCK)) {
cwiid_err(wiimote, "File control error (mesg write pipe): %s", strerror(errno));
goto ERR_HND;
}
if (wiimote->flags & CWIID_FLAG_NONBLOCK) {
if (fcntl(wiimote->mesg_pipe[0], F_SETFL, O_NONBLOCK)) {
cwiid_err(wiimote, "File control error (mesg read pipe): %s", strerror(errno));
goto ERR_HND;
}
}
/* Init mutexes */
err = pthread_mutex_init(&wiimote->state_mutex, NULL);
if (err) {
cwiid_err(wiimote, "Mutex initialization error (state mutex): %s", strerror(err));
goto ERR_HND;
}
state_mutex_init = 1;
err = pthread_mutex_init(&wiimote->rw_mutex, NULL);
if (err) {
cwiid_err(wiimote, "Mutex initialization error (rw mutex): %s", strerror(err));
goto ERR_HND;
}
rw_mutex_init = 1;
err = pthread_mutex_init(&wiimote->rpt_mutex, NULL);
if (err) {
cwiid_err(wiimote, "Mutex initialization error (rpt mutex): %s", strerror(err));
goto ERR_HND;
}
rpt_mutex_init = 1;
/* Set rw_status before starting router thread */
wiimote->rw_status = RW_IDLE;
/* Launch interrupt socket listener and dispatch threads */
err = pthread_create(&wiimote->router_thread, NULL,
(void *(*)(void *))&router_thread, wiimote);
if (err) {
cwiid_err(wiimote, "Thread creation error (router thread): %s", strerror(err));
goto ERR_HND;
}
router_thread_init = 1;
err = pthread_create(&wiimote->status_thread, NULL,
(void *(*)(void *))&status_thread, wiimote);
if (err) {
cwiid_err(wiimote, "Thread creation error (status thread): %s", strerror(err));
goto ERR_HND;
}
status_thread_init = 1;
/* Success! Update state */
memset(&wiimote->state, 0, sizeof wiimote->state);
wiimote->mesg_callback = NULL;
cwiid_set_led(wiimote, 0);
cwiid_request_status(wiimote);
return wiimote;
ERR_HND:
if (wiimote) {
/* Close threads */
if (router_thread_init) {
pthread_cancel(wiimote->router_thread);
err = pthread_join(wiimote->router_thread, &pthread_ret);
if (err) {
cwiid_err(wiimote, "Thread join error (router thread): %s", strerror(err));
}
else if (!((pthread_ret == PTHREAD_CANCELED) &&
(pthread_ret == NULL))) {
cwiid_err(wiimote, "Bad return value from router thread");
}
}
if (status_thread_init) {
pthread_cancel(wiimote->status_thread);
err = pthread_join(wiimote->status_thread, &pthread_ret);
if (err) {
cwiid_err(wiimote, "Thread join error (status thread): %s", strerror(err));
}
else if (!((pthread_ret == PTHREAD_CANCELED) && (pthread_ret == NULL))) {
cwiid_err(wiimote, "Bad return value from status thread");
}
}
/* Close Pipes */
if (mesg_pipe_init) {
if (close(wiimote->mesg_pipe[0]) || close(wiimote->mesg_pipe[1])) {
cwiid_err(wiimote, "Pipe close error (mesg pipe): %s", strerror(errno));
}
}
if (status_pipe_init) {
if (close(wiimote->status_pipe[0]) ||
close(wiimote->status_pipe[1])) {
cwiid_err(wiimote, "Pipe close error (status pipe): %s", strerror(errno));
}
}
if (rw_pipe_init) {
if (close(wiimote->rw_pipe[0]) || close(wiimote->rw_pipe[1])) {
cwiid_err(wiimote, "Pipe close error (rw pipe): %s", strerror(errno));
}
}
/* Destroy Mutexes */
if (state_mutex_init) {
err = pthread_mutex_destroy(&wiimote->state_mutex);
if (err) {
cwiid_err(wiimote, "Mutex destroy error (state mutex): %s", strerror(err));
}
}
if (rw_mutex_init) {
err = pthread_mutex_destroy(&wiimote->rw_mutex);
if (err) {
cwiid_err(wiimote, "Mutex destroy error (rw mutex): %s", strerror(err));
}
}
if (rpt_mutex_init) {
err = pthread_mutex_destroy(&wiimote->rpt_mutex);
if (err) {
cwiid_err(wiimote, "Mutex destroy error (rpt mutex): %s", strerror(err));
}
}
free(wiimote);
}
return NULL;
}
void *passthrough_polling_thread(struct wiimote *wiimote)
{
unsigned char buf, data;
while(1)
{
if ((wiimote->passthrough_activate_flag && wiimote->ext == NUNCHUK_MOTIONPLUS_PRESENT) ||
(wiimote->passthrough_activate_flag && wiimote->ext == CLASSIC_MOTIONPLUS_PRESENT) ||
(!wiimote->passthrough_activate_flag && wiimote->ext == MOTIONPLUS_PRESENT) ||
(!wiimote->passthrough_activate_flag && wiimote->ext == EXT_PRESENT_NOT_MOTIONPLUS))
{
pthread_mutex_lock(&wiimote->poll_mutex);
pthread_cond_wait(&wiimote->condition_var,&wiimote->poll_mutex);
pthread_mutex_unlock(&wiimote->poll_mutex);
}
//fprintf(stderr,"passthrough_thread: resume\n");
if (wiimote->passthrough_activate_flag) {
//fprintf(stderr,"passthrough_thread: try activating passthrough\n");
if (wiimote->ext != NUNCHUK_MOTIONPLUS_PRESENT && wiimote->ext != CLASSIC_MOTIONPLUS_PRESENT && wiimote->ext != MOTIONPLUS_PRESENT) {
/* re-detecting motionplus */
buf=0xAB; //any junk value initialisation
cwiid_try_read(wiimote, CWIID_RW_REG, 0xA600FE, 1, &buf); //Checking for the MotionPlus jackin
if(buf==0x00 || buf==0x04 || buf==0x05 )
{
cwiid_enable(wiimote, CWIID_FLAG_MOTIONPLUS);
}
}
if (wiimote->ext == MOTIONPLUS_PRESENT) {
/* motionplus_present trying passthrough */
data = 0x55;
cwiid_write(wiimote, CWIID_RW_REG, 0xA400F0, 1, &data);
data = 0x00;
cwiid_write(wiimote, CWIID_RW_REG, 0xA400FB, 1, &data);
}
usleep(POLL_THREAD_DELAY);
cwiid_request_status(wiimote);
} else {
/* disabling passthrough and/or re-detecting motionplus if necessary */
/* only do this call if some other expansion is not already connected
this works because in this implementation motionplus takes precedence
over other kinds of extensions, like nunchuk and classic */
//fprintf(stderr,"passthrough_thread: reverting to motionplus\n");
if (wiimote->ext != EXT_PRESENT_NOT_MOTIONPLUS && wiimote->ext != EXT_UNINITIALIZED) {
//fprintf(stderr,"passthrough_thread: !EXT_PRESENT_NOT_MOTIONPLUS\n");
data = 0x55;
cwiid_write(wiimote, CWIID_RW_REG, 0xA400F0, 1, &data);
data = 0x00;
cwiid_write(wiimote, CWIID_RW_REG, 0xA400FB, 1, &data);
buf=0xAB; //any junk value initialisation
cwiid_try_read(wiimote, CWIID_RW_REG, 0xA600FE, 1, &buf); //Checking for the MotionPlus jackin
if(buf==0x00 || buf==0x04 || buf==0x05 || buf==0x07)
{
cwiid_enable(wiimote, CWIID_FLAG_MOTIONPLUS);
usleep(POLL_THREAD_DELAY);
cwiid_request_status(wiimote);
}
}
}
}
return NULL;
}
