iio-sensors: support conversion matrix gyroscope

With the prop hal.sensors.iio.anglvel.matrix=f1x,f1y,f1z,f2x,f2y,f2z,f3x,f3y,f3z
it is possible to manipulate the sensors directions and assignment to the
corresponding axes. The default is
hal.sensors.iio.anglvel.matrix=1,0,0,0,1,0,0,0,1

diff --git a/iio-sensors.cpp b/iio-sensors.cpp
index ca50c91..230ccb7 100644 (file)
--- a/iio-sensors.cpp
+++ b/iio-sensors.cpp
@@ -343,11 +343,21 @@ template<> int Sensor<ID_GYROSCOPE>::readEvents(sensors_event_t *data, int cnt)
 {
        static float scale = read_sysfs_float((*nodes)[0]);
        int ret = SensorBase::readEvents(data, cnt);
-       // TODO: read orientation from the properties
+       char cm[PROPERTY_VALUE_MAX];
+       float m[9];
+       int v[3];
+
+       property_get("hal.sensors.iio.anglvel.matrix", cm, "1,0,0,0,1,0,0,0,1" );
+       sscanf(cm, "%f,%f,%f,%f,%f,%f,%f,%f,%f", &m[0], &m[1], &m[2], &m[3], &m[4], &m[5], &m[6], &m[7], &m[8]);
+
        for (int i = 0; i < ret; ++i) {
-               data[i].gyro.x = scale * read_sysfs_int("in_anglvel_x_raw");
-               data[i].gyro.y = scale * read_sysfs_int("in_anglvel_y_raw");
-               data[i].gyro.z = scale * read_sysfs_int("in_anglvel_z_raw");
+               v[0] = read_sysfs_int("in_anglvel_x_raw");
+               v[1] = read_sysfs_int("in_anglvel_y_raw");
+               v[2] = read_sysfs_int("in_anglvel_z_raw");
+               // create matrix * vector product
+               data[i].gyro.x = scale * (m[0] * v[0] + m[1] * v[1] + m[2] * v[2]);
+               data[i].gyro.y = scale * (m[3] * v[0] + m[4] * v[1] + m[5] * v[2]);
+               data[i].gyro.z = scale * (m[6] * v[0] + m[7] * v[1] + m[8] * v[2]);
                data[i].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
        }
        return ret;