From 58d8819cf6005ad493ae9fd1bbe7f77b7d853f17 Mon Sep 17 00:00:00 2001 From: Bastien Nocera Date: Mon, 21 Nov 2016 19:14:53 +0100 Subject: [PATCH] main: Add helper functions for mount matrices One function to parse the mount matrix string in the format exported by the iio core in Linux to a 3x3 matrix, and another to apply the mount matrix to an existing 3-dimensional vector. --- src/Makefile.am | 2 ++ src/accel-mount-matrix.c | 72 ++++++++++++++++++++++++++++++++++++++++ src/accel-mount-matrix.h | 22 ++++++++++++ 3 files changed, 96 insertions(+) create mode 100644 src/accel-mount-matrix.c create mode 100644 src/accel-mount-matrix.h diff --git a/src/Makefile.am b/src/Makefile.am index a672e69..da8c16b 100644 --- a/src/Makefile.am +++ b/src/Makefile.am @@ -26,6 +26,8 @@ iio_sensor_proxy_SOURCES = \ drv-iio-buffer-compass.c \ iio-buffer-utils.h \ iio-buffer-utils.c \ + accel-mount-matrix.h \ + accel-mount-matrix.c \ $(BUILT_SOURCES) iio_sensor_proxy_CPPFLAGS = \ diff --git a/src/accel-mount-matrix.c b/src/accel-mount-matrix.c new file mode 100644 index 0000000..716bd82 --- /dev/null +++ b/src/accel-mount-matrix.c @@ -0,0 +1,72 @@ +/* + * Copyright (c) 2016 Bastien Nocera + * + * 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 +#include + +#include "accel-mount-matrix.h" + +/* The format is the same used in the iio core to export the values: + * https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/drivers/iio/industrialio-core.c?id=dfc57732ad38f93ae6232a3b4e64fd077383a0f1#n431 + */ + +static IioAccelVec3 id_matrix[3] = { + { 1.0, 0.0, 0.0 }, + { 0.0, 1.0, 0.0 }, + { 0.0, 0.0, 1.0 } +}; + +gboolean +parse_mount_matrix (const char *mtx, + IioAccelVec3 *vecs[3]) +{ + IioAccelVec3 *ret; + + g_return_val_if_fail (vecs != NULL, FALSE); + + + /* Empty string means we use the identity matrix */ + if (mtx == NULL || *mtx == '\0') { + *vecs = g_memdup (id_matrix, sizeof(id_matrix)); + return TRUE; + } + + ret = g_new0 (IioAccelVec3, 3); + if (sscanf (mtx, "%f, %f, %f; %f, %f, %f; %f, %f, %f", + &ret[0].x, &ret[0].y, &ret[0].z, + &ret[1].x, &ret[1].y, &ret[1].z, + &ret[2].x, &ret[2].y, &ret[2].z) != 9) { + g_free (ret); + g_warning ("Failed to parse '%s' as a mount matrix", mtx); + return FALSE; + } + + *vecs = ret; + + return TRUE; +} + +gboolean +apply_mount_matrix (const IioAccelVec3 vecs[3], + IioAccelVec3 *accel) +{ + float _x, _y, _z; + + g_return_val_if_fail (accel != NULL, FALSE); + + _x = accel->x * vecs[0].x + accel->y * vecs[0].y + accel->z * vecs[0].z; + _y = accel->x * vecs[1].x + accel->y * vecs[1].y + accel->z * vecs[1].z; + _z = accel->x * vecs[2].x + accel->y * vecs[2].y + accel->z * vecs[2].z; + + accel->x = _x; + accel->y = _y; + accel->z = _z; + + return TRUE; +} diff --git a/src/accel-mount-matrix.h b/src/accel-mount-matrix.h new file mode 100644 index 0000000..1ae162a --- /dev/null +++ b/src/accel-mount-matrix.h @@ -0,0 +1,22 @@ +/* + * Copyright (c) 2016 Bastien Nocera + * + * 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 + +typedef struct { + float x; + float y; + float z; +} IioAccelVec3; + +gboolean parse_mount_matrix (const char *mtx, + IioAccelVec3 *vecs[3]); + +gboolean apply_mount_matrix (const IioAccelVec3 vecs[3], + IioAccelVec3 *accel);