static PyObject *Wiimote_read(Wiimote *self, PyObject *args, PyObject *kwds)
{
static char *kwlist[] = { "flags", "offset", "len", NULL };
unsigned char flags;
unsigned int offset;
Py_ssize_t len;
void *buf;
PyObject *pyRetBuf;
if (!self->wiimote) {
SET_CLOSED_ERROR;
return NULL;
}
if (!PyArg_ParseTupleAndKeywords(args, kwds, "BII:cwiid.Wiimote.read",
kwlist, &flags, &offset, &len)) {
return NULL;
}
if (!(pyRetBuf = PyBuffer_New(len))) {
return NULL;
}
if (PyObject_AsWriteBuffer(pyRetBuf, &buf, &len)) {
Py_DECREF(pyRetBuf);
return NULL;
}
if (cwiid_read(self->wiimote,flags,offset,len,buf)) {
PyErr_SetString(PyExc_RuntimeError, "Error reading wiimote data");
Py_DECREF(pyRetBuf);
return NULL;
}
return pyRetBuf;
}
void
WiimoteController::read_wiimote_calibration()
{
uint8_t buf[7];
if (cwiid_read(m_wiimote, CWIID_RW_EEPROM, 0x16, 7, buf))
{
std::cout << "Wiimote: Unable to retrieve accelerometer calibration" << std::endl;
}
else
{
m_wiimote_zero.x = buf[0];
m_wiimote_zero.y = buf[1];
m_wiimote_zero.z = buf[2];
m_wiimote_one.x = buf[4];
m_wiimote_one.y = buf[5];
m_wiimote_one.z = buf[6];
}
std::cout << "Wiimote Calibration: "
<< static_cast<int>(m_wiimote_zero.x) << ", "
<< static_cast<int>(m_wiimote_zero.x) << ", "
<< static_cast<int>(m_wiimote_zero.x) << " - "
<< static_cast<int>(m_wiimote_one.x) << ", "
<< static_cast<int>(m_wiimote_one.x) << ", "
<< static_cast<int>(m_wiimote_one.x) << std::endl;
}
void
WiimoteController::read_nunchuk_calibration()
{
uint8_t buf[14];
if (cwiid_read(m_wiimote, CWIID_RW_REG | CWIID_RW_DECODE, 0xA40020, sizeof(buf), buf))
{
log_error("unable to retrieve nunchuk calibration data");
}
else
{
m_nunchuk_zero.x = buf[0];
m_nunchuk_zero.y = buf[1];
m_nunchuk_zero.z = buf[2];
m_nunchuk_one.x = buf[4];
m_nunchuk_one.y = buf[5];
m_nunchuk_one.z = buf[6];
m_nunchuk_x.x = buf[8]; // max
m_nunchuk_x.y = buf[9]; // min
m_nunchuk_x.z = buf[10]; // center
m_nunchuk_y.x = buf[11]; // max
m_nunchuk_y.y = buf[12]; // min
m_nunchuk_y.z = buf[13]; // center
log_tmp("X: " << m_nunchuk_x);
log_tmp("Y: " << m_nunchuk_y);
}
}
int cwiid_get_acc_cal(cwiid_wiimote_t *wiimote, enum cwiid_ext_type ext_type,
struct acc_cal *acc_cal)
{
uint8_t flags;
uint32_t offset;
unsigned char buf[7];
char *err_str;
switch (ext_type) {
case CWIID_EXT_NONE:
flags = CWIID_RW_EEPROM;
offset = 0x16;
err_str = "";
break;
case CWIID_EXT_NUNCHUK:
flags = CWIID_RW_REG;
offset = 0xA40020;
err_str = "nunchuk ";
break;
default:
cwiid_err(wiimote, "Unsupported calibration request");
return -1;
}
if (cwiid_read(wiimote, flags, offset, 7, buf)) {
cwiid_err(wiimote, "Read error (%scal)", err_str);
return -1;
}
acc_cal->zero[CWIID_X] = buf[0];
acc_cal->zero[CWIID_Y] = buf[1];
acc_cal->zero[CWIID_Z] = buf[2];
acc_cal->one[CWIID_X] = buf[4];
acc_cal->one[CWIID_Y] = buf[5];
acc_cal->one[CWIID_Z] = buf[6];
return 0;
}
int cwiid_get_balance_cal(cwiid_wiimote_t *wiimote,
struct balance_cal *balance_cal)
{
unsigned char buf[24];
if (cwiid_read(wiimote, CWIID_RW_REG, 0xa40024, 24, buf)) {
cwiid_err(wiimote, "Read error (balancecal)");
return -1;
}
balance_cal->right_top[0] = ((uint16_t)buf[0]<<8 | (uint16_t)buf[1]);
balance_cal->right_bottom[0] = ((uint16_t)buf[2]<<8 | (uint16_t)buf[3]);
balance_cal->left_top[0] = ((uint16_t)buf[4]<<8 | (uint16_t)buf[5]);
balance_cal->left_bottom[0] = ((uint16_t)buf[6]<<8 | (uint16_t)buf[7]);
balance_cal->right_top[1] = ((uint16_t)buf[8]<<8 | (uint16_t)buf[9]);
balance_cal->right_bottom[1] = ((uint16_t)buf[10]<<8 | (uint16_t)buf[11]);
balance_cal->left_top[1] = ((uint16_t)buf[12]<<8 | (uint16_t)buf[13]);
balance_cal->left_bottom[1] = ((uint16_t)buf[14]<<8 | (uint16_t)buf[15]);
balance_cal->right_top[2] = ((uint16_t)buf[16]<<8 | (uint16_t)buf[17]);
balance_cal->right_bottom[2] = ((uint16_t)buf[18]<<8 | (uint16_t)buf[19]);
balance_cal->left_top[2] = ((uint16_t)buf[20]<<8 | (uint16_t)buf[21]);
balance_cal->left_bottom[2] = ((uint16_t)buf[22]<<8 | (uint16_t)buf[23]);
return 0;
}
void *status_thread(struct wiimote *wiimote)
{
struct mesg_array ma;
struct cwiid_status_mesg *status_mesg;
unsigned char buf[2];
ma.count = 1;
status_mesg = &ma.array[0].status_mesg;
while (1) {
if (full_read(wiimote->status_pipe[0], status_mesg,
sizeof *status_mesg)) {
cwiid_err(wiimote, "Pipe read error (status)");
/* Quit! */
break;
}
if (status_mesg->type != CWIID_MESG_STATUS) {
cwiid_err(wiimote, "Bad message on status pipe");
continue;
}
if (status_mesg->ext_type == CWIID_EXT_UNKNOWN) {
/* Read extension ID */
if (cwiid_read(wiimote, CWIID_RW_REG, 0xA400FE, 1, &buf[0])) {
cwiid_err(wiimote, "Read error (extension error)");
status_mesg->ext_type = CWIID_EXT_UNKNOWN;
}
/* If the extension didn't change, or if the extension is a
* MotionPlus, no init necessary */
switch (buf[0]) {
case EXT_NONE:
status_mesg->ext_type = CWIID_EXT_NONE;
break;
case EXT_NUNCHUK:
status_mesg->ext_type = CWIID_EXT_NUNCHUK;
break;
case EXT_CLASSIC:
status_mesg->ext_type = CWIID_EXT_CLASSIC;
break;
case EXT_BALANCE:
status_mesg->ext_type = CWIID_EXT_BALANCE;
break;
case EXT_MOTIONPLUS:
status_mesg->ext_type = CWIID_EXT_MOTIONPLUS;
break;
case EXT_PARTIAL:
/* Everything (but MotionPlus) shows up as partial until initialized */
buf[0] = 0x55;
buf[1] = 0x00;
/* Initialize extension register space */
if (cwiid_write(wiimote, CWIID_RW_REG, 0xA400F0, 1, &buf[0])) {
cwiid_err(wiimote, "Extension initialization error");
status_mesg->ext_type = CWIID_EXT_UNKNOWN;
}
else if (cwiid_write(wiimote, CWIID_RW_REG, 0xA400FB, 1, &buf[1])) {
cwiid_err(wiimote, "Extension initialization error");
status_mesg->ext_type = CWIID_EXT_UNKNOWN;
}
/* Read extension ID */
else if (cwiid_read(wiimote, CWIID_RW_REG, 0xA400FE, 1, &buf[0])) {
cwiid_err(wiimote, "Read error (extension error)");
status_mesg->ext_type = CWIID_EXT_UNKNOWN;
}
else {
switch (buf[0]) {
case EXT_NONE:
case EXT_PARTIAL:
status_mesg->ext_type = CWIID_EXT_NONE;
break;
case EXT_NUNCHUK:
status_mesg->ext_type = CWIID_EXT_NUNCHUK;
break;
case EXT_CLASSIC:
status_mesg->ext_type = CWIID_EXT_CLASSIC;
break;
case EXT_BALANCE:
status_mesg->ext_type = CWIID_EXT_BALANCE;
break;
default:
status_mesg->ext_type = CWIID_EXT_UNKNOWN;
break;
}
}
break;
}
}
if (update_state(wiimote, &ma)) {
cwiid_err(wiimote, "State update error");
}
if (update_rpt_mode(wiimote, -1)) {
cwiid_err(wiimote, "Error reseting report mode");
}
if ((wiimote->state.rpt_mode & CWIID_RPT_STATUS) &&
(wiimote->flags & CWIID_FLAG_MESG_IFC)) {
if (write_mesg_array(wiimote, &ma)) {
/* prints its own errors */
}
}
}
return NULL;
}
void *status_thread(struct wiimote *wiimote)
{
struct mesg_array ma;
struct cwiid_status_mesg *status_mesg;
unsigned char buf[2];
unsigned char data[2];
ma.count = 1;
status_mesg = &ma.array[0].status_mesg;
while (1) {
if (full_read(wiimote->status_pipe[0], status_mesg,
sizeof *status_mesg)) {
cwiid_err(wiimote, "Pipe read error (status)");
/* Quit! */
break;
}
if (status_mesg->type != CWIID_MESG_STATUS) {
cwiid_err(wiimote, "Bad message on status pipe");
continue;
}
if (status_mesg->ext_type == CWIID_EXT_UNKNOWN) {
/* Read extension ID */
if (cwiid_read(wiimote, CWIID_RW_REG, 0xA400FE, 2, &buf)) {
cwiid_err(wiimote, "Read error (extension error)");
status_mesg->ext_type = CWIID_EXT_UNKNOWN;
}
/* If we have just disabled passthrough override passthrough with just plain motionplus */
/*if ((!wiimote->passthrough_activate_flag && ((buf[0] << 8) | buf[1]) == EXT_NUNCHUK_MOTIONPLUS) ||
(!wiimote->passthrough_activate_flag && ((buf[0] << 8) | buf[1]) == EXT_CLASSIC_MOTIONPLUS)) {
fprintf(stderr, "status_thread: reverting to motionplus\n");
buf[0] = EXT_MOTIONPLUS;
}*/
/* If the extension didn't change, or if the extension is a
* MotionPlus, no init necessary */
switch ((buf[0] << 8) | buf[1]) {
case EXT_NONE:
status_mesg->ext_type = CWIID_EXT_NONE;
break;
case EXT_NUNCHUK:
if (wiimote->ext == MOTIONPLUS_PRESENT && wiimote->passthrough_activate_flag){
data[0] = 0x05;
cwiid_write(wiimote, CWIID_RW_REG, 0xA600FE, 1, &data[0]);
cwiid_read(wiimote, CWIID_RW_REG, 0xA400FE, 1, &data[1]);
if (data[1] == 0x05) {
status_mesg->ext_type = CWIID_EXT_NUNCHUK_MPLUS_PASSTHROUGH;
wiimote->ext = NUNCHUK_MOTIONPLUS_PRESENT;
break;
}
}else{
status_mesg->ext_type = CWIID_EXT_NUNCHUK;
wiimote->ext = EXT_PRESENT_NOT_MOTIONPLUS;
break;
}
case EXT_NUNCHUK_MOTIONPLUS:
status_mesg->ext_type = CWIID_EXT_NUNCHUK_MPLUS_PASSTHROUGH;
wiimote->ext = NUNCHUK_MOTIONPLUS_PRESENT;
break;
case EXT_CLASSIC:
if (wiimote->ext == MOTIONPLUS_PRESENT && wiimote->passthrough_activate_flag){
data[0] = 0x07;
cwiid_write(wiimote, CWIID_RW_REG, 0xA600FE, 1, &data[0]);
cwiid_read(wiimote, CWIID_RW_REG, 0xA400FE, 1, &data[1]);
if (data[1] == 0x07) {
status_mesg->ext_type = CWIID_EXT_CLASSIC_MPLUS_PASSTHROUGH;
wiimote->ext = CLASSIC_MOTIONPLUS_PRESENT;
break;
}
}else{
status_mesg->ext_type = CWIID_EXT_CLASSIC;
wiimote->ext = EXT_PRESENT_NOT_MOTIONPLUS;
break;
}
case EXT_CLASSIC_MOTIONPLUS:
status_mesg->ext_type = CWIID_EXT_CLASSIC_MPLUS_PASSTHROUGH;
wiimote->ext = CLASSIC_MOTIONPLUS_PRESENT;
break;
case EXT_BALANCE:
status_mesg->ext_type = CWIID_EXT_BALANCE;
wiimote->ext = EXT_PRESENT_NOT_MOTIONPLUS;
break;
case EXT_MOTIONPLUS:
status_mesg->ext_type = CWIID_EXT_MOTIONPLUS;
wiimote->ext = MOTIONPLUS_PRESENT;
break;
case EXT_PARTIAL:
/* Everything (but MotionPlus) shows up as partial until initialized */
buf[0] = 0x55;
buf[1] = 0x00;
/* Initialize extension register space */
if (cwiid_write(wiimote, CWIID_RW_REG, 0xA400F0, 1, &buf[0])) {
cwiid_err(wiimote, "Extension initialization error");
status_mesg->ext_type = CWIID_EXT_UNKNOWN;
}
else if (cwiid_write(wiimote, CWIID_RW_REG, 0xA400FB, 1, &buf[1])) {
cwiid_err(wiimote, "Extension initialization error");
status_mesg->ext_type = CWIID_EXT_UNKNOWN;
}
/* Read extension ID */
else if (cwiid_read(wiimote, CWIID_RW_REG, 0xA400FE, 2, &buf)) {
cwiid_err(wiimote, "Read error (extension error)");
status_mesg->ext_type = CWIID_EXT_UNKNOWN;
}
else {
switch ((buf[0] << 8) | buf[1]) {
case EXT_NONE:
case EXT_PARTIAL:
status_mesg->ext_type = CWIID_EXT_NONE;
break;
case EXT_NUNCHUK:
status_mesg->ext_type = CWIID_EXT_NUNCHUK;
break;
case EXT_CLASSIC:
status_mesg->ext_type = CWIID_EXT_CLASSIC;
break;
case EXT_BALANCE:
status_mesg->ext_type = CWIID_EXT_BALANCE;
break;
case EXT_NUNCHUK_MOTIONPLUS:
status_mesg->ext_type = CWIID_EXT_NUNCHUK_MPLUS_PASSTHROUGH;
break;
case EXT_CLASSIC_MOTIONPLUS:
status_mesg->ext_type = CWIID_EXT_CLASSIC_MPLUS_PASSTHROUGH;
break;
default:
status_mesg->ext_type = CWIID_EXT_UNKNOWN;
break;
}
}
break;
}
}
else {
//fprintf(stderr,"ext_type=%d ext=%d\n", status_mesg->ext_type, wiimote->ext);
if (!(wiimote->passthrough_activate_flag && wiimote->ext == MOTIONPLUS_PRESENT) ||
!wiimote->passthrough_activate_flag) {
//fprintf(stderr,"NO EXTENSION\n");
wiimote->ext = NO_EXTENSION;
}
pthread_cond_signal(&wiimote->condition_var);
}
if (update_state(wiimote, &ma)) {
cwiid_err(wiimote, "State update error");
}
if (update_rpt_mode(wiimote, -1)) {
cwiid_err(wiimote, "Error reseting report mode");
}
if ((wiimote->state.rpt_mode & CWIID_RPT_STATUS) &&
(wiimote->flags & CWIID_FLAG_MESG_IFC)) {
if (write_mesg_array(wiimote, &ma)) {
/* prints its own errors */
}
}
}
return NULL;
}
void btnRead_clicked(void)
{
static unsigned char buf[CWIID_MAX_READ_LEN];
static char txt[CWIID_MAX_READ_LEN*4+50]; /* 3 chars per byte, with
* plenty extra */
GtkTextIter text_iter;
GtkTextMark *p_text_mark;
char *cursor;
unsigned int offset, len;
int flags;
unsigned int i;
/* Decode arguments */
offset = strtol(gtk_entry_get_text(GTK_ENTRY(txtReadOffset)), &cursor, 16);
if (*cursor != '\0') {
message(GTK_MESSAGE_ERROR, "Invalid read offset", GTK_WINDOW(winRW));
}
len = strtol(gtk_entry_get_text(GTK_ENTRY(txtReadLen)), &cursor, 16);
if (*cursor != '\0') {
message(GTK_MESSAGE_ERROR, "Invalid read len", GTK_WINDOW(winRW));
}
if (gtk_toggle_button_get_active(GTK_TOGGLE_BUTTON(radReadReg))) {
flags = CWIID_RW_REG;
}
else {
flags = CWIID_RW_EEPROM;
}
/* Make the call */
if (cwiid_read(wiimote, flags, offset, len, buf)) {
message(GTK_MESSAGE_ERROR, "Wiimote read error", GTK_WINDOW(winRW));
}
else {
/* construct the hexedit-style string */
cursor=txt;
sprintf(cursor, "0x%08X:", offset & ~0xF);
cursor+=11;
for (i=0; i < (offset & 0xF); i++) {
sprintf(cursor, " ");
cursor+=3;
}
for (i=0; i < len; i++) {
if ((((i + offset) & 0xF) == 0) && (i!=0)) {
sprintf(cursor, "\n0x%08X:", offset+i);
cursor+=12;
}
if (((i +offset) & 0x7) == 0) {
sprintf(cursor, " ");
cursor++;
}
sprintf(cursor, "%02X ", buf[i]);
cursor+=3;
}
sprintf(cursor, "\n\n");
gtk_text_buffer_get_end_iter(tbRW, &text_iter);
p_text_mark = gtk_text_buffer_create_mark(tbRW, NULL, &text_iter,
TRUE);
gtk_text_buffer_insert(tbRW, &text_iter, txt, -1);
gtk_text_view_scroll_to_mark(GTK_TEXT_VIEW(tvRW), p_text_mark, 0.01,
TRUE, 0.0, 0.0);
}
}
