iio-sensors: add the missing closedir

[android-x86/hardware-libsensors.git] / iio-sensors.cpp

diff --git a/iio-sensors.cpp b/iio-sensors.cpp
index d62db90..d4ffa04 100644 (file)
--- a/iio-sensors.cpp
+++ b/iio-sensors.cpp
@@ -12,8 +12,8 @@
 
 #define LOG_TAG "iio-sensors"
 
-//#include <cmath>
 #include <new>
+#include <cmath>
 #include <cerrno>
 #include <cstdlib>
 #include <cstring>
@@ -239,11 +239,21 @@ template<> int Sensor<ID_ACCELERATION>::readEvents(sensors_event_t *data, int cn
 {
        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.accel.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].acceleration.x = -scale * read_sysfs_int("in_accel_x_raw");
-               data[i].acceleration.y =  scale * read_sysfs_int("in_accel_y_raw");
-               data[i].acceleration.z = -scale * read_sysfs_int("in_accel_z_raw");
+               v[0] = read_sysfs_int("in_accel_x_raw");
+               v[1] = read_sysfs_int("in_accel_y_raw");
+               v[2] = read_sysfs_int("in_accel_z_raw");
+               // create matrix * vector product
+               data[i].acceleration.x = scale * (m[0] * v[0] + m[1] * v[1] + m[2] * v[2]);
+               data[i].acceleration.y = scale * (m[3] * v[0] + m[4] * v[1] + m[5] * v[2]);
+               data[i].acceleration.z = scale * (m[6] * v[0] + m[7] * v[1] + m[8] * v[2]);
                data[i].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
        }
        return ret;
@@ -270,10 +280,21 @@ template<> int Sensor<ID_MAGNETIC_FIELD>::readEvents(sensors_event_t *data, int
        static float scale_y = (*nodes)[1] ? read_sysfs_float((*nodes)[1]) : scale_x;
        static float scale_z = (*nodes)[2] ? read_sysfs_float((*nodes)[2]) : scale_x;
        int ret = SensorBase::readEvents(data, cnt);
+       char cm[PROPERTY_VALUE_MAX];
+       float m[9];
+       int v[3];
+
+       property_get("hal.sensors.iio.magn.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].magnetic.x = scale_x * read_sysfs_int("in_magn_x_raw");
-               data[i].magnetic.y = scale_y * read_sysfs_int("in_magn_y_raw");
-               data[i].magnetic.z = scale_z * read_sysfs_int("in_magn_z_raw");
+               v[0] = read_sysfs_int("in_magn_x_raw");
+               v[1] = read_sysfs_int("in_magn_y_raw");
+               v[2] = read_sysfs_int("in_magn_z_raw");
+               // create matrix * vector product
+               data[i].magnetic.x = scale_x * (m[0] * v[0] + m[1] * v[1] + m[2] * v[2]);
+               data[i].magnetic.y = scale_y * (m[3] * v[0] + m[4] * v[1] + m[5] * v[2]);
+               data[i].magnetic.z = scale_z * (m[6] * v[0] + m[7] * v[1] + m[8] * v[2]);
                data[i].magnetic.status = SENSOR_STATUS_ACCURACY_HIGH;
        }
        return ret;
@@ -322,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;
@@ -381,14 +412,16 @@ SensorPollContext::SensorPollContext(const struct hw_module_t *module, struct hw
                while (struct dirent *de = readdir(dir)) {
                        if (!strncmp(de->d_name, "iio:device", 10)) {
                                strcpy(path + len, de->d_name);
-                               for (int i = 0; probeSensors[i] && i < MAX_SENSORS; ++i) {
+                               for (size_t i = 0; i < (sizeof(probeSensors) / sizeof(*probeSensors)); ++i) {
                                        if (SensorBase *s = probeSensors[i](path)) {
                                                sensors[i] = s;
-                                               sensors_list[count++] =*s;
+                                               sensors_list[count++] = *s;
+                                               ALOGD("found %s", __FUNCTION__, s->name);
                                        }
                                }
                        }
                }
+               closedir(dir);
        }
        ALOGD("%s: module=%p sensors: %d", __FUNCTION__, module, count);
 }
@@ -500,5 +533,6 @@ struct sensors_module_t HAL_MODULE_INFO_SYM = {
                dso: 0,
                reserved: { }
        },
-       get_sensors_list: sensors_get_sensors_list
+       get_sensors_list: sensors_get_sensors_list,
+       set_operation_mode: 0
 };