wiiuse/motion_plus.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <math.h>
   4 #include <string.h>
   5
   6 #ifdef WIN32
   7         #include <Winsock2.h>
   8 #endif
   9
  10 #include "definitions.h"
  11 #include "wiiuse_internal.h"
  12 #include "dynamics.h"
  13 #include "events.h"
  14 #include "wiiboard.h"
  15 #include "io.h"
  16
  17 void wiiuse_motion_plus_check(struct wiimote_t *wm,ubyte *data,uword len)
  18 {
  19         u32 val;
  20         if(data == NULL)
  21         {
  22                 wiiuse_read_data(wm, wm->motion_plus_id, WM_EXP_ID, 6, wiiuse_motion_plus_check);
  23         }
  24         else
  25         {
  26                 WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP);
  27                 WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_FAILED);
  28                 WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);
  29                 val = (data[3] << 16) | (data[2] << 24) | (data[4] << 8) | data[5];
  30                 if(val == EXP_ID_CODE_MOTION_PLUS)
  31                 {
  32                         /* handshake done */
  33                         wm->event = WIIUSE_MOTION_PLUS_ACTIVATED;
  34                         wm->exp.type = EXP_MOTION_PLUS;
  35
  36                         WIIMOTE_ENABLE_STATE(wm,WIIMOTE_STATE_EXP);
  37                         wiiuse_set_ir_mode(wm);
  38                 }
  39         }
  40 }
  41
  42 static void wiiuse_set_motion_plus_clear2(struct wiimote_t *wm,ubyte *data,uword len)
  43 {
  44         WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP);
  45         WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_FAILED);
  46         WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);
  47         wiiuse_set_ir_mode(wm);
  48         wiiuse_status(wm,NULL);
  49 }
  50
  51 static void wiiuse_set_motion_plus_clear1(struct wiimote_t *wm,ubyte *data,uword len)
  52 {
  53         ubyte val = 0x00;
  54         wiiuse_write_data(wm,WM_EXP_MEM_ENABLE1,&val,1,wiiuse_set_motion_plus_clear2);
  55 }
  56
  57
  58 void wiiuse_set_motion_plus(struct wiimote_t *wm, int status)
  59 {
  60         ubyte val;
  61
  62         if(WIIMOTE_IS_SET(wm,WIIMOTE_STATE_EXP_HANDSHAKE))
  63                 return;
  64
  65         WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);
  66         if(status)
  67         {
  68                 val = 0x04;
  69                 wiiuse_write_data(wm,WM_EXP_MOTION_PLUS_ENABLE,&val,1,wiiuse_motion_plus_check);
  70         }
  71         else
  72         {
  73                 wiiuse_disable_expansion(wm);
  74                 val = 0x55;
  75                 wiiuse_write_data(wm,WM_EXP_MEM_ENABLE1,&val,1,wiiuse_set_motion_plus_clear1);
  76         }
  77 }
  78
  79 void motion_plus_disconnected(struct motion_plus_t* mp)
  80 {
  81         WIIUSE_DEBUG("Motion plus disconnected");
  82         memset(mp, 0, sizeof(struct motion_plus_t));
  83 }
  84
  85 void motion_plus_event(struct motion_plus_t* mp, ubyte* msg)
  86 {
  87         mp->rx = ((msg[5] & 0xFC) << 6) | msg[2]; // Pitch
  88         mp->ry = ((msg[4] & 0xFC) << 6) | msg[1]; // Roll
  89         mp->rz = ((msg[3] & 0xFC) << 6) | msg[0]; // Yaw
  90
  91         mp->ext = msg[4] & 0x1;
  92         mp->status = (msg[3] & 0x3) | ((msg[4] & 0x2) << 1); // roll, yaw, pitch
  93 }