From patchwork Fri Feb 3 00:21:01 2017 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Roderick Colenbrander X-Patchwork-Id: 9553377 Return-Path: Received: from mail.wl.linuxfoundation.org (pdx-wl-mail.web.codeaurora.org [172.30.200.125]) by pdx-korg-patchwork.web.codeaurora.org (Postfix) with ESMTP id 16D4060453 for ; Fri, 3 Feb 2017 00:21:36 +0000 (UTC) Received: from mail.wl.linuxfoundation.org (localhost [127.0.0.1]) by mail.wl.linuxfoundation.org (Postfix) with ESMTP id 05C1C21F61 for ; Fri, 3 Feb 2017 00:21:36 +0000 (UTC) Received: by mail.wl.linuxfoundation.org (Postfix, from userid 486) id EEA0B25D9E; Fri, 3 Feb 2017 00:21:35 +0000 (UTC) X-Spam-Checker-Version: SpamAssassin 3.3.1 (2010-03-16) on pdx-wl-mail.web.codeaurora.org X-Spam-Level: X-Spam-Status: No, score=-6.3 required=2.0 tests=BAYES_00,DKIM_SIGNED, RCVD_IN_DNSWL_HI, RCVD_IN_SORBS_SPAM, T_DKIM_INVALID autolearn=ham version=3.3.1 Received: from vger.kernel.org (vger.kernel.org [209.132.180.67]) by mail.wl.linuxfoundation.org (Postfix) with ESMTP id 2D42521F61 for ; Fri, 3 Feb 2017 00:21:35 +0000 (UTC) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S1751595AbdBCAVe (ORCPT ); Thu, 2 Feb 2017 19:21:34 -0500 Received: from mail-pg0-f43.google.com ([74.125.83.43]:35620 "EHLO mail-pg0-f43.google.com" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S1751570AbdBCAVc (ORCPT ); Thu, 2 Feb 2017 19:21:32 -0500 Received: by mail-pg0-f43.google.com with SMTP id 194so1226404pgd.2 for ; Thu, 02 Feb 2017 16:21:32 -0800 (PST) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=gaikai-com.20150623.gappssmtp.com; s=20150623; h=from:to:cc:subject:date:message-id:in-reply-to:references; bh=My4Xtk8NWVe1ohotHnjarqerGiPbWu7MUyvoYeX+LXo=; b=rjPjxvLr4SGKB4r+zvnMCBYFP3OFBVEPHffGMFsj26TBKQGS1G7odaRRS4FcRTC0xI RdVAlm1GKU79ajkNmRCFyYigLRFVD/mEOC/KD8InyuQV3xB/Ta+pUJceMBl40CJV8JUv 2z1fSzYHCJ1dX6XyXLZNdn9D/2dqq2ZIOf1phyR3kChDrpo8NnphJ6INPvI9zSp3kb6a mkaI71YoNmWXvj5QJAwmEoDoWgfO6QDF4tQHVwt9m0Crh4trZs2I+e9E0GpG5MQiYGKV DeehKHvkr2ActFOK+7S47d6aqCrCjBmy9F9/OPY39pMoi/Gs9WGvlj0DKBUFr8Up29IW twpg== X-Google-DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=1e100.net; s=20161025; h=x-gm-message-state:from:to:cc:subject:date:message-id:in-reply-to :references; bh=My4Xtk8NWVe1ohotHnjarqerGiPbWu7MUyvoYeX+LXo=; b=rx30MmMRAZms6qBZ6HmIZnwCCmVkYW3DoUlt6JmQEZZ7PIUimnBzUBJFIUJ3Dpy+/a EHsvwVbltBtZNzu0yhQZJUPLb4jBn8Q+6ylDTNT9tJTxvv2U+WyHU7doXWUtrlOlVjJZ gwuQejpp2YvZBes+JbMmPGSyWdVNQUKS3xCLJmYWHFrCmlbZhJldmP7umTdQNk023+nQ 81qksWeHwzpxC75p64vGa7ebfV8GnMHO/LSsUnUjwMLhKVo4Z7mw7OJadu6/uthpx3b0 Af02Kq5KeYLQvObn/PLYrRV7cbtzB+aeob9B3yDjkHlrWinfy7NCP+o0nIO9vhSlkWqV esSw== X-Gm-Message-State: AIkVDXLdnysWNvpcw272htMZE6Ew0eu7pSzNSd9ksrP3grGf2wFkSY6Grody1qNcwcfHr9i7 X-Received: by 10.84.214.16 with SMTP id h16mr16815370pli.11.1486081291525; Thu, 02 Feb 2017 16:21:31 -0800 (PST) Received: from konan1.dev.biz ([100.42.98.197]) by smtp.gmail.com with ESMTPSA id x81sm61343282pff.69.2017.02.02.16.21.30 (version=TLS1_2 cipher=ECDHE-RSA-AES128-GCM-SHA256 bits=128/128); Thu, 02 Feb 2017 16:21:31 -0800 (PST) From: Roderick Colenbrander To: linux-input@vger.kernel.org Cc: Dmitry Torokhov , Jiri Kosina , Benjamin Tissoires , Simon Wood , Frank Praznik , Tim Bird , Roderick Colenbrander Subject: [PATCH v3 2/7] HID: sony: Calibrate DS4 motion sensors Date: Thu, 2 Feb 2017 16:21:01 -0800 Message-Id: <20170203002106.23225-3-roderick@gaikai.com> X-Mailer: git-send-email 2.9.3 In-Reply-To: <20170203002106.23225-1-roderick@gaikai.com> References: <20170203002106.23225-1-roderick@gaikai.com> Sender: linux-input-owner@vger.kernel.org Precedence: bulk List-ID: X-Mailing-List: linux-input@vger.kernel.org X-Virus-Scanned: ClamAV using ClamSMTP From: Roderick Colenbrander The DS4 motion sensors require calibration for accurate operation. This patch adds calibration for both the accelerometer and the gyroscope. Calibration requires reading device specific scaling factors and offsets. For precision reasons we store these values as a numerator and denominator and apply the values when processing the data. Signed-off-by: Roderick Colenbrander --- drivers/hid/hid-sony.c | 212 ++++++++++++++++++++++++++++++++++++++++++------- 1 file changed, 184 insertions(+), 28 deletions(-) diff --git a/drivers/hid/hid-sony.c b/drivers/hid/hid-sony.c index 1385ac1..a7a3692 100644 --- a/drivers/hid/hid-sony.c +++ b/drivers/hid/hid-sony.c @@ -768,6 +768,7 @@ struct motion_output_report_02 { }; #define DS4_FEATURE_REPORT_0x02_SIZE 37 +#define DS4_FEATURE_REPORT_0x05_SIZE 41 #define DS4_FEATURE_REPORT_0x81_SIZE 7 #define DS4_INPUT_REPORT_0x11_SIZE 78 #define DS4_OUTPUT_REPORT_0x05_SIZE 32 @@ -787,10 +788,25 @@ struct motion_output_report_02 { #define DS4_SENSOR_SUFFIX " Motion Sensors" #define DS4_TOUCHPAD_SUFFIX " Touchpad" +#define DS4_GYRO_RES_PER_DEG_S 1024 +#define DS4_ACC_RES_PER_G 8192 + static DEFINE_SPINLOCK(sony_dev_list_lock); static LIST_HEAD(sony_device_list); static DEFINE_IDA(sony_device_id_allocator); +/* Used for calibration of DS4 accelerometer and gyro. */ +struct ds4_calibration_data { + int abs_code; + short bias; + /* Calibration requires scaling against a sensitivity value, which is a + * float. Store sensitivity as a fraction to limit floating point + * calculations until final calibration. + */ + int sens_numer; + int sens_denom; +}; + struct sony_sc { spinlock_t lock; struct list_head list_node; @@ -823,6 +839,8 @@ struct sony_sc { u8 led_delay_off[MAX_LEDS]; u8 led_count; bool ds4_dongle_connected; + /* DS4 calibration data */ + struct ds4_calibration_data ds4_calib_data[6]; }; static void sony_set_leds(struct sony_sc *sc); @@ -1015,9 +1033,6 @@ static void dualshock4_parse_report(struct sony_sc *sc, u8 *rd, int size) int n, m, offset, num_touch_data, max_touch_data; u8 cable_state, battery_capacity, battery_charging; - /* Order of hw axes is gyro first, then accelerometer. */ - int axes[6] = {ABS_RX, ABS_RY, ABS_RZ, ABS_X, ABS_Y, ABS_Z}; - /* When using Bluetooth the header is 2 bytes longer, so skip these. */ int data_offset = (sc->quirks & DUALSHOCK4_CONTROLLER_USB) ? 0 : 2; @@ -1026,10 +1041,22 @@ static void dualshock4_parse_report(struct sony_sc *sc, u8 *rd, int size) input_report_key(sc->touchpad, BTN_LEFT, rd[offset+2] & 0x2); offset = data_offset + DS4_INPUT_REPORT_GYRO_X_OFFSET; - for (n = 0; n < 6; n++, offset += 2) { - short value = get_unaligned_le16(&rd[offset]); + for (n = 0; n < 6; n++) { + /* Store data in int for more precision during mult_frac. */ + int raw_data = (short)((rd[offset+1] << 8) | rd[offset]); + struct ds4_calibration_data *calib = &sc->ds4_calib_data[n]; + + /* High precision is needed during calibration, but the + * calibrated values are within 32-bit. + * Note: we swap numerator 'x' and 'numer' in mult_frac for + * precision reasons so we don't need 64-bit. + */ + int calib_data = mult_frac(calib->sens_numer, + raw_data - calib->bias, + calib->sens_denom); - input_report_abs(sc->sensor_dev, axes[n], value); + input_report_abs(sc->sensor_dev, calib->abs_code, calib_data); + offset += 2; } input_sync(sc->sensor_dev); @@ -1316,6 +1343,7 @@ static int sony_register_sensors(struct sony_sc *sc) size_t name_sz; char *name; int ret; + int range; sc->sensor_dev = input_allocate_device(); if (!sc->sensor_dev) @@ -1342,13 +1370,21 @@ static int sony_register_sensors(struct sony_sc *sc) snprintf(name, name_sz, "%s" DS4_SENSOR_SUFFIX, sc->hdev->name); sc->sensor_dev->name = name; - input_set_abs_params(sc->sensor_dev, ABS_X, -32768, 32767, 0, 0); - input_set_abs_params(sc->sensor_dev, ABS_Y, -32768, 32767, 0, 0); - input_set_abs_params(sc->sensor_dev, ABS_Z, -32768, 32767, 0, 0); - - input_set_abs_params(sc->sensor_dev, ABS_RX, -32768, 32767, 0, 0); - input_set_abs_params(sc->sensor_dev, ABS_RY, -32768, 32767, 0, 0); - input_set_abs_params(sc->sensor_dev, ABS_RZ, -32768, 32767, 0, 0); + range = DS4_ACC_RES_PER_G*4; + input_set_abs_params(sc->sensor_dev, ABS_X, -range, range, 16, 0); + input_set_abs_params(sc->sensor_dev, ABS_Y, -range, range, 16, 0); + input_set_abs_params(sc->sensor_dev, ABS_Z, -range, range, 16, 0); + input_abs_set_res(sc->sensor_dev, ABS_X, DS4_ACC_RES_PER_G); + input_abs_set_res(sc->sensor_dev, ABS_Y, DS4_ACC_RES_PER_G); + input_abs_set_res(sc->sensor_dev, ABS_Z, DS4_ACC_RES_PER_G); + + range = DS4_GYRO_RES_PER_DEG_S*2048; + input_set_abs_params(sc->sensor_dev, ABS_RX, -range, range, 16, 0); + input_set_abs_params(sc->sensor_dev, ABS_RY, -range, range, 16, 0); + input_set_abs_params(sc->sensor_dev, ABS_RZ, -range, range, 16, 0); + input_abs_set_res(sc->sensor_dev, ABS_RX, DS4_GYRO_RES_PER_DEG_S); + input_abs_set_res(sc->sensor_dev, ABS_RY, DS4_GYRO_RES_PER_DEG_S); + input_abs_set_res(sc->sensor_dev, ABS_RZ, DS4_GYRO_RES_PER_DEG_S); __set_bit(INPUT_PROP_ACCELEROMETER, sc->sensor_dev->propbit); @@ -1446,23 +1482,145 @@ static int sixaxis_set_operational_bt(struct hid_device *hdev) } /* - * Requesting feature report 0x02 in Bluetooth mode changes the state of the - * controller so that it sends full input reports of type 0x11. + * Request DS4 calibration data for the motion sensors. + * For Bluetooth this also affects the operating mode (see below). */ -static int dualshock4_set_operational_bt(struct hid_device *hdev) +static int dualshock4_get_calibration_data(struct sony_sc *sc) { u8 *buf; int ret; + short gyro_pitch_bias, gyro_pitch_plus, gyro_pitch_minus; + short gyro_yaw_bias, gyro_yaw_plus, gyro_yaw_minus; + short gyro_roll_bias, gyro_roll_plus, gyro_roll_minus; + short gyro_speed_plus, gyro_speed_minus; + short acc_x_plus, acc_x_minus; + short acc_y_plus, acc_y_minus; + short acc_z_plus, acc_z_minus; + int speed_2x; + int range_2g; + + /* For Bluetooth we use a different request, which supports CRC. + * Note: in Bluetooth mode feature report 0x02 also changes the state + * of the controller, so that it sends input reports of type 0x11. + */ + if (sc->quirks & DUALSHOCK4_CONTROLLER_USB) { + buf = kmalloc(DS4_FEATURE_REPORT_0x02_SIZE, GFP_KERNEL); + if (!buf) + return -ENOMEM; - buf = kmalloc(DS4_FEATURE_REPORT_0x02_SIZE, GFP_KERNEL); - if (!buf) - return -ENOMEM; + ret = hid_hw_raw_request(sc->hdev, 0x02, buf, + DS4_FEATURE_REPORT_0x02_SIZE, + HID_FEATURE_REPORT, + HID_REQ_GET_REPORT); + if (ret < 0) + goto err_stop; + } else { + u8 bthdr = 0xA3; + u32 crc; + u32 report_crc; + int retries; - ret = hid_hw_raw_request(hdev, 0x02, buf, DS4_FEATURE_REPORT_0x02_SIZE, - HID_FEATURE_REPORT, HID_REQ_GET_REPORT); + buf = kmalloc(DS4_FEATURE_REPORT_0x05_SIZE, GFP_KERNEL); + if (!buf) + return -ENOMEM; - kfree(buf); + for (retries = 0; retries < 3; retries++) { + ret = hid_hw_raw_request(sc->hdev, 0x05, buf, + DS4_FEATURE_REPORT_0x05_SIZE, + HID_FEATURE_REPORT, + HID_REQ_GET_REPORT); + if (ret < 0) + goto err_stop; + /* CRC check */ + crc = crc32_le(0xFFFFFFFF, &bthdr, 1); + crc = ~crc32_le(crc, buf, DS4_FEATURE_REPORT_0x05_SIZE-4); + report_crc = get_unaligned_le32(&buf[DS4_FEATURE_REPORT_0x05_SIZE-4]); + if (crc != report_crc) { + hid_warn(sc->hdev, "DualShock 4 calibration report's CRC check failed, received crc 0x%0x != 0x%0x\n", + report_crc, crc); + if (retries < 2) { + hid_warn(sc->hdev, "Retrying DualShock 4 get calibration report request\n"); + continue; + } else { + ret = -EILSEQ; + goto err_stop; + } + } else { + break; + } + } + } + + gyro_pitch_bias = get_unaligned_le16(&buf[1]); + gyro_yaw_bias = get_unaligned_le16(&buf[3]); + gyro_roll_bias = get_unaligned_le16(&buf[5]); + if (sc->quirks & DUALSHOCK4_CONTROLLER_USB) { + gyro_pitch_plus = get_unaligned_le16(&buf[7]); + gyro_pitch_minus = get_unaligned_le16(&buf[9]); + gyro_yaw_plus = get_unaligned_le16(&buf[11]); + gyro_yaw_minus = get_unaligned_le16(&buf[13]); + gyro_roll_plus = get_unaligned_le16(&buf[15]); + gyro_roll_minus = get_unaligned_le16(&buf[17]); + } else { + gyro_pitch_plus = get_unaligned_le16(&buf[7]); + gyro_yaw_plus = get_unaligned_le16(&buf[9]); + gyro_roll_plus = get_unaligned_le16(&buf[11]); + gyro_pitch_minus = get_unaligned_le16(&buf[13]); + gyro_yaw_minus = get_unaligned_le16(&buf[15]); + gyro_roll_minus = get_unaligned_le16(&buf[17]); + } + gyro_speed_plus = get_unaligned_le16(&buf[19]); + gyro_speed_minus = get_unaligned_le16(&buf[21]); + acc_x_plus = get_unaligned_le16(&buf[23]); + acc_x_minus = get_unaligned_le16(&buf[25]); + acc_y_plus = get_unaligned_le16(&buf[27]); + acc_y_minus = get_unaligned_le16(&buf[29]); + acc_z_plus = get_unaligned_le16(&buf[31]); + acc_z_minus = get_unaligned_le16(&buf[33]); + + /* Set gyroscope calibration and normalization parameters. + * Data values will be normalized to 1/DS4_GYRO_RES_PER_DEG_S degree/s. + */ + speed_2x = (gyro_speed_plus + gyro_speed_minus); + sc->ds4_calib_data[0].abs_code = ABS_RX; + sc->ds4_calib_data[0].bias = gyro_pitch_bias; + sc->ds4_calib_data[0].sens_numer = speed_2x*DS4_GYRO_RES_PER_DEG_S; + sc->ds4_calib_data[0].sens_denom = gyro_pitch_plus - gyro_pitch_minus; + + sc->ds4_calib_data[1].abs_code = ABS_RY; + sc->ds4_calib_data[1].bias = gyro_yaw_bias; + sc->ds4_calib_data[1].sens_numer = speed_2x*DS4_GYRO_RES_PER_DEG_S; + sc->ds4_calib_data[1].sens_denom = gyro_yaw_plus - gyro_yaw_minus; + + sc->ds4_calib_data[2].abs_code = ABS_RZ; + sc->ds4_calib_data[2].bias = gyro_roll_bias; + sc->ds4_calib_data[2].sens_numer = speed_2x*DS4_GYRO_RES_PER_DEG_S; + sc->ds4_calib_data[2].sens_denom = gyro_roll_plus - gyro_roll_minus; + + /* Set accelerometer calibration and normalization parameters. + * Data values will be normalized to 1/DS4_ACC_RES_PER_G G. + */ + range_2g = acc_x_plus - acc_x_minus; + sc->ds4_calib_data[3].abs_code = ABS_X; + sc->ds4_calib_data[3].bias = acc_x_plus - range_2g / 2; + sc->ds4_calib_data[3].sens_numer = 2*DS4_ACC_RES_PER_G; + sc->ds4_calib_data[3].sens_denom = range_2g; + + range_2g = acc_y_plus - acc_y_minus; + sc->ds4_calib_data[4].abs_code = ABS_Y; + sc->ds4_calib_data[4].bias = acc_y_plus - range_2g / 2; + sc->ds4_calib_data[4].sens_numer = 2*DS4_ACC_RES_PER_G; + sc->ds4_calib_data[4].sens_denom = range_2g; + + range_2g = acc_z_plus - acc_z_minus; + sc->ds4_calib_data[5].abs_code = ABS_Z; + sc->ds4_calib_data[5].bias = acc_z_plus - range_2g / 2; + sc->ds4_calib_data[5].sens_numer = 2*DS4_ACC_RES_PER_G; + sc->ds4_calib_data[5].sens_denom = range_2g; + +err_stop: + kfree(buf); return ret; } @@ -2365,12 +2523,10 @@ static int sony_input_configured(struct hid_device *hdev, ret = sixaxis_set_operational_bt(hdev); sony_init_output_report(sc, sixaxis_send_output_report); } else if (sc->quirks & DUALSHOCK4_CONTROLLER) { - if (sc->quirks & DUALSHOCK4_CONTROLLER_BT) { - ret = dualshock4_set_operational_bt(hdev); - if (ret < 0) { - hid_err(hdev, "failed to set the Dualshock 4 operational mode\n"); - goto err_stop; - } + ret = dualshock4_get_calibration_data(sc); + if (ret < 0) { + hid_err(hdev, "Failed to get calibration data from Dualshock 4\n"); + goto err_stop; } /*