From 3155a09fd5060190eea4f2585d55d6c5431837c4 Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Mon, 17 Sep 2012 12:31:36 +0200 Subject: [PATCH] HID: wiimote: Parse calibration data of balance boards The raw pressure-data that is reported by balance-boards is pretty useless unless calibration data is applied. Therefore, we read the full calibration data on extension initialization and apply it to every reported data. Signed-off-by: David Herrmann Signed-off-by: Florian Echtler Signed-off-by: Jiri Kosina --- drivers/hid/hid-wiimote-ext.c | 39 ++++++++++++++++++++++++++++++++++- 1 file changed, 38 insertions(+), 1 deletion(-) diff --git a/drivers/hid/hid-wiimote-ext.c b/drivers/hid/hid-wiimote-ext.c index ae022b86359..fc98cc9ed42 100644 --- a/drivers/hid/hid-wiimote-ext.c +++ b/drivers/hid/hid-wiimote-ext.c @@ -28,6 +28,7 @@ struct wiimote_ext { bool mp_plugged; bool motionp; __u8 ext_type; + __u16 calib[4][3]; }; enum wiiext_type { @@ -127,6 +128,7 @@ error: static __u8 ext_read(struct wiimote_ext *ext) { ssize_t ret; + __u8 buf[24], i, j, offs = 0; __u8 rmem[2], wmem; __u8 type = WIIEXT_NONE; @@ -156,6 +158,26 @@ static __u8 ext_read(struct wiimote_ext *ext) type = WIIEXT_BALANCE_BOARD; } + /* get balance board calibration data */ + if (type == WIIEXT_BALANCE_BOARD) { + ret = wiimote_cmd_read(ext->wdata, 0xa40024, buf, 12); + ret += wiimote_cmd_read(ext->wdata, 0xa40024 + 12, + buf + 12, 12); + + if (ret != 24) { + type = WIIEXT_NONE; + } else { + for (i = 0; i < 3; i++) { + for (j = 0; j < 4; j++) { + ext->calib[j][i] = buf[offs]; + ext->calib[j][i] <<= 8; + ext->calib[j][i] |= buf[offs + 1]; + offs += 2; + } + } + } + } + wiimote_cmd_release(ext->wdata); return type; @@ -514,7 +536,8 @@ static void handler_classic(struct wiimote_ext *ext, const __u8 *payload) static void handler_balance_board(struct wiimote_ext *ext, const __u8 *payload) { - __s32 val[4]; + __s32 val[4], tmp; + unsigned int i; /* Byte | 8 7 6 5 4 3 2 1 | * -----+--------------------------+ @@ -553,6 +576,20 @@ static void handler_balance_board(struct wiimote_ext *ext, const __u8 *payload) val[3] <<= 8; val[3] |= payload[7]; + /* apply calibration data */ + for (i = 0; i < 4; i++) { + if (val[i] < ext->calib[i][1]) { + tmp = val[i] - ext->calib[i][0]; + tmp *= 1700; + tmp /= ext->calib[i][1] - ext->calib[i][0]; + } else { + tmp = val[i] - ext->calib[i][1]; + tmp *= 1700; + tmp /= ext->calib[i][2] - ext->calib[i][1] + 1700; + } + val[i] = tmp; + } + input_report_abs(ext->input, ABS_HAT0X, val[0]); input_report_abs(ext->input, ABS_HAT0Y, val[1]); input_report_abs(ext->input, ABS_HAT1X, val[2]); -- 2.41.0