mirror of
https://github.com/dolphin-emu/dolphin.git
synced 2024-10-01 08:42:32 +02:00
93 lines
2.4 KiB
C
93 lines
2.4 KiB
C
|
#include <stdio.h>
|
||
|
|
#include <stdlib.h>
|
||
|
|
#include <math.h>
|
||
|
|
#include <string.h>
|
||
|
|
|
||
|
|
#ifdef WIN32
|
||
|
|
#include <Winsock2.h>
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#include "definitions.h"
|
||
|
|
#include "wiiuse_internal.h"
|
||
|
|
#include "dynamics.h"
|
||
|
|
#include "events.h"
|
||
|
|
#include "wiiboard.h"
|
||
|
|
#include "io.h"
|
||
|
|
|
||
|
|
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);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static void wiiuse_set_motion_plus_clear2(struct wiimote_t *wm,ubyte *data,uword len)
|
||
|
|
{
|
||
|
|
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP);
|
||
|
|
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_FAILED);
|
||
|
|
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);
|
||
|
|
wiiuse_set_ir_mode(wm);
|
||
|
|
wiiuse_status(wm,NULL);
|
||
|
|
}
|
||
|
|
|
||
|
|
static void wiiuse_set_motion_plus_clear1(struct wiimote_t *wm,ubyte *data,uword len)
|
||
|
|
{
|
||
|
|
ubyte val = 0x00;
|
||
|
|
wiiuse_write_data(wm,WM_EXP_MEM_ENABLE1,&val,1,wiiuse_set_motion_plus_clear2);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
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);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void motion_plus_disconnected(struct motion_plus_t* mp)
|
||
|
|
{
|
||
|
|
WIIUSE_DEBUG("Motion plus disconnected");
|
||
|
|
memset(mp, 0, sizeof(struct motion_plus_t));
|
||
|
|
}
|
||
|
|
|
||
|
|
void motion_plus_event(struct motion_plus_t* mp, ubyte* msg)
|
||
|
|
{
|
||
|
|
mp->rx = ((msg[5] & 0xFC) << 6) | msg[2]; // Pitch
|
||
|
|
mp->ry = ((msg[4] & 0xFC) << 6) | msg[1]; // Roll
|
||
|
|
mp->rz = ((msg[3] & 0xFC) << 6) | msg[0]; // Yaw
|
||
|
|
|
||
|
|
mp->ext = msg[4] & 0x1;
|
||
|
|
mp->status = (msg[3] & 0x3) | ((msg[4] & 0x2) << 1); // roll, yaw, pitch
|
||
|
|
}
|