Balance Board fixes and features

gc/wiiuse/wiiuse.h

@@ -515,25 +515,39 @@ typedef struct guitar_hero_3_t {
 	struct joystick_t js;			/**< joystick calibration					*/
 } guitar_hero_3_t;
 
+enum wii_board_sensor_id_t {
+	WII_BOARD_SENSOR_ID_TOP_RIGHT,
+	WII_BOARD_SENSOR_ID_BOT_RIGHT,
+	WII_BOARD_SENSOR_ID_TOP_LEFT,
+	WII_BOARD_SENSOR_ID_BOT_LEFT,
+	WII_BOARD_NUM_SENSORS
+};
+
+typedef struct wii_board_sensor_cal_t {
+	short ref_0;
+	short ref_17;
+	short ref_34;
+} wii_board_sensor_cal_t;
+
 /**
   * @struct wii_board_t
   * @brief Wii Balance Board expansion device.
   */
 typedef struct wii_board_t {
-	float tl;  /* Interpolated */
-	float tr;
-	float bl;
-	float br;  /* End interp */
-	short rtl; /* RAW */
-	short rtr;
-	short rbl;
-	short rbr; /* /RAW */
-	short ctl[3]; /* Calibration */
-	short ctr[3];
-	short cbl[3];
-	short cbr[3]; /* /Calibration */
-	float x;
-	float y;
+	/* RAW */
+	short raw_sensor[WII_BOARD_NUM_SENSORS];
+	ubyte raw_temp;
+	ubyte raw_bat;
+	/* Calibration */
+	wii_board_sensor_cal_t cal_sensor[WII_BOARD_NUM_SENSORS];
+	ubyte cal_temp;
+	ubyte cal_bat;	/**< always 0x69 */
+	/* Calculated */
+	float weight[WII_BOARD_NUM_SENSORS];
+	float total_weight;
+	float x;	/**< normalized to [-1, +1], positive is right */
+	float y;	/**< normalized [-1, +1], positive is up */
+	ubyte battery;	/**< number of battery bars, from 0 to 4 */
 } wii_board_t;
 
 typedef struct motion_plus_t

wiiuse/dynamics.c

@@ -225,49 +225,74 @@ void apply_smoothing(struct accel_t* ac, struct orient_t* orient, int type) {
 	}
 }
 
-void calc_balanceboard_state(struct wii_board_t *wb)
+static float calc_balanceboard_weight(wii_board_t *wb, int idx)
 {
-	/* 
-	Interpolate values 
-	Calculations borrowed from wiili.org - No names to mention sadly :( http://www.wiili.org/index.php/Wii_Balance_Board_PC_Drivers
-	*/
-
-	if(wb->rtr<wb->ctr[1])
-	{
-		wb->tr = 17.0f*(f32)(wb->rtr-wb->ctr[0])/(f32)(wb->ctr[1]-wb->ctr[0]);
-	}
+	/*
+	 * Calculations borrowed from wiili.org - No names to mention sadly :(
+	 * http://www.wiili.org/index.php/Wii_Balance_Board_PC_Drivers
+	 */
+	short raw = wb->raw_sensor[idx];
+	const wii_board_sensor_cal_t *cal = &wb->cal_sensor[idx];
+	if (raw < cal->ref_17)
+		return 17.0f * (raw - cal->ref_0 ) / (cal->ref_17 - cal->ref_0 );
 	else
-	{
-		wb->tr = 17.0f*(f32)(wb->rtr-wb->ctr[1])/(f32)(wb->ctr[2]-wb->ctr[1]) + 17.0f;
-	}
+		return 17.0f * (raw - cal->ref_17) / (cal->ref_34 - cal->ref_17) + 17.0f;
+}
 
-	if(wb->rtl<wb->ctl[1])
-	{
-		wb->tl = 17.0f*(f32)(wb->rtl-wb->ctl[0])/(f32)(wb->ctl[1]-wb->ctl[0]);
-	}
-	else
-	{
-		wb->tl = 17.0f*(f32)(wb->rtl-wb->ctl[1])/(f32)(wb->ctl[2]-wb->ctl[1]) + 17.0f;
-	}
+static float calc_balanceboard_temp_correct(float w, const wii_board_t* wb)
+{
+	/* Temperature correction, based on https://wiibrew.org/wiki/Wii_Balance_Board */
+	static const double a = 0.9990813732147217;
+	static const double b = 0.007000000216066837;
+	double c = 1.0 - b * (wb->raw_temp - wb->cal_temp) / 10.0;
+	return a * w * c;
+}
 
-	if(wb->rbr<wb->cbr[1])
-	{
-		wb->br = 17.0f*(f32)(wb->rbr-wb->cbr[0])/(f32)(wb->cbr[1]-wb->cbr[0]);
-	}
-	else
-	{
-		wb->br = 17.0f*(f32)(wb->rbr-wb->cbr[1])/(f32)(wb->cbr[2]-wb->cbr[1]) + 17.0f;
-	}
+static unsigned calc_balanceboard_battery_bars(const struct wii_board_t *wb)
+{
+    if (wb->raw_bat >= 0x82)
+	return 4;
+    if (wb->raw_bat >= 0x7d)
+	return 3;
+    if (wb->raw_bat >= 0x78)
+	return 2;
+    if (wb->raw_bat > wb->cal_bat)
+	return 1;
+    return 0;
+}
 
-	if(wb->rbl<wb->cbl[1])
-	{
-		wb->bl = 17.0f*(f32)(wb->rbl-wb->cbl[0])/(f32)(wb->cbl[1]-wb->cbl[0]);
+void calc_balanceboard_state(struct wii_board_t *wb)
+{
+	/* Convert raw sensor data to Kg. */
+	for (unsigned i = 0; i < WII_BOARD_NUM_SENSORS; ++i)
+		wb->weight[i] = calc_balanceboard_weight(wb, i);
+
+	/* Apply temperature and zero correction. */
+	for (unsigned i = 0; i < WII_BOARD_NUM_SENSORS; ++i)
+		wb->weight[i] = calc_balanceboard_temp_correct(wb->weight[i], wb);
+
+	wb->total_weight = 0;
+	for (unsigned i = 0; i < WII_BOARD_NUM_SENSORS; ++i)
+		wb->total_weight += wb->weight[i];
+
+	/* Calculate x,y members only if there's at least 2 Kg on the board. */
+	if (wb->total_weight >= 2.0f) {
+		wb->x = ((wb->weight[WII_BOARD_SENSOR_ID_TOP_RIGHT] +
+			  wb->weight[WII_BOARD_SENSOR_ID_BOT_RIGHT])
+			 -
+			 (wb->weight[WII_BOARD_SENSOR_ID_TOP_LEFT] +
+			  wb->weight[WII_BOARD_SENSOR_ID_BOT_LEFT]))
+			/ wb->total_weight;
+		wb->y = ((wb->weight[WII_BOARD_SENSOR_ID_TOP_RIGHT] +
+			  wb->weight[WII_BOARD_SENSOR_ID_TOP_LEFT])
+			 -
+			 (wb->weight[WII_BOARD_SENSOR_ID_BOT_RIGHT] +
+			  wb->weight[WII_BOARD_SENSOR_ID_BOT_LEFT]))
+			/ wb->total_weight;
+	} else {
+		wb->x = 0;
+		wb->y = 0;
 	}
-	else
-	{
-		wb->bl = 17.0f*(f32)(wb->rbl-wb->cbl[1])/(f32)(wb->cbl[2]-wb->cbl[1]) + 17.0f;
-	}	 
 
-	wb->x = ((wb->tr+wb->br) - (wb->tl+wb->bl))/2.0f;
-	wb->y = ((wb->bl+wb->br) - (wb->tl+wb->tr))/2.0f;
+	wb->battery = calc_balanceboard_battery_bars(wb);
 }

wiiuse/dynamics.h

@@ -47,8 +47,8 @@ extern "C" {
 void calculate_orientation(struct accel_t* ac, struct vec3w_t* accel, struct orient_t* orient, int smooth);
 void calculate_gforce(struct accel_t* ac, struct vec3w_t* accel, struct gforce_t* gforce);
 void calc_joystick_state(struct joystick_t* js, float x, float y);
-void calc_balanceboard_state(struct wii_board_t *wb);
 void apply_smoothing(struct accel_t* ac, struct orient_t* orient, int type);
+void calc_balanceboard_state(struct wii_board_t *wb);
 
 #ifdef __cplusplus
 }

wiiuse/wiiboard.c

@@ -47,6 +47,11 @@
 #include "wiiboard.h"
 #include "io.h"
 
+static u16 load_be_u16(u8* data)
+{
+	return ((u16)(data[0] << 8)) | ((u16)data[1]);
+}
+
 /**
  *	@brief Handle the handshake data from the wiiboard.
  *
@@ -58,36 +63,40 @@
  */
 int wii_board_handshake(struct wiimote_t* wm, struct wii_board_t* wb, ubyte* data, uword len) 
 {
-	int offset = 0;
+	unsigned offset = 0;
 
-	if (data[offset]==0xff) {
-		if (data[offset+16]==0xff) {
+	if (data[offset] == 0xff) {
+		if (data[offset + 0x10] == 0xff) {
 			WIIUSE_DEBUG("Wii Balance Board handshake appears invalid, trying again.");
 			wiiuse_read_data(wm, data, WM_EXP_MEM_CALIBR, EXP_HANDSHAKE_LEN, wiiuse_handshake_expansion);
 			return 0;
 		}
-		offset += 16;
+		offset += 0x10;
 	}
 
-	wb->ctr[0] = (data[offset+4]<<8)|data[offset+5];
-	wb->cbr[0] = (data[offset+6]<<8)|data[offset+7];
-	wb->ctl[0] = (data[offset+8]<<8)|data[offset+9];
-	wb->cbl[0] = (data[offset+10]<<8)|data[offset+11];
+
+	for (unsigned i = 0; i < WII_BOARD_NUM_SENSORS; ++i) {
+		/* The reference values for 0 Kg */
+		wb->cal_sensor[i].ref_0  = load_be_u16(data + offset + 0x04 + 2*i);
+		/* The reference values for 17 Kg */
+		wb->cal_sensor[i].ref_17 = load_be_u16(data + offset + 0x0c + 2*i);
+		/* The reference values for 34 Kg */
+		wb->cal_sensor[i].ref_34 = load_be_u16(data + offset + 0x14 + 2*i);
+	}
 
-	wb->ctr[1] = (data[offset+12]<<8)|data[offset+13];
-	wb->cbr[1] = (data[offset+14]<<8)|data[offset+15];
-	wb->ctl[1] = (data[offset+16]<<8)|data[offset+17];
-	wb->cbl[1] = (data[offset+18]<<8)|data[offset+19];
+	/* The critical battery value (always 0x69). */
+	wb->cal_bat = data[offset + 0x01];
 
-	wb->ctr[2] = (data[offset+20]<<8)|data[offset+21];
-	wb->cbr[2] = (data[offset+22]<<8)|data[offset+23];
-	wb->ctl[2] = (data[offset+24]<<8)|data[offset+25];
-	wb->cbl[2] = (data[offset+26]<<8)|data[offset+27];
+	/* The reference temperature. */
+        wb->cal_temp = data[offset + 0x40];
 
 	/* handshake done */
 	wm->event = WIIUSE_WII_BOARD_INSERTED;
 	wm->exp.type = EXP_WII_BOARD;
 
+	/* Balance Board only has the first LED, so make sure it's on. */
+	wiiuse_set_leds(wm, WIIMOTE_LED_1, NULL);
+
 	return 1; 
 }
 
@@ -110,9 +119,9 @@ void wii_board_disconnected(struct wii_board_t* wb)
  *	@param msg		The message specified in the event packet.
  */
 void wii_board_event(struct wii_board_t* wb, ubyte* msg)
-{ 
-	wb->rtr = (msg[0]<<8)|msg[1];
-	wb->rbr = (msg[2]<<8)|msg[3];
-	wb->rtl = (msg[4]<<8)|msg[5];
-	wb->rbl = (msg[6]<<8)|msg[7];	
+{
+	for (unsigned i = 0; i < WII_BOARD_NUM_SENSORS; ++i)
+		wb->raw_sensor[i] = load_be_u16(msg + 2*i);
+	wb->raw_temp = msg[0x8];
+	wb->raw_bat = msg[0xa];
 }

wiiuse/wpad.c

@@ -619,10 +619,10 @@ static u32 __wpad_read_expansion(struct wiimote_t *wm,WPADData *data, struct _wp
 			break;
  		case EXP_WII_BOARD:
 			data->exp.wb = wm->exp.wb;
-			STATE_CHECK(thresh->wb,wm->exp.wb.rtl,wm->lstate.exp.wb.rtl);
-			STATE_CHECK(thresh->wb,wm->exp.wb.rtr,wm->lstate.exp.wb.rtr);
-			STATE_CHECK(thresh->wb,wm->exp.wb.rbl,wm->lstate.exp.wb.rbl);
-			STATE_CHECK(thresh->wb,wm->exp.wb.rbr,wm->lstate.exp.wb.rbr);
+			for (unsigned i = 0; i < WII_BOARD_NUM_SENSORS; ++i)
+				STATE_CHECK(thresh->wb,
+					    wm->exp.wb.raw_sensor[i],
+					    wm->lstate.exp.wb.raw_sensor[i]);
  			break;
 		case EXP_MOTION_PLUS:
 			data->exp.mp = wm->exp.mp;