Balance board patch from http://www.akhphd.au.dk/~bertho/wii/

example/example.c

@@ -36,6 +36,7 @@
 
 #include <stdio.h>
 #include <stdlib.h>
+#include <math.h>
 
 #ifndef WIN32
 	#include <unistd.h>
@@ -56,6 +57,7 @@
  *	event occurs on the specified wiimote.
  */
 void handle_event(struct wiimote_t* wm) {
+	if (wm->exp.type != EXP_BALANCE_BOARD)
 	printf("\n\n--- EVENT [id %i] ---\n", wm->unid);
 
 	/* if a button is pressed, report it */
@@ -181,6 +183,32 @@ void handle_event(struct wiimote_t* wm) {
 		printf("Guitar whammy bar:          %f\n", gh3->whammy_bar);
 		printf("Guitar joystick angle:      %f\n", gh3->js.ang);
 		printf("Guitar joystick magnitude:  %f\n", gh3->js.mag);
+	} else if (wm->exp.type == EXP_BALANCE_BOARD) {
+		static int first = 1;
+		const struct balance_board_t *bb = &wm->exp.bb;
+		float v = bb->tr + bb->tl + bb->br + bb->bl;
+		float x = ((bb->tr + bb->br) - (bb->tl + bb->bl));
+		float y = ((bb->tl + bb->tr) - (bb->bl + bb->br));
+		float a = atan2f(y, x) / M_PI * 180.0;
+
+		/*
+		 * Board orientation:
+		 * +---------------------+
+		 * | tl               tr |
+		 * |        y            |
+		 * |        ^            |
+		 * |        |            |
+		 * |        +----> x     |
+		 * | bl               br |
+		 * +---------vvv---------+
+		 *           Button-A (power)
+		 */
+		if(first) {
+			printf("  tr    br    tl    bl  |   x     y    |v|   ang\n");
+			first = 0;
+		}
+		printf("% 5.1f % 5.1f % 5.1f % 5.1f | % 5.1f %5.1f % 5.1f % 6.1f\r", bb->tr, bb->br, bb->tl, bb->bl, x, y, v, a);
+		fflush(stdout);
 	}
 }
 
@@ -407,6 +435,10 @@ int main(int argc, char** argv) {
 						printf("Classic controller inserted.\n");
 						break;
 
+					case WIIUSE_BALANCE_BOARD_CTRL_INSERTED:
+						printf("Balance board controller inserted.\n");
+						break;
+
 					case WIIUSE_GUITAR_HERO_3_CTRL_INSERTED:
 						/* some expansion was inserted */
 						handle_ctrl_status(wiimotes[i]);
@@ -416,6 +448,7 @@ int main(int argc, char** argv) {
 					case WIIUSE_NUNCHUK_REMOVED:
 					case WIIUSE_CLASSIC_CTRL_REMOVED:
 					case WIIUSE_GUITAR_HERO_3_CTRL_REMOVED:
+					case WIIUSE_BALANCE_BOARD_CTRL_REMOVED:
 						/* some expansion was removed */
 						handle_ctrl_status(wiimotes[i]);
 						printf("An expansion was removed.\n");

src/Makefile

@@ -48,6 +48,7 @@ INCLUDES = -I.
 # Generate a list of object files
 #
 OBJS = \
+	$(OBJ_DIR)/balance_board.o \
 	$(OBJ_DIR)/classic.o \
 	$(OBJ_DIR)/dynamics.o \
 	$(OBJ_DIR)/events.o \

src/balance_board.c

@@ -0,0 +1,140 @@
+/*
+ *	wiiuse
+ *
+ *	Written By:
+ *		Michael Laforest	< para >
+ *		Email: < thepara (--AT--) g m a i l [--DOT--] com >
+ *	This file:
+ *		Bertho Stultiens	< para >
+ *
+ *	Copyright 2009
+ *
+ *	This file is part of wiiuse.
+ *
+ *	This program is free software; you can redistribute it and/or modify
+ *	it under the terms of the GNU General Public License as published by
+ *	the Free Software Foundation; either version 3 of the License, or
+ *	(at your option) any later version.
+ *
+ *	This program is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *	GNU General Public License for more details.
+ *
+ *	You should have received a copy of the GNU General Public License
+ *	along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *	$Header$
+ *
+ */
+
+/**
+ *	@file
+ *	@brief Balance Board expansion device.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#ifdef WIN32
+	#include <Winsock2.h>
+#endif
+
+#include "definitions.h"
+#include "wiiuse_internal.h"
+#include "dynamics.h"
+#include "events.h"
+#include "balance_board.h"
+
+/**
+ *	@brief Handle the handshake data from the guitar.
+ *
+ *	@param cc		A pointer to a classic_ctrl_t structure.
+ *	@param data		The data read in from the device.
+ *	@param len		The length of the data block, in bytes.
+ *
+ *	@return	Returns 1 if handshake was successful, 0 if not.
+ */
+int balance_board_handshake(struct wiimote_t* wm, struct balance_board_t* bb, byte* data, unsigned short len) {
+	int i;
+
+	bb->tr = 0.0;
+	bb->br = 0.0;
+	bb->tl = 0.0;
+	bb->br = 0.0;
+
+	if (len < 0xe0)
+		return 0;
+
+	if (data[0xdc] != 0xa4) {
+		/* decrypt data */
+		for (i = 0; i < len; ++i)
+			data[i] = (data[i] ^ 0x17) + 0x17;
+	}
+
+	/* See: http://wiibrew.org/wiki/Wii_Balance_Board */
+	/* Unknown data at 0xa40020..23 (mine says: 0x44 0x69 0x00 0x00) */
+	bb->cal_0.tr = (data[0x04] << 8) + data[0x05];
+	bb->cal_0.br = (data[0x06] << 8) + data[0x07];
+	bb->cal_0.tl = (data[0x08] << 8) + data[0x09];
+	bb->cal_0.bl = (data[0x0a] << 8) + data[0x0b];
+	bb->cal_17.tr = (data[0x0c] << 8) + data[0x0d];
+	bb->cal_17.br = (data[0x0e] << 8) + data[0x0f];
+	bb->cal_17.tl = (data[0x10] << 8) + data[0x11];
+	bb->cal_17.bl = (data[0x12] << 8) + data[0x13];
+	bb->cal_34.tr = (data[0x14] << 8) + data[0x15];
+	bb->cal_34.br = (data[0x16] << 8) + data[0x17];
+	bb->cal_34.tl = (data[0x18] << 8) + data[0x19];
+	bb->cal_34.bl = (data[0x1a] << 8) + data[0x1b];
+	/* Unknown data at 0xa4003c..3f (mine says: 0x4c 0x81 0x59 0x95) */
+
+	WIIUSE_DEBUG(stderr, "calibration  0: %04x %04x %04x %04x\n", bb->cal_0.tr, bb->cal_0.br, bb->cal_0.tl, bb->cal_0.br);
+	WIIUSE_DEBUG(stderr, "calibration 17: %04x %04x %04x %04x\n", bb->cal_17.tr, bb->cal_17.br, bb->cal_17.tl, bb->cal_17.br);
+	WIIUSE_DEBUG(stderr, "calibration 34: %04x %04x %04x %04x\n", bb->cal_34.tr, bb->cal_34.br, bb->cal_34.tl, bb->cal_34.br);
+
+	/* handshake done */
+	wm->exp.type = EXP_BALANCE_BOARD;
+
+	#ifdef WIN32
+	wm->timeout = WIIMOTE_DEFAULT_TIMEOUT;
+	#endif
+
+	return 1;
+}
+
+
+/**
+ *	@brief The balance board disconnected.
+ *
+ *	@param cc		A pointer to a balance_board_t structure.
+ */
+void balance_board_disconnected(struct balance_board_t* bb) {
+	memset(bb, 0, sizeof(*bb));
+}
+
+
+
+/**
+ *	@brief Handle balance board event.
+ *
+ *	@param cc		A pointer to a balance board_t structure.
+ *	@param msg		The message specified in the event packet.
+ */
+#define set_bbval(x)	do {\
+		if(bb->raw.x < bb->cal_17.x) \
+			bb->x = 17.0 * (float)(bb->raw.x - bb->cal_0.x) / (float)(bb->cal_17.x - bb->cal_0.x); \
+		else \
+			bb->x = 17.0 * (1.0 + (float)(bb->raw.x - bb->cal_17.x) / (float)(bb->cal_34.x - bb->cal_17.x)); \
+	} while(0)
+void balance_board_event(struct balance_board_t* bb, byte* msg) {
+	bb->raw.tr = (msg[0] << 8) + msg[1];
+	bb->raw.br = (msg[2] << 8) + msg[3];
+	bb->raw.tl = (msg[4] << 8) + msg[5];
+	bb->raw.bl = (msg[6] << 8) + msg[7];
+	set_bbval(tr);
+	set_bbval(br);
+	set_bbval(tl);
+	set_bbval(bl);
+}
+

src/balance_board.h

@@ -0,0 +1,53 @@
+/*
+ *	wiiuse
+ *
+ *	Written By:
+ *		Michael Laforest	< para >
+ *		Email: < thepara (--AT--) g m a i l [--DOT--] com >
+ *	This file:
+ *		Bertho Stultiens	< para >
+ *
+ *	Copyright 2009
+ *
+ *	This file is part of wiiuse.
+ *
+ *	This program is free software; you can redistribute it and/or modify
+ *	it under the terms of the GNU General Public License as published by
+ *	the Free Software Foundation; either version 3 of the License, or
+ *	(at your option) any later version.
+ *
+ *	This program is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *	GNU General Public License for more details.
+ *
+ *	You should have received a copy of the GNU General Public License
+ *	along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *	$Header$
+ *
+ */
+
+/**
+ *	@file
+ *	@brief Balance Board expansion device.
+ */
+
+#ifndef BALANCE_BOARD_H_INCLUDED
+#define BALANCE_BOARD_H_INCLUDED
+
+#include "wiiuse_internal.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int balance_board_handshake(struct wiimote_t* wm, struct balance_board_t* bb, byte* data, unsigned short len);
+void balance_board_disconnected(struct balance_board_t* bb);
+void balance_board_event(struct balance_board_t* bb, byte* msg);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // BALANCE_BOARD_H_INCLUDED

src/events.c

@@ -56,6 +56,7 @@
 #include "nunchuk.h"
 #include "classic.h"
 #include "guitar_hero_3.h"
+#include "balance_board.h"
 #include "events.h"
 
 static void idle_cycle(struct wiimote_t* wm);
@@ -611,6 +612,9 @@ static void handle_expansion(struct wiimote_t* wm, byte* msg) {
 		case EXP_GUITAR_HERO_3:
 			guitar_hero_3_event(&wm->exp.gh3, msg);
 			break;
+		case EXP_BALANCE_BOARD:
+			balance_board_event(&wm->exp.bb, msg);
+			break;
 		default:
 			break;
 	}
@@ -680,6 +684,12 @@ void handshake_expansion(struct wiimote_t* wm, byte* data, unsigned short len) {
 				wm->event = WIIUSE_GUITAR_HERO_3_CTRL_INSERTED;
 			break;
 		}
+		case EXP_ID_CODE_BALANCE_BOARD:
+		{
+			if (balance_board_handshake(wm, &wm->exp.bb, data, len))
+				wm->event = WIIUSE_BALANCE_BOARD_CTRL_INSERTED;
+			break;
+		}
 		default:
 		{
 			WIIUSE_WARNING("Unknown expansion type. Code: 0x%x", id);
@@ -720,6 +730,10 @@ void disable_expansion(struct wiimote_t* wm) {
 			guitar_hero_3_disconnected(&wm->exp.gh3);
 			wm->event = WIIUSE_GUITAR_HERO_3_CTRL_REMOVED;
 			break;
+		case EXP_BALANCE_BOARD:
+			balance_board_disconnected(&wm->exp.bb);
+			wm->event = WIIUSE_BALANCE_BOARD_CTRL_REMOVED;
+			break;
 		default:
 			break;
 	}
@@ -771,6 +785,9 @@ static void save_state(struct wiimote_t* wm) {
 			wm->lstate.exp_btns = wm->exp.gh3.btns;
 			break;
 
+		case EXP_BALANCE_BOARD:
+			wm->lstate.exp_bb_raw = wm->exp.bb.raw;
+
 		case EXP_NONE:
 			break;
 	}
@@ -866,6 +883,14 @@ static int state_changed(struct wiimote_t* wm) {
 			STATE_CHANGED(wm->lstate.exp_btns, wm->exp.gh3.btns);
 			break;
 		}
+		case EXP_BALANCE_BOARD:
+		{
+			STATE_CHANGED(wm->lstate.exp_bb_raw.tr, wm->exp.bb.raw.tr);
+			STATE_CHANGED(wm->lstate.exp_bb_raw.br, wm->exp.bb.raw.br);
+			STATE_CHANGED(wm->lstate.exp_bb_raw.tl, wm->exp.bb.raw.tl);
+			STATE_CHANGED(wm->lstate.exp_bb_raw.bl, wm->exp.bb.raw.bl);
+			break;
+		}
 		case EXP_NONE:
 		{
 			break;

src/ir.c

@@ -623,11 +623,10 @@ static void reorder_ir_dots(struct ir_dot_t* dot) {
 		dot[i].order = 0;
 
 	for (order = 1; order < 5; ++order) {
-		i = 0;
-
-		for (; !dot[i].visible || dot[i].order; ++i)
-		if (i > 4)
+		for (i = 0; i < 4 && (!dot[i].visible || dot[i].order); ++i) {
+			if (i >= 4)
 			return;
+		}
 
 		for (j = 0; j < 4; ++j) {
 			if (dot[j].visible && !dot[j].order && (dot[j].x < dot[i].x))