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:
- <https://gitlab.freedesktop.org/hadess/iio-sensor-proxy/-/merge_requests/316>

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"

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;
 

src/drv-iio-poll-compass-uncalibrated.c

@@ -0,0 +1,189 @@
+/*
+ * Copyright (c) 2020 Evangelos Ribeiro Tzaras <devrtz@fortysixandtwo.eu>
+ *
+ * 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 <string.h>
+#include <errno.h>
+#include <math.h>
+
+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 / G_PI;
+	} else
+		readings.heading = atan2 (magn_x, magn_y) * 180 / 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,
+};

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,
 };

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',