wiimotelayer.c

linux下wii的鼠标驱动

/*
 * Lum, Darrell
 * Murphy, Ryan
 * CPE 454-01, Spring 07, Dr. Haungs
 * This file contains functions for parsing wii-mote data.
 */
 
#include "WiimoteLayer.h"
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
 
int initialize_wiimote_layer()
{
    return initialize_wiimote_support();
}
 
void cleanup_wiimote_layer()
{
    cleanup_wiimote_support();
}

Wiimote * wiimote_connect()
{
    int n = 0;
    Wiimote *wm;
    
    wm = (Wiimote *)malloc(sizeof(Wiimote));
    
    if(wm == NULL)
    {
        printf("Unable to allocate memory for Wiimote.\n");
        goto exit;
    }
    
   if((n = find_wiimotes(&wm->wiimoted, 1, 100)) <= 0)
   {
      wiimote_disconnect(wm);    
      wm = NULL;

      printf("No wiimotes found, or there was an error.\n");
      goto exit;
   }
   
   if(connect_wiimotes(&wm->wiimoted, n) != n)
   {
      wiimote_disconnect(wm);
      wm = NULL;

      printf("unable to connect to wiimote.\n");
      goto exit;
   }
   
   //Initialize data
   wm->data.motion_sensitivity= MOTION_DEFAULT_SENSITIVITY;

exit:
   return wm;    
}

void wiimote_disconnect(Wiimote *wm)
{
    disconnect_wiimotes(&wm->wiimoted, 0);   
    free(wm);
}
 
int get_data(Wiimote *wm)
{
    unsigned char buf[MAX_WIIPKT_SIZE];
    int bytes_read, i = 0;
    unsigned char test[MAX_WIIPKT_SIZE];
    float xg, yg, zg, x, y, z;
    i++;

    while (wiimote_check_data(&wm->wiimoted, WIIMOTE_COMM_INTERUPT))
    {
    	bytes_read = wiimote_recv(&wm->wiimoted, buf, MAX_WIIPKT_SIZE, WIIMOTE_COMM_INTERUPT);
        
       	if (bytes_read <= 0)
        	return bytes_read;
        
        switch (buf[0])
        {
            case WIIPKT_BUTTONS_ONLY:
                wm->data.ButtonField = (((short)buf[1]) << 8 & 0xFF00) | (((short)buf[2]) & 0xFF);
                break;
            case WIIPKT_BUTTONS_ACCEL:
                wm->data.ButtonField = (((short)buf[1]) << 8 & 0xFF00) | (((short)buf[2]) & 0xFF);
                
                xg = (float)wm->data.calg_x - (float)wm->data.calz_x;
                yg = (float)wm->data.calg_y - (float)wm->data.calz_y;
                zg = (float)wm->data.calg_z - (float)wm->data.calz_z;
                
                if (xg != 0.0f && yg != 0.0f && zg != 0.0f)
                {
                	x = ((float)buf[3] - (float)wm->data.calz_x) / xg;
                	y = ((float)buf[4] - (float)wm->data.calz_x) / yg;
                	z = ((float)buf[5] - (float)wm->data.calz_x) / zg;
                	
                	wm->data.ax = 2.0f * atan((double)x);
                	wm->data.ay = 2.0f * atan((double)y);
                	wm->data.az = 2.0f * atan((double)z);
                	
                	wm->data.roll = RAD_TO_DEGREE(wm->data.ax);
                	wm->data.pitch = RAD_TO_DEGREE(wm->data.ay);	
                	
                	wm->data.x = wm->data.roll * wm->data.roll / wm->data.motion_sensitivity;
                	wm->data.y = -wm->data.pitch * wm->data.roll / wm->data.motion_sensitivity;	
			
					if(wm->data.roll < 0)
						wm->data.x *= -1;
					if(wm->data.pitch < 0)
						wm->data.y *= -1;
                }
                break;
            case WIIPKT_IR_SENSOR:
                wm->data.ButtonField = (((short)buf[1]) << 8 & 0xFF00) | (((short)buf[2]) & 0xFF);
                
                xg = (float)wm->data.calg_x - (float)wm->data.calz_x;
                yg = (float)wm->data.calg_y - (float)wm->data.calz_y;
                zg = (float)wm->data.calg_z - (float)wm->data.calz_z;
                
                if (xg != 0.0f && yg != 0.0f && zg != 0.0f)
                {
                	x = ((float)buf[3] - (float)wm->data.calz_x) / xg;
                	y = ((float)buf[4] - (float)wm->data.calz_x) / yg;
                	z = ((float)buf[5] - (float)wm->data.calz_x) / zg;
                	
                	wm->data.ax = 2.0f * atan((double)x);
                	wm->data.ay = 2.0f * atan((double)y);
                	wm->data.az = 2.0f * atan((double)z);
                	
                	wm->data.roll = RAD_TO_DEGREE(wm->data.ax);
                	wm->data.pitch = RAD_TO_DEGREE(wm->data.ay);		
                }
                
                memset(test, 0xFF, MAX_WIIPKT_SIZE);
                if(memcmp(buf + 6, test + 6, 3))
                {
                    wm->data.x = (((int)buf[6]) & 0xFF)
                        | (((int)buf[8]) << 4 & 0x0300);
                    wm->data.y = (((int)buf[10]) & 0xFF)
                        | (((int)buf[11]) << 2 & 0x0300);
                }
                else {
                	// No IR data detected so use pitch and roll
                	if (xg != 0.0f && yg != 0.0f && zg != 0.0f)
                	{
	                	wm->data.x = wm->data.roll * wm->data.roll / wm->data.motion_sensitivity;
	                	wm->data.y = -wm->data.pitch * wm->data.pitch / wm->data.motion_sensitivity;
						if(wm->data.roll < 0)
							wm->data.x *= -1;
						if(wm->data.pitch < 0)
							wm->data.y *= -1;
                	}	
                }
                break;
            case WIIPKT_READ_RESPONSE:
            	wm->data.calz_x = buf[6];
            	wm->data.calz_y = buf[7];
            	wm->data.calz_z = buf[8];
            	
            	wm->data.calg_x = buf[10];
            	wm->data.calg_y = buf[11];
            	wm->data.calg_z = buf[12];
            	break;
            default:
                printf("data: %X, len = %i\n", buf[0], bytes_read);
        }
    }
    
    return 1;
}
 
int send_data(Wiimote *wm)
{
    unsigned char buf[2];
    
    buf[0] = 0x11;
    buf[1] = 0x00;
    
    buf[1] |= wm->data.LEDField;
    buf[1] |= wm->data.isRumble;

    return wiimote_send(&wm->wiimoted, buf, 2, WIIMOTE_COMM_CONTROL);
}
 
int set_functionality(Wiimote *wm, unsigned char wiiPkt)
{
    unsigned char buf[MAX_WIIPKT_SIZE];
    
    if(wiiPkt == WIIPKT_IR_SENSOR)
    {
    	/* Request calibration data */
    	memset(buf, 0, 22);
    	buf[0] = WIIPKT_READ_REQUEST; buf[4] = 0x16; buf[6] = 7;
    	wiimote_send(&wm->wiimoted, buf, 7, WIIMOTE_COMM_CONTROL);
    	
        /* Cmd 1 */
        buf[0] = 0x12; buf[1] = 0x00; buf[2] = WIIPKT_IR_SENSOR;        
        wiimote_send(&wm->wiimoted, buf, 3, WIIMOTE_COMM_CONTROL);

        /* Cmd 2 */
        buf[0] = 0x13; buf[1] = 0x04;
        wiimote_send(&wm->wiimoted, buf, 2, WIIMOTE_COMM_CONTROL);
        
        /* Cmd 3 */
        buf[0] = 0x1A; buf[1] = 0x04;
        wiimote_send(&wm->wiimoted, buf, 2, WIIMOTE_COMM_CONTROL);
        
        /* Cmd 4 */
        memset(buf, 0, 22);
        buf[0] = 0x16; buf[1] = 0x04; buf[2] = 0xB0; buf[3] = 0x00;
        buf[4] = 0x30; buf[5] = 0x01; buf[6] = 0x08;
        wiimote_send(&wm->wiimoted, buf, 22, WIIMOTE_COMM_CONTROL);
        
        /* Cmd 5 */
        buf[4] = 0x06; buf[6] = 0x90;
        wiimote_send(&wm->wiimoted, buf, 22, WIIMOTE_COMM_CONTROL);
        
        /* Cmd 6 */     
        buf[4] = 0x08; buf[6] = 0xC0;
        wiimote_send(&wm->wiimoted, buf, 22, WIIMOTE_COMM_CONTROL);
        
        /* Cmd 7 */     
        buf[4] = 0x1A; buf[6] = 0x40;
        wiimote_send(&wm->wiimoted, buf, 22, WIIMOTE_COMM_CONTROL);
        
        /* Cmd 8 */
        buf[4] = 0x33; buf[6] = 0x3;
        return wiimote_send(&wm->wiimoted, buf, 22, WIIMOTE_COMM_CONTROL);                                                              
    }
    else if(wiiPkt == WIIPKT_BUTTONS_ACCEL)
    {
    	/* Request calibration data */
    	memset(buf, 0, 22);
    	buf[0] = WIIPKT_READ_REQUEST; buf[4] = 0x16; buf[6] = 7;
    	wiimote_send(&wm->wiimoted, buf, 7, WIIMOTE_COMM_CONTROL);
    	
    	/* Enable Buttons and Accel */
    	buf[0] = 0x12; buf[1] = 0x00; buf[2] = wiiPkt;
        return wiimote_send(&wm->wiimoted, buf, 3, WIIMOTE_COMM_CONTROL);
    }
    else
    {
        buf[0] = 0x12; buf[1] = 0x00; buf[2] = wiiPkt;
        return wiimote_send(&wm->wiimoted, buf, 3, WIIMOTE_COMM_CONTROL);
    }
}