From 07299fb51b8d80f12d74dc044f189e410078b241 Mon Sep 17 00:00:00 2001 From: Bastien Nocera Date: Wed, 14 Sep 2016 14:12:01 +0200 Subject: [PATCH] accel: Fix accelerometer unit confusion MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The original orientation code was based upon code that expected 1G to roughly correspond to a reading of 256, to work-around the fact that the input layer could not pass fractional values easily. So the orientation_calc() code expected that too. But IIO readings after scaling are properly in m/s², and our readings are integers. We'll pass the scaling around to be applied at the last minute, when doing calculations. Closes: #100 --- src/drivers.h | 1 + src/drv-iio-buffer-accel.c | 11 ++++------- src/drv-iio-poll-accel.c | 13 +++++-------- src/drv-input-accel.c | 3 +++ src/iio-sensor-proxy.c | 5 +++-- src/orientation.c | 13 ++++++++++++- src/orientation.h | 3 ++- 7 files changed, 30 insertions(+), 19 deletions(-) diff --git a/src/drivers.h b/src/drivers.h index 3163458..dc4739b 100644 --- a/src/drivers.h +++ b/src/drivers.h @@ -40,6 +40,7 @@ typedef struct { int accel_x; int accel_y; int accel_z; + gdouble scale; } AccelReadings; typedef struct { diff --git a/src/drv-iio-buffer-accel.c b/src/drv-iio-buffer-accel.c index e546be1..2691204 100644 --- a/src/drv-iio-buffer-accel.c +++ b/src/drv-iio-buffer-accel.c @@ -14,10 +14,6 @@ #include #include -/* 1G (9.81m/s²) corresponds to "256" - * value x scale is in m/s² */ -#define SCALE_TO_FF(scale) (scale * 256 / 9.81) - typedef struct { guint timeout_id; ReadingsUpdateFunc callback_func; @@ -69,9 +65,10 @@ process_scan (IIOSensorData data, DrvData *or_data) accel_y = -accel_y; //FIXME report errors - readings.accel_x = accel_x * scale; - readings.accel_y = accel_y * scale; - readings.accel_z = accel_z * scale; + readings.accel_x = accel_x; + readings.accel_y = accel_y; + readings.accel_z = accel_z; + readings.scale = scale; or_data->callback_func (&iio_buffer_accel, (gpointer) &readings, or_data->user_data); return 1; diff --git a/src/drv-iio-poll-accel.c b/src/drv-iio-poll-accel.c index b5b4f95..8a6cad6 100644 --- a/src/drv-iio-poll-accel.c +++ b/src/drv-iio-poll-accel.c @@ -15,10 +15,6 @@ #include #include -/* 1G (9.81m/s²) corresponds to "256" - * value x scale is in m/s² */ -#define SCALE_TO_FF(scale) (scale * 256 / 9.81) - typedef struct DrvData { guint timeout_id; ReadingsUpdateFunc callback_func; @@ -56,9 +52,10 @@ poll_orientation (gpointer user_data) int accel_x, accel_y, accel_z; AccelReadings readings; - accel_x = sysfs_get_int (data->dev, "in_accel_x_raw") * data->scale; - accel_y = sysfs_get_int (data->dev, "in_accel_y_raw") * data->scale; - accel_z = sysfs_get_int (data->dev, "in_accel_z_raw") * data->scale; + accel_x = sysfs_get_int (data->dev, "in_accel_x_raw"); + accel_y = sysfs_get_int (data->dev, "in_accel_y_raw"); + accel_z = sysfs_get_int (data->dev, "in_accel_z_raw"); + readings.scale = data->scale; //FIXME report errors if (g_strcmp0 ("i2c-SMO8500:00", g_udev_device_get_sysfs_attr (data->dev, "name")) == 0) { @@ -127,7 +124,7 @@ iio_poll_accel_open (GUdevDevice *device, drv_data->callback_func = callback_func; drv_data->user_data = user_data; - drv_data->scale = SCALE_TO_FF(g_udev_device_get_sysfs_attr_as_double (device, "in_accel_scale")); + drv_data->scale = g_udev_device_get_sysfs_attr_as_double (device, "in_accel_scale"); if (drv_data->scale == 0.0) drv_data->scale = 1.0; diff --git a/src/drv-input-accel.c b/src/drv-input-accel.c index 2ceaf9a..db4c0e4 100644 --- a/src/drv-input-accel.c +++ b/src/drv-input-accel.c @@ -76,6 +76,9 @@ accelerometer_changed (void) readings.accel_x = accel_x; readings.accel_y = accel_y; readings.accel_z = accel_z; + /* Scale from 1G ~= 256 to a value in m/s² */ + readings.scale = 1.0 / 256 * 9.81; + drv_data->callback_func (&input_accel, (gpointer) &readings, drv_data->user_data); } diff --git a/src/iio-sensor-proxy.c b/src/iio-sensor-proxy.c index d0121b8..14e9d36 100644 --- a/src/iio-sensor-proxy.c +++ b/src/iio-sensor-proxy.c @@ -589,9 +589,10 @@ accel_changed_func (SensorDriver *driver, OrientationUp orientation = data->previous_orientation; //FIXME handle errors - g_debug ("Accel sent by driver (quirk applied): %d, %d, %d", readings->accel_x, readings->accel_y, readings->accel_z); + g_debug ("Accel sent by driver (quirk applied): %d, %d, %d (scale: %lf)", + readings->accel_x, readings->accel_y, readings->accel_z, readings->scale); - orientation = orientation_calc (data->previous_orientation, readings->accel_x, readings->accel_y, readings->accel_z); + orientation = orientation_calc (data->previous_orientation, readings->accel_x, readings->accel_y, readings->accel_z, readings->scale); data->accel_x = readings->accel_x; data->accel_y = readings->accel_y; diff --git a/src/orientation.c b/src/orientation.c index ef4dbaa..20d3129 100644 --- a/src/orientation.c +++ b/src/orientation.c @@ -56,12 +56,23 @@ string_to_orientation (const char *orientation) #define THRESHOLD_LANDSCAPE 35 #define THRESHOLD_PORTRAIT 35 +/* First apply scale to get m/s², then + * convert to 1G ~= 256 as the code expects */ +#define SCALE(a) ((int) ((gdouble) a * scale * 256.0 / 9.81)) + OrientationUp orientation_calc (OrientationUp prev, - int x, int y, int z) + int in_x, int in_y, int in_z, + gdouble scale) { int rotation; OrientationUp ret = prev; + int x, y, z; + + /* this code expects 1G ~= 256 */ + x = SCALE(in_x); + y = SCALE(in_y); + z = SCALE(in_z); /* Portrait check */ rotation = round(atan((double) x / sqrt(y * y + z * z)) * RADIANS_TO_DEGREES); diff --git a/src/orientation.h b/src/orientation.h index fa7e2c5..1d134f9 100644 --- a/src/orientation.h +++ b/src/orientation.h @@ -30,4 +30,5 @@ OrientationUp string_to_orientation (const char *orientation); OrientationUp orientation_calc (OrientationUp prev, int x, int y, - int z); + int z, + gdouble scale);