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.
This commit is contained in:
@@ -26,6 +26,8 @@ iio_sensor_proxy_SOURCES = \
|
|||||||
drv-iio-buffer-compass.c \
|
drv-iio-buffer-compass.c \
|
||||||
iio-buffer-utils.h \
|
iio-buffer-utils.h \
|
||||||
iio-buffer-utils.c \
|
iio-buffer-utils.c \
|
||||||
|
accel-mount-matrix.h \
|
||||||
|
accel-mount-matrix.c \
|
||||||
$(BUILT_SOURCES)
|
$(BUILT_SOURCES)
|
||||||
|
|
||||||
iio_sensor_proxy_CPPFLAGS = \
|
iio_sensor_proxy_CPPFLAGS = \
|
||||||
|
72
src/accel-mount-matrix.c
Normal file
72
src/accel-mount-matrix.c
Normal file
@@ -0,0 +1,72 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (c) 2016 Bastien Nocera <hadess@hadess.net>
|
||||||
|
*
|
||||||
|
* 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 <glib.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
#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;
|
||||||
|
}
|
22
src/accel-mount-matrix.h
Normal file
22
src/accel-mount-matrix.h
Normal file
@@ -0,0 +1,22 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (c) 2016 Bastien Nocera <hadess@hadess.net>
|
||||||
|
*
|
||||||
|
* 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 <glib.h>
|
||||||
|
|
||||||
|
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);
|
Reference in New Issue
Block a user