From patchwork Wed Mar 28 00:27:11 2018 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Martin Kelly X-Patchwork-Id: 10311937 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 8808260212 for ; Wed, 28 Mar 2018 00:27:35 +0000 (UTC) Received: from mail.wl.linuxfoundation.org (localhost [127.0.0.1]) by mail.wl.linuxfoundation.org (Postfix) with ESMTP id 70D1F28F09 for ; Wed, 28 Mar 2018 00:27:35 +0000 (UTC) Received: by mail.wl.linuxfoundation.org (Postfix, from userid 486) id 64651291CB; Wed, 28 Mar 2018 00:27: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.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 87C2C28F09 for ; Wed, 28 Mar 2018 00:27:34 +0000 (UTC) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S1752279AbeC1A1e (ORCPT ); Tue, 27 Mar 2018 20:27:34 -0400 Received: from mail-bn3nam01on0084.outbound.protection.outlook.com ([104.47.33.84]:10537 "EHLO NAM01-BN3-obe.outbound.protection.outlook.com" rhost-flags-OK-OK-OK-FAIL) by vger.kernel.org with ESMTP id S1752176AbeC1A1c (ORCPT ); Tue, 27 Mar 2018 20:27:32 -0400 DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=uievolution.onmicrosoft.com; s=selector1-xevo-com; h=From:Date:Subject:Message-ID:Content-Type:MIME-Version; bh=dK4gQyrPH3e55yYwMlhAPk3At7DfvlNj4bSy7w53OOY=; b=xTWsAdCN45Tviy5OnI4q6icM9m84hYuGnQ9reexr/3m2o8FwhkFRi4GOeiU8BEa9bRC2PGtrCBzzoy0VV/KOmbVU6gIoFSEQscj8f4CjzcLTYjeHGewGQXuVqAm++cFWT3mpZp2QDSuY6qTo+DDwfhecdSoqnbWf+IdHZe6Ff98= Authentication-Results: spf=none (sender IP is ) smtp.mailfrom=mkelly@xevo.com; Received: from columbia.corp.xevo.com (65.122.179.226) by SN6PR0102MB3422.prod.exchangelabs.com (2603:10b6:805:3::11) with Microsoft SMTP Server (version=TLS1_2, cipher=TLS_ECDHE_RSA_WITH_AES_256_CBC_SHA384_P256) id 15.20.609.10; Wed, 28 Mar 2018 00:27:30 +0000 From: Martin Kelly To: linux-iio@vger.kernel.org Cc: Jonathan Cameron , Jean-Baptiste Maneyrol , Martin Kelly Subject: [PATCH v2] imu: inv_mpu6050: interpolate missing timestamps Date: Tue, 27 Mar 2018 17:27:11 -0700 Message-Id: <20180328002711.26896-1-mkelly@xevo.com> X-Mailer: git-send-email 2.11.0 MIME-Version: 1.0 X-Originating-IP: [65.122.179.226] X-ClientProxiedBy: DM5PR21CA0039.namprd21.prod.outlook.com (2603:10b6:3:ed::25) To SN6PR0102MB3422.prod.exchangelabs.com (2603:10b6:805:3::11) X-MS-PublicTrafficType: Email X-MS-Office365-Filtering-Correlation-Id: 75f53e90-72fe-4e57-232e-08d59442b142 X-Microsoft-Antispam: UriScan:; BCL:0; PCL:0; RULEID:(7020095)(4652020)(5600026)(4604075)(4534165)(4627221)(201703031133081)(201702281549075)(2017052603328)(7153060)(7193020); SRVR:SN6PR0102MB3422; X-Microsoft-Exchange-Diagnostics: 1; SN6PR0102MB3422; 3:GpJZSbRlNr4E6x394/4JzhOrDxI73R2qjs3tDuBtgAcaib/2SbTlL5UnX2S3mLfRVsVpbm3yfWu4HsFc5T8tyvj3UKbRT2XKStu+cUapKC+Zep3214+Z0/8mkqOC0vwRgCOLkmirOEt4AyFbMud7D9DfggeCszGPCZxIgZt0BojxQVgDoYaZwR6dEUVgbm4JcWE4m+9fSA0piWeE5YULYjViADliM79hsITre40QxVEWca5wEVHckJGzM+wkYJVp; 25:XF6xtafvGtuuqN5Nq++tLYHkcsvTK+LjU/tNMNcAJ5wvfTKZdpvBYsvmYfEgZWFe5P2WFD4cMP/0xDFidtfTtEdJcDm9crcYFSLSI2N5J8lc2/RG7cvHh8THr7MlMKbvzDNoPqurGH/vvNakPPfmacR98hHUdn9o76OaAWO6pxlYn+6ghgLEx3x0s771UqHc5ZvMqDG6r35+Tn3tYxRatgvW3iB3hP0u3Em+NrmxGDhsnhjvU6oa8JWm1L8hc1PcjgadhQ2kQWTPmfHd+TuT4/NXEEmQZbUCKXZMm8ztYyy+CZKlqr4FbltnrU0FhoDGsFAXFkct/lXSxJsOVIcspQ==; 31:e1Z2zT56vslJS7TUYvzO6Xxb9jxCZ2wUai3+Lo5fvzXWJ0SDpTExFbZtSzqd6GpKXV4phJZrSd2enPeZu0SzrRiYLO/BdPMWZS3zxg7k+3WHHx4MrWogVeT5T8W+x+6GmwUx+lPS3NQMH0U1sSTxTVXwpL7+FsiZV1dzcxzrSR+yIEBVTLKJ+KyaI8w+wo9XjI/CvxkrZ58+qUsICamePTam1n61jQS2p5LgkGl9DJQ= X-MS-TrafficTypeDiagnostic: SN6PR0102MB3422: X-Microsoft-Exchange-Diagnostics: 1; SN6PR0102MB3422; 20:8/P6WQ5OkhWiyfU6eDw1jPJmsePE50v5EI8plhSlWIhKmtM6pNLTlZlJN874Tpth8EM/mggWn8KBhD4osxFvhOn5dyUWH6IeWk1KnPUkOhEgPEqjlICvS5XmnJPpK5w7NgrQS0ZbLgPsVWRepaT64c6LtTAyg/pb/hN2X1/Fi852+Ru7ljyB1qttXs8PzSKTpWbyl31c78GQAd6CnCHWTtk8z51Ui4Twss77VQi1MJE/0OIin/jWoT93ox2zWtgDtXGWloLWT4QcB0HdaA97EHHJ7uNO6TMj6e4yCFbTHQA2Dz+oCz8aH2o2UY/TeCj3A+9jH3xKMFAmRw2iAhFnNYdeF+Gq3+KV1xj1jPAvR/WDM2qur9tN6swPOPiI398r6/PgITJ8fxGWDfmTZHXgnrW6CsuGJ3EyLBSYgn6OOY/oV64UmxZsBmrY3/endnh/CczTzAh+v+ZVHcgih6nt9xSEd9reInZ2v8jFYKQ9hjdTEJVhgYAXX+5TKebXc8U5; 4:yiomoZmE18gyvwXagDclG/xT1Kbl4zoFl67WC1Eb8q+KJPSwwpUgBAoOxohtLZXUk0qIsNmkoZGll0y4+3Bd0bdNHatU9kmUybfiYIHxSp33g/ZuhA+D1OMudKlt7zXdy+mB6JPTSwJWNCF2U7Ba8HYcGdao9wMKWKv+cQbAzJ8qhoCGb/K6cS2Okg3IFwbYMDYI8yQYHS5nIfT6OqISjtxNPcupV5RsBEDOp4xZgN34zbePmoiwOjQJoUK8JLsWcGn/+pyencChXTXA/KkETQ== X-Microsoft-Antispam-PRVS: X-Exchange-Antispam-Report-Test: UriScan:; X-Exchange-Antispam-Report-CFA-Test: BCL:0; PCL:0; RULEID:(6040522)(2401047)(8121501046)(5005006)(3231221)(944501327)(52105095)(10201501046)(3002001)(93006095)(93001095)(6041310)(20161123562045)(201703131423095)(201702281528075)(20161123555045)(201703061421075)(201703061406153)(20161123564045)(20161123558120)(20161123560045)(6072148)(201708071742011); SRVR:SN6PR0102MB3422; BCL:0; PCL:0; RULEID:; SRVR:SN6PR0102MB3422; X-Forefront-PRVS: 06259BA5A2 X-Forefront-Antispam-Report: SFV:NSPM; SFS:(10009020)(346002)(376002)(39840400004)(39380400002)(366004)(396003)(189003)(199004)(86362001)(3846002)(478600001)(6666003)(2351001)(186003)(6116002)(386003)(36756003)(53936002)(16526019)(2361001)(2616005)(956004)(50226002)(2906002)(51416003)(68736007)(52116002)(59450400001)(7696005)(26005)(476003)(1076002)(6916009)(105586002)(8936002)(66066001)(16586007)(50466002)(316002)(305945005)(81156014)(81166006)(48376002)(6486002)(97736004)(486005)(106356001)(486005)(107886003)(25786009)(8676002)(7736002)(54906003)(4326008)(5660300001)(47776003); DIR:OUT; SFP:1101; SCL:1; SRVR:SN6PR0102MB3422; H:columbia.corp.xevo.com; FPR:; SPF:None; PTR:InfoNoRecords; MX:1; A:1; LANG:en; Received-SPF: None (protection.outlook.com: xevo.com does not designate permitted sender hosts) X-Microsoft-Exchange-Diagnostics: =?us-ascii?Q?1; SN6PR0102MB3422; 23:H54xb6h+IYzzxuBU6kCTNCg+/osHQUWHBAcEHTo?= =?us-ascii?Q?UPgzsqSlcHCWUP7rKoPdv8O+PXM2IHhhrKJAOB6/wEF1udNptP1VlfupyItt?= =?us-ascii?Q?ktcIwbI2O+F8IuHIHL59OKCmbAr23i8eZpJCToRUuTJlMcdSiJg9euBfXsdT?= =?us-ascii?Q?66FvU29oSE7+DlUoac4+g39QMpkc+1X5IGkZbKqknPdp3r6e/6J00WmiM4Xw?= =?us-ascii?Q?lPZvKo+uHcM4977sUP7iLt2zZan/d4AWf3LUe7s7DzyUdmTyfw0dDXsw44rg?= =?us-ascii?Q?g+s16RV2hzqiMEuOhSUp/DHIUWCuDothPbf8u3mtYdap+Ktnm2dtCISQ61sr?= =?us-ascii?Q?kEXNkpRzbDempOtC72c7blzsS9m790W5XLB+d2kScvSSvHjTpCoDsEyc+tQz?= =?us-ascii?Q?d+VK5esjLNCSGSZExnmsrCiW2JrJDCLJ6rm8QQJ2pf4/yDwIXzaMV4cXL41g?= =?us-ascii?Q?/FlcHrw8YwYhLeScMeoEnWu/8ecqQSvNG5knC+rhci7U+IfBhBNiSReT6xg1?= =?us-ascii?Q?UgFzgMfHY1o2zRyp6KhfuQHfMtGYHMVtoQTIBToyetp6q+RmX2y5xdA4pfYS?= =?us-ascii?Q?JZ00IOh8sfHQyfFm836A19wIXYoEMX2Awn3C6x6rs/em0VImKQvKBdwuHhxY?= =?us-ascii?Q?QyuVqThtM+iW9Uq8WIHxOVFWZ5Or+ft8/FzYBk9quPVKwZh7Tt6buq2T1IPk?= =?us-ascii?Q?/rKyiT5MF5A7AY16PO/QS+vAlTsniaZtMFM+6s0AUhYBkA4v2/gklpCzKJ8c?= =?us-ascii?Q?8DtKz3vIdbWRxGj0jy9Tw8e29KCwZvYCzQEqU/5+lfXE0I3cnp1f4HMuusfS?= =?us-ascii?Q?0p32uBPRcO3lw4ZxXs3mSSe4oNQ8omCwnGl7zrlj9rwzUqW2nWqs3fLZeymo?= =?us-ascii?Q?g0ny8k68D3TfIrm0G5dqPDo2ezgOtBzcKq7r2IjPT/cVv6MUhPNTGoUZw7KA?= =?us-ascii?Q?eWDRXQ+Uzkvx+rpUwkJ82FujAwRjMG4n/3/aoH9Djnn+Wo8Xuor47WArECB/?= =?us-ascii?Q?w2P4bVERv5n8tCj2KyjLrymNcgGa3azIzzmvLl8KU5MrlGGNUmQYZ/VpYq3d?= =?us-ascii?Q?MUCjaVR02WQUe87l+Iz/KKiMYNMEMKFUFVj+ih/cM9NaMrqTJBp/kaAS1Tjm?= =?us-ascii?Q?Q0IXe/G64M+YfBxPzsdDlX7ZxE/fk0nifiXG/p92u5MHQRWUMERTfvM/HDH6?= =?us-ascii?Q?Ih5UWY6ra6Eo1G5NcOguqZ8lFVBn19ekK7CK6bkso45wYbs6dyu0SNNcWwhg?= =?us-ascii?Q?yjtG14V/oPeMHZ0DW0oo=3D?= X-Microsoft-Antispam-Message-Info: ROm4EDDD37zlnr7FkdO7uGkODUUTueGpgyTng9j/jJmQEJtdPtFanHYiZWXpV3wkVeS4UwNvLq2YoIY2HxdADhYarijT/6qXY8vUDh5bBul32cpA15JxEa7tLiMqFSKYGTX07Y2EDcGuQbWog6p1Osvm/QdSrxXbU9/osN4jE5b9O2/uZfZs2QxixSjauJPq X-Microsoft-Exchange-Diagnostics: 1; SN6PR0102MB3422; 6:+ogyFXSN354drlcyHCQbKWdNg13gL+7C73doEc6jv5R4TSa9+T9A2/eAr+dBeQEfq5aGaAAFsuP77yGnjWlvYRzfK5PTJAzrj3QVrxWhc6qKaLzZBTgA+rTJD7cDogyTN83LyZRYt/M5G3GCOAMxHPt18LD1cDr2izBfKWfCyUpDCxSjcdnwQIPm8Oq0W00GhFMq8PAVgtkMlPFruZSVL8j2twyJEaPbJquDPo8eJr2M8M4ItD0LutyqfQSzfwGrbZblHVz5DSt3UeGYYu80b3+1duLkDYqW1k68QHGETk9YtNBDTC7kPpBOi9g7ioznD0h87ZU83bWTyPXduFVNQflwpMCidE6P/hsp3CebUGApCkMhRzKo+q/Q+qoy+y8PJ4Nuaz1pmzOqo62CD1fQBpJ3nT9RHW8vrLPAvSmBA/9Bz4DaTKQphZUsj3ikDLpwgUhNmj0uQ0ce4G2eeM/Bow==; 5:vbZz+mkSRRtVuVwj+gIt9+bbnWLL08Qj+twDG73voIE49m/aiqoNwqIJXY0chcAKcLwnpFaVXjqHYNLS5mQ9QTJ8rjIeA+zuZ+hFutrBh1ahWSAcuQGCmPDfdqGeY8l0Xl6ZmpBqaZQGMDPXxcTHCz/vEtyz9TrQ7+XW0KPu8U0=; 24:0Bucurlwg3IC7WY+a6T4mj0eD0e9F6GqGSnrftDfIVWNHg7bCfArXH8V5gLukEmjLcCvYRZzavz0KZy/luQ6YhleEFmUw1Njyc+E3Bd6qBs= SpamDiagnosticOutput: 1:99 SpamDiagnosticMetadata: NSPM X-Microsoft-Exchange-Diagnostics: 1; SN6PR0102MB3422; 7:mEmZnrMzViezXebVW1aCEao/qp1NmDKOwpSTqugwcQKui44n+OcRMWZBr0J5EsSBENpA9sWWngvxYmnn4aBd1ya6aWP4ebJRDbSZDav0v72QM8lJGKdJVCs24YWZS7hWLbQSaKxy+hvRYY3SLser6DxkeIhQyiZxEeIYsxOrBmbMZ377T1gM/BxBiY1fuHWrKtusinBFfNZne4RH8vg4HdUsSG7DyYbTd/uU6E+s8vgGtzIQNs+BCElCJbKo9SIZ X-OriginatorOrg: xevo.com X-MS-Exchange-CrossTenant-OriginalArrivalTime: 28 Mar 2018 00:27:30.0759 (UTC) X-MS-Exchange-CrossTenant-Network-Message-Id: 75f53e90-72fe-4e57-232e-08d59442b142 X-MS-Exchange-CrossTenant-FromEntityHeader: Hosted X-MS-Exchange-CrossTenant-Id: e0a7ca1f-2458-4cd6-a7c7-d733c07495ab X-MS-Exchange-Transport-CrossTenantHeadersStamped: SN6PR0102MB3422 Sender: linux-iio-owner@vger.kernel.org Precedence: bulk List-ID: X-Mailing-List: linux-iio@vger.kernel.org X-Virus-Scanned: ClamAV using ClamSMTP When interrupts are generated at a slower rate than the FIFO fills up, we will have fewer timestamps than samples. Currently, we fill in 0 for any unmatched timestamps. However, this is very confusing for userspace, which does not expect discontinuities in timestamps and must somehow work around the issue. Improve the situation by interpolating timestamps when we can't map them 1:1 to data. We do this by assuming the timestamps we have are the most recent and interpolating backwards to fill the older data. This makes sense because inv_mpu6050_read_fifo gets called right after an interrupt is generated, presumably when a datum was generated, so the most recent timestamp matches up with the most recent datum. In addition, this assumption is borne out by observation, which shows monotonically increasing timestamps when interpolating this way but discontinuities when interpolating in the other direction. Although this method is not perfectly accurate, it is probably the best we can do if we don't get one interrupt per datum. Signed-off-by: Martin Kelly --- drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 2 ++ drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h | 2 ++ drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 40 +++++++++++++++++++++++++++--- 3 files changed, 41 insertions(+), 3 deletions(-) diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c index 7d64be353403..4a95ff8df3b9 100644 --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c @@ -83,6 +83,7 @@ static const struct inv_mpu6050_chip_config chip_config_6050 = { .fsr = INV_MPU6050_FSR_2000DPS, .lpf = INV_MPU6050_FILTER_20HZ, .fifo_rate = INV_MPU6050_INIT_FIFO_RATE, + .fifo_period = NSEC_PER_SEC / INV_MPU6050_INIT_FIFO_RATE, .gyro_fifo_enable = false, .accl_fifo_enable = false, .accl_fs = INV_MPU6050_FS_02G, @@ -630,6 +631,7 @@ inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr, if (result) goto fifo_rate_fail_power_off; st->chip_config.fifo_rate = fifo_rate; + st->chip_config.fifo_period = NSEC_PER_SEC / fifo_rate; result = inv_mpu6050_set_lpf(st, fifo_rate); if (result) diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h index 065794162d65..3bc7d62822ca 100644 --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h @@ -86,6 +86,7 @@ enum inv_devices { * @accl_fifo_enable: enable accel data output * @gyro_fifo_enable: enable gyro data output * @fifo_rate: FIFO update rate. + * @fifo_period: FIFO update period, in nanoseconds. */ struct inv_mpu6050_chip_config { unsigned int fsr:2; @@ -94,6 +95,7 @@ struct inv_mpu6050_chip_config { unsigned int accl_fifo_enable:1; unsigned int gyro_fifo_enable:1; u16 fifo_rate; + u32 fifo_period; }; /** diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c index ff81c6aa009d..759844f9d8e6 100644 --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c @@ -126,7 +126,11 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p) int result; u8 data[INV_MPU6050_OUTPUT_DATA_SIZE]; u16 fifo_count; + u16 fifo_items; + s32 fifo_diff; s64 timestamp; + s64 timestamp_interp; + s64 offset; mutex_lock(&st->lock); if (!(st->chip_config.accl_fifo_enable | @@ -156,9 +160,25 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p) if (fifo_count > INV_MPU6050_FIFO_THRESHOLD) goto flush_fifo; /* Timestamp mismatch. */ + fifo_items = fifo_count / bytes_per_datum; if (kfifo_len(&st->timestamps) > - fifo_count / bytes_per_datum + INV_MPU6050_TIME_STAMP_TOR) + fifo_items + INV_MPU6050_TIME_STAMP_TOR) goto flush_fifo; + + /* + * If we have more data than timestamps, the timestamps we have + * correspond to the newest items in the FIFO, since some data was + * generated without a corresponding interrupt and thus timestamp. Since + * we remove FIFO items oldest to newest, we need to interpolate the + * older timestamps based on the number of missing timestamps. + */ + fifo_diff = fifo_items - kfifo_len(&st->timestamps); + if (fifo_diff > 0) + offset = st->chip_config.fifo_period * fifo_diff; + else + offset = 0; + + timestamp_interp = 0; while (fifo_count >= bytes_per_datum) { result = regmap_bulk_read(st->map, st->reg->fifo_r_w, data, bytes_per_datum); @@ -166,9 +186,23 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p) goto flush_fifo; result = kfifo_out(&st->timestamps, ×tamp, 1); - /* when there is no timestamp, put timestamp as 0 */ + /* When there is no timestamp, interpolate based on period */ if (result == 0) - timestamp = 0; + /* + * We have no more timestamps left, so interpolate the + * rest of them. + */ + timestamp = timestamp_interp; + else { + /* + * If we are interpolating, this is an older datum with + * a newer timestamp, so offset it backwards to account + * for the time gap. Otherwise, offset will be 0. + */ + timestamp -= offset; + timestamp_interp = timestamp; + } + timestamp_interp += st->chip_config.fifo_period; result = iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp);