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..9af4e98 --- /dev/null +++ b/src/drv-iio-poll-compass-uncalibrated.c @@ -0,0 +1,189 @@ +/* + * 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 CalibrationData { + gint x_max; + gint x_min; + gint y_max; + gint y_min; + gint z_max; + gint z_min; + gboolean is_calibrated; +} CalibrationData; + +typedef struct DrvData { + guint timeout_id; + GUdevDevice *dev; + CalibrationData *calibration_data; +} DrvData; + +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) +{ + SensorDevice *sensor_device = user_data; + DrvData *drv_data = (DrvData *) sensor_device->priv; + 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 (drv_data->dev, "in_magn_x_raw"); + magn_y = sysfs_get_int (drv_data->dev, "in_magn_y_raw"); + magn_z = sysfs_get_int (drv_data->dev, "in_magn_z_raw"); + + if (drv_data->calibration_data->is_calibrated) { + offset_x = (drv_data->calibration_data->x_min + drv_data->calibration_data->x_max) / 2; + offset_y = (drv_data->calibration_data->y_min + drv_data->calibration_data->y_max) / 2; + offset_z = (drv_data->calibration_data->z_min + drv_data->calibration_data->z_max) / 2; + + avg_delta_x = (drv_data->calibration_data->x_max - drv_data->calibration_data->x_min) / 2; + avg_delta_y = (drv_data->calibration_data->y_max - drv_data->calibration_data->y_min) / 2; + avg_delta_z = (drv_data->calibration_data->z_max - drv_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 / M_PI; + } else + readings.heading = atan2 (magn_x, magn_y) * 180 / M_PI; + + // Mount matrix? + + sensor_device->callback_func (sensor_device, (gpointer) &readings, sensor_device->user_data); + + return G_SOURCE_CONTINUE; +} + +static gboolean +iio_compass_discover (GUdevDevice *device) +{ + return drv_check_udev_sensor_type (device, "iio-poll-compass-uncalibrated", "IIO poll compass uncalibrated"); +} + +static SensorDevice * +iio_compass_open (GUdevDevice *device) +{ + SensorDevice *sensor_device; + DrvData *drv_data; + + iio_fixup_sampling_frequency (device); + + sensor_device = g_new0 (SensorDevice, 1); + sensor_device->name = g_strdup (g_udev_device_get_property (device, "NAME")); + if (!sensor_device->name) + sensor_device->name = g_strdup (g_udev_device_get_name (device)); + sensor_device->priv = g_new0 (DrvData, 1); + drv_data = (DrvData *) sensor_device->priv; + drv_data->dev = g_object_ref (device); + 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; + + + // 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 sensor_device; +} + +static void +iio_compass_set_polling (SensorDevice *sensor_device, + gboolean state) +{ + DrvData *drv_data = (DrvData *) sensor_device->priv; + + 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, sensor_device); + g_source_set_name_by_id (drv_data->timeout_id, "[iio_compass_set_polling] poll_heading"); + } +} + +static void +iio_compass_close (SensorDevice *sensor_device) +{ + DrvData *drv_data = (DrvData *) sensor_device->priv; + + // iio_compass_set_polling (FALSE); + g_clear_object (&drv_data->dev); + g_free (&drv_data->calibration_data); + // g_clear_pointer (&drv_data, g_free); + g_free (sensor_device); +} + +SensorDriver iio_poll_compass_uncalibrated = { + .driver_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',