From patchwork Wed Nov 23 22:07:12 2016 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Roderick Colenbrander X-Patchwork-Id: 9444427 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 6CAE360779 for ; Wed, 23 Nov 2016 22:08:39 +0000 (UTC) Received: from mail.wl.linuxfoundation.org (localhost [127.0.0.1]) by mail.wl.linuxfoundation.org (Postfix) with ESMTP id 6565B271CB for ; Wed, 23 Nov 2016 22:08:39 +0000 (UTC) Received: by mail.wl.linuxfoundation.org (Postfix, from userid 486) id 5909627D76; Wed, 23 Nov 2016 22:08:39 +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.8 required=2.0 tests=BAYES_00,DKIM_SIGNED, RCVD_IN_DNSWL_HI,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 70D5827D7F for ; Wed, 23 Nov 2016 22:08:38 +0000 (UTC) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S933548AbcKWWId (ORCPT ); Wed, 23 Nov 2016 17:08:33 -0500 Received: from mail-yw0-f181.google.com ([209.85.161.181]:34430 "EHLO mail-yw0-f181.google.com" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S934152AbcKWWIS (ORCPT ); Wed, 23 Nov 2016 17:08:18 -0500 Received: by mail-yw0-f181.google.com with SMTP id t125so23659026ywc.1 for ; Wed, 23 Nov 2016 14:08:18 -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=FoUE5F12WOQJ10knK72wRIwj+7zD0rmdRq8a+5OFM+w=; b=mFL9TJxHeWfNbmzgxS2QXW5ZOm9gcq0LRCg6GcXKm4CR+9TUGBTrPoIPymTspbjQK8 xRnCPgOQuumwNTSmM0skwtN6dJf15L6Q3hEGUt5MDbBmOH378s8M1FqwXmX6dyALir7i elUAnWwGqDEAn654ffTyWS4iF8t8bkUtg85MYdXimwCYsMgWwTGcLrYC6Jrns33RNx4V EWeC/ofE7rFgQa869pzxW/5nmfeHYQPDUWPcPy9wPbnx88qAjTl/82KW0v8kBoWGhs5v aU3BX1Khd2932g3REzizNLIQFICs5HYrXAm0AKYVSmBK4+9e99KqArZv9qX3jYO7G/81 Y4JA== X-Google-DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=1e100.net; s=20130820; h=x-gm-message-state:from:to:cc:subject:date:message-id:in-reply-to :references; bh=FoUE5F12WOQJ10knK72wRIwj+7zD0rmdRq8a+5OFM+w=; b=UDjxkc/kiJ9LbkK4VdhLDPtrl0thuPsO73aSCbq5jhVSsfWSY4gUnFGUg/aquJcEJu 22UL/CADJda5Jtf4IMwyp+o9+rlnvzulFJypQPjUi1m2MBXgfS8/XsV3JqzFMZaTKrYd KYm2bE9OiclQ7EwD8F5xVKtP4Nhv5i/fsnVZO1WvZozEJ8IBTSGXmZrzhWGcEJdO4RXs 1AsQWtGBuErYr0zlYiAwVjkwBci3f5G77RpeoiGgxUA3X1FGodiYJMqgBNXtkDfn8VYM jxWQFuaPpPRuNJdN5pg6lxoz/hv9vHjwh6WHuJFaLAjl5B9z0f9KJkWOk6fZHuJO/rI+ 6ceg== X-Gm-Message-State: AKaTC032S61NSTSgXWYysWzIFWoHW1JclM0upEwFmsUUiYqINUeECQyHeuLsElmwMzB+m/nO X-Received: by 10.129.114.130 with SMTP id n124mr6751523ywc.142.1479938897409; Wed, 23 Nov 2016 14:08:17 -0800 (PST) Received: from konan1.dev.biz ([100.42.98.197]) by smtp.gmail.com with ESMTPSA id u134sm12154004ywg.31.2016.11.23.14.08.16 (version=TLS1_2 cipher=ECDHE-RSA-AES128-GCM-SHA256 bits=128/128); Wed, 23 Nov 2016 14:08:17 -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 7/8] HID: sony: Calibrate DS4 motion sensors Date: Wed, 23 Nov 2016 14:07:12 -0800 Message-Id: <1479938833-26424-8-git-send-email-roderick@gaikai.com> X-Mailer: git-send-email 2.7.4 In-Reply-To: <1479938833-26424-1-git-send-email-roderick@gaikai.com> References: <1479938833-26424-1-git-send-email-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 | 213 ++++++++++++++++++++++++++++++++++++++++++------- 1 file changed, 185 insertions(+), 28 deletions(-) diff --git a/drivers/hid/hid-sony.c b/drivers/hid/hid-sony.c index f27c9b8..d9e5254 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; @@ -822,6 +838,9 @@ struct sony_sc { u8 led_delay_on[MAX_LEDS]; u8 led_delay_off[MAX_LEDS]; u8 led_count; + + /* DS4 calibration data */ + struct ds4_calibration_data ds4_calib_data[6]; }; static inline void sony_schedule_work(struct sony_sc *sc) @@ -1012,9 +1031,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; @@ -1023,10 +1039,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); @@ -1288,6 +1316,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) @@ -1314,13 +1343,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); @@ -1418,23 +1455,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; } @@ -2337,12 +2496,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; } /*