colin/iio-sensor-proxy
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

compass: Add support for polling uncalibrated devices

Browse Source 
* 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>
This commit is contained in:
Evangelos Ribeiro Tzaras
2020-09-27 22:28:57 +02:00
committed by Colin
parent fe56bdba12
commit fd21f1f4bf
5 changed files with 193 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
data/80-iio-sensor-proxy.rules
View File

@@ -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"

1
src/drivers.h
View File

@@ -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;

189
src/drv-iio-poll-compass-uncalibrated.c Normal file
View File

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

1
src/iio-sensor-proxy.c
View File

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

1
src/meson.build
View File

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