From 227f3f3646df6089d3a81d380f3e30e80cb789a5 Mon Sep 17 00:00:00 2001 From: Evangelos Ribeiro Tzaras Date: Sun, 27 Sep 2020 22:28:57 +0200 Subject: [PATCH] compass: Add support for polling uncalibrated devices * data/80-iio-sensor-proxy.rules: Add rule matching pollable compasses * src/drivers.h: Add iio_poll_compass_uncalibrated * src/drv-iio-poll-compass-uncalibrated.c: Add driver able to handle calibration data * src/meson.build: Build new driver TODO: - Gather calibration samples - Start/Stop Calibration (DBus) - Load Calibration Data from disk (currently hardcoded for my device) - Not sure if I'm doing the heading correctly See: - --- data/80-iio-sensor-proxy.rules | 1 + src/drivers.h | 1 + src/drv-iio-poll-compass-uncalibrated.c | 167 ++++++++++++++++++++++++ src/iio-sensor-proxy.c | 1 + src/meson.build | 1 + 5 files changed, 171 insertions(+) create mode 100644 src/drv-iio-poll-compass-uncalibrated.c diff --git a/data/80-iio-sensor-proxy.rules b/data/80-iio-sensor-proxy.rules index 3b8a149..5d7fed4 100644 --- a/data/80-iio-sensor-proxy.rules +++ b/data/80-iio-sensor-proxy.rules @@ -8,6 +8,7 @@ SUBSYSTEM=="hwmon", TEST=="light", ENV{IIO_SENSOR_PROXY_TYPE}+="hwmon-als" SUBSYSTEM=="iio", TEST=="in_accel_x_raw", TEST=="in_accel_y_raw", TEST=="in_accel_z_raw", ENV{IIO_SENSOR_PROXY_TYPE}+="iio-poll-accel" SUBSYSTEM=="iio", TEST=="scan_elements/in_accel_x_en", TEST=="scan_elements/in_accel_y_en", TEST=="scan_elements/in_accel_z_en", ENV{IIO_SENSOR_PROXY_TYPE}+="iio-buffer-accel" SUBSYSTEM=="iio", TEST=="scan_elements/in_rot_from_north_magnetic_tilt_comp_en", ENV{IIO_SENSOR_PROXY_TYPE}+="iio-buffer-compass" +SUBSYSTEM=="iio", TEST=="in_magn_x_raw", TEST=="in_magn_y_raw", TEST=="in_magn_z_raw", ENV{IIO_SENSOR_PROXY_TYPE}+="iio-poll-compass-uncalibrated" SUBSYSTEM=="iio", TEST=="in_illuminance_input", ENV{IIO_SENSOR_PROXY_TYPE}+="iio-poll-als" SUBSYSTEM=="iio", TEST=="in_illuminance0_input", ENV{IIO_SENSOR_PROXY_TYPE}+="iio-poll-als" SUBSYSTEM=="iio", TEST=="in_illuminance_clear_raw", ENV{IIO_SENSOR_PROXY_TYPE}+="iio-poll-als" diff --git a/src/drivers.h b/src/drivers.h index 33ad667..89d41a3 100644 --- a/src/drivers.h +++ b/src/drivers.h @@ -158,6 +158,7 @@ extern SensorDriver iio_poll_light; extern SensorDriver hwmon_light; extern SensorDriver iio_buffer_light; extern SensorDriver iio_buffer_compass; +extern SensorDriver iio_poll_compass_uncalibrated; extern SensorDriver iio_poll_proximity; extern SensorDriver input_proximity; diff --git a/src/drv-iio-poll-compass-uncalibrated.c b/src/drv-iio-poll-compass-uncalibrated.c new file mode 100644 index 0000000..4209163 --- /dev/null +++ b/src/drv-iio-poll-compass-uncalibrated.c @@ -0,0 +1,167 @@ +/* + * Copyright (c) 2020 Evangelos Ribeiro Tzaras + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 3 as published by + * the Free Software Foundation. + */ + +#include "drivers.h" +#include "iio-buffer-utils.h" + +#include +#include +#include + +typedef struct { + gint x_max; + gint x_min; + gint y_max; + gint y_min; + gint z_max; + gint z_min; + gboolean is_calibrated; +} CalibrationData; + +typedef struct { + guint timeout_id; + ReadingsUpdateFunc callback_func; + gpointer user_data; + + GUdevDevice *dev; + const char *dev_path; + const char *name; + + CalibrationData *calibration_data; +} DrvData; + +static DrvData *drv_data = NULL; + +static int +sysfs_get_int (GUdevDevice *dev, + const char *attribute) +{ + int result; + char *contents; + char *filename; + + result = 0; + filename = g_build_filename (g_udev_device_get_sysfs_path (dev), attribute, NULL); + if (g_file_get_contents (filename, &contents, NULL, NULL)) { + result = atoi (contents); + g_free (contents); + } + g_free (filename); + + return result; +} + +static gboolean +poll_heading (gpointer user_data) +{ + DrvData *data = user_data; + int magn_x, magn_y, magn_z; + CompassReadings readings; + double avg_delta_x, avg_delta_y, avg_delta_z, avg_delta; + double offset_x, offset_y, offset_z; + double scale_x, scale_y, scale_z, corrected_x, corrected_y, corrected_z; + + magn_x = sysfs_get_int (data->dev, "in_magn_x_raw"); + magn_y = sysfs_get_int (data->dev, "in_magn_y_raw"); + magn_z = sysfs_get_int (data->dev, "in_magn_z_raw"); + + if (data->calibration_data->is_calibrated) { + offset_x = (data->calibration_data->x_min + data->calibration_data->x_max) / 2; + offset_y = (data->calibration_data->y_min + data->calibration_data->y_max) / 2; + offset_z = (data->calibration_data->z_min + data->calibration_data->z_max) / 2; + + avg_delta_x = (data->calibration_data->x_max - data->calibration_data->x_min) / 2; + avg_delta_y = (data->calibration_data->y_max - data->calibration_data->y_min) / 2; + avg_delta_z = (data->calibration_data->z_max - data->calibration_data->z_min) / 2; + + avg_delta = (avg_delta_x + avg_delta_y + avg_delta_z) / 3; + + scale_x = avg_delta / avg_delta_x; + scale_y = avg_delta / avg_delta_y; + scale_z = avg_delta / avg_delta_z; + + corrected_x = (magn_x - offset_x) * scale_x; + corrected_y = (magn_y - offset_y) * scale_y; + //corrected_z = (magn_z - offset_z) * scale_z; + + readings.heading = atan2 (corrected_x, corrected_y) * 180 / G_PI; + } else + readings.heading = atan2 (magn_x, magn_y) * 180 / PI; + + // Mount matrix? + + drv_data->callback_func (&iio_poll_compass_uncalibrated, (gpointer) &readings, drv_data->user_data); + + return G_SOURCE_CONTINUE; +} + +gboolean +iio_compass_discover (GUdevDevice *device) +{ + return drv_check_udev_sensor_type (device, "iio-poll-compass-uncalibrated", "IIO poll compass uncalibrated"); +} + +gboolean iio_compass_open (GUdevDevice *device, + ReadingsUpdateFunc callback_func, + gpointer user_data) +{ + io_fixup_sampling_frequency (device); + drv_data = g_new0 (DrvData, 1); + drv_data->calibration_data = g_new0 (CalibrationData, 1); + drv_data->calibration_data->x_min = -7912; + drv_data->calibration_data->x_max = -1648; + drv_data->calibration_data->y_min = -6554; + drv_data->calibration_data->y_max = -528; + drv_data->calibration_data->z_min = -3074; + drv_data->calibration_data->z_max = 2720; + drv_data->calibration_data->is_calibrated = TRUE; + + drv_data->dev = g_object_ref (device); + drv_data->name = g_udev_device_get_sysfs_attr (device, "name"); + + drv_data->callback_func = callback_func; + drv_data->user_data = user_data; + + return TRUE; +} + +void iio_compass_set_polling (gboolean state) +{ + if (drv_data->timeout_id > 0 && state) + return; + if (drv_data->timeout_id == 0 && !state) + return; + + if (drv_data->timeout_id) { + g_source_remove (drv_data->timeout_id); + drv_data->timeout_id = 0; + } + + if (state) { + drv_data->timeout_id = g_timeout_add (700, poll_heading, drv_data); + g_source_set_name_by_id (drv_data->timeout_id, "[iio_compass_set_polling] poll_heading"); + } +} + +void iio_compass_close (void) +{ + iio_compass_set_polling (FALSE); + g_clear_object (&drv_data->dev); + g_free (&drv_data->calibration_data); + g_clear_pointer (&drv_data, g_free); +} + +SensorDriver iio_poll_compass_uncalibrated = { + .name = "IIO Poll Uncalibrated Compass", + .type = DRIVER_TYPE_COMPASS, + + .discover = iio_compass_discover, + .open = iio_compass_open, + .set_polling = iio_compass_set_polling, + .close = iio_compass_close, +}; diff --git a/src/iio-sensor-proxy.c b/src/iio-sensor-proxy.c index 1884839..50a4d3c 100644 --- a/src/iio-sensor-proxy.c +++ b/src/iio-sensor-proxy.c @@ -73,6 +73,7 @@ static const SensorDriver * const drivers[] = { &fake_compass, &fake_light, &iio_buffer_compass, + &iio_poll_compass_uncalibrated, &iio_poll_proximity, &input_proximity, }; diff --git a/src/meson.build b/src/meson.build index 4b822fc..7f1e7e4 100644 --- a/src/meson.build +++ b/src/meson.build @@ -27,6 +27,7 @@ sources = [ 'drv-hwmon-light.c', 'drv-iio-buffer-light.c', 'drv-iio-buffer-compass.c', + 'drv-iio-poll-compass-uncalibrated.c', 'drv-iio-poll-proximity.c', 'drv-input-proximity.c', 'iio-buffer-utils.c',