summaryrefslogtreecommitdiffstats
path: root/sensors/geomagneticd/geomagneticd.c
diff options
context:
space:
mode:
Diffstat (limited to 'sensors/geomagneticd/geomagneticd.c')
-rw-r--r--sensors/geomagneticd/geomagneticd.c521
1 files changed, 521 insertions, 0 deletions
diff --git a/sensors/geomagneticd/geomagneticd.c b/sensors/geomagneticd/geomagneticd.c
new file mode 100644
index 0000000..61c4795
--- /dev/null
+++ b/sensors/geomagneticd/geomagneticd.c
@@ -0,0 +1,521 @@
+/*
+ * Copyright (C) 2013 Paul Kocialkowski <contact@paulk.fr>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <poll.h>
+#include <linux/input.h>
+
+#include <hardware/sensors.h>
+#include <hardware/hardware.h>
+
+#define LOG_TAG "geomagneticd"
+#include <utils/Log.h>
+
+#include "geomagneticd.h"
+
+// This geomagnetic daemon is in charge of finding the correct calibration
+// offsets to apply to the YAS530 magnetic field sensor.
+// This is done by finding raw data extrema (minimum and maximum) for each axis
+// and calculating the offset so that these values are -45uT and 45uT.
+
+/*
+ * Config
+ */
+
+int geomagneticd_config_read(struct geomagneticd_data *data)
+{
+ char buffer[100] = { 0 };
+ int config_fd = -1;
+ int rc;
+
+ if (data == NULL)
+ return -EINVAL;
+
+ config_fd = open(GEOMAGNETICD_CONFIG_PATH, O_RDONLY);
+ if (config_fd < 0) {
+ ALOGE("%s: Unable to open config", __func__);
+ goto error;
+ }
+
+ rc = read(config_fd, buffer, sizeof(buffer));
+ if (rc <= 0) {
+ ALOGE("%s: Unable to read config", __func__);
+ goto error;
+ }
+
+ rc = sscanf(buffer, "%d,%d,%d,%d,%d,%d,%d",
+ &data->hard_offsets[0], &data->hard_offsets[1], &data->hard_offsets[2],
+ &data->calib_offsets[0], &data->calib_offsets[1], &data->calib_offsets[2],
+ &data->accuracy);
+ if (rc != 7) {
+ ALOGE("%s: Unable to parse config", __func__);
+ goto error;
+ }
+
+ rc = 0;
+ goto complete;
+
+error:
+ rc = -1;
+
+complete:
+ if (config_fd >= 0)
+ close(config_fd);
+
+ return rc;
+}
+
+int geomagneticd_config_write(struct geomagneticd_data *data)
+{
+ char buffer[100] = { 0 };
+ int config_fd = -1;
+ int rc;
+
+ if (data == NULL)
+ return -EINVAL;
+
+ config_fd = open(GEOMAGNETICD_CONFIG_BACKUP_PATH, O_WRONLY | O_TRUNC | O_CREAT, 0644);
+ if (config_fd < 0) {
+ ALOGE("%s: Unable to open config", __func__);
+ goto error;
+ }
+
+ sprintf(buffer, "%d,%d,%d,%d,%d,%d,%d",
+ data->hard_offsets[0], data->hard_offsets[1], data->hard_offsets[2],
+ data->calib_offsets[0], data->calib_offsets[1], data->calib_offsets[2],
+ data->accuracy);
+
+ rc = write(config_fd, buffer, strlen(buffer) + 1);
+ if (rc < (int) strlen(buffer) + 1) {
+ ALOGE("%s: Unable to write config", __func__);
+ goto error;
+ }
+
+ rename(GEOMAGNETICD_CONFIG_BACKUP_PATH, GEOMAGNETICD_CONFIG_PATH);
+
+ rc = 0;
+ goto complete;
+
+error:
+ rc = -1;
+
+complete:
+ if (config_fd >= 0)
+ close(config_fd);
+
+ return rc;
+}
+
+/*
+ * Offsets
+ */
+
+int geomagneticd_offsets_read(struct geomagneticd_data *data)
+{
+ char buffer[100] = { 0 };
+ int offsets_fd = -1;
+ int rc;
+
+ if (data == NULL)
+ return -EINVAL;
+
+ offsets_fd = open(data->path_offsets, O_RDONLY);
+ if (offsets_fd < 0) {
+ ALOGE("%s: Unable to open offsets", __func__);
+ goto error;
+ }
+
+ rc = read(offsets_fd, buffer, sizeof(buffer));
+ if (rc <= 0) {
+ ALOGE("%s: Unable to read offsets", __func__);
+ goto error;
+ }
+
+ rc = sscanf(buffer, "%d %d %d %d %d %d %d",
+ &data->hard_offsets[0], &data->hard_offsets[1], &data->hard_offsets[2],
+ &data->calib_offsets[0], &data->calib_offsets[1], &data->calib_offsets[2],
+ &data->accuracy);
+ if (rc != 7) {
+ ALOGE("%s: Unable to parse offsets", __func__);
+ goto error;
+ }
+
+ rc = 0;
+ goto complete;
+
+error:
+ rc = -1;
+
+complete:
+ if (offsets_fd >= 0)
+ close(offsets_fd);
+
+ return rc;
+}
+
+int geomagneticd_offsets_write(struct geomagneticd_data *data)
+{
+ char buffer[100] = { 0 };
+ int offsets_fd = -1;
+ int rc;
+
+ if (data == NULL)
+ return -EINVAL;
+
+ offsets_fd = open(data->path_offsets, O_WRONLY);
+ if (offsets_fd < 0) {
+ ALOGE("%s: Unable to open offsets", __func__);
+ goto error;
+ }
+
+ sprintf(buffer, "%d %d %d %d %d %d %d\n",
+ data->hard_offsets[0], data->hard_offsets[1], data->hard_offsets[2],
+ data->calib_offsets[0], data->calib_offsets[1], data->calib_offsets[2],
+ data->accuracy);
+
+ rc = write(offsets_fd, buffer, strlen(buffer) + 1);
+ if (rc < (int) strlen(buffer) + 1) {
+ ALOGE("%s: Unable to write offsets", __func__);
+ goto error;
+ }
+
+ rc = 0;
+ goto complete;
+
+error:
+ rc = -1;
+
+complete:
+ if (offsets_fd >= 0)
+ close(offsets_fd);
+
+ return rc;
+}
+
+int geomagneticd_offsets_init(struct geomagneticd_data *data)
+{
+ int count;
+ int i;
+
+ if (data == NULL)
+ return -EINVAL;
+
+ count = sizeof(data->hard_offsets) / sizeof(int);
+
+ // 0x7f is an invalid value for hard offsets
+ for (i = 0; i < count; i++)
+ data->hard_offsets[i] = 0x7f;
+
+ count = sizeof(data->calib_offsets) / sizeof(int);
+
+ // 0x0x7fffffff is an invalid value for calib offsets
+ for (i = 0; i < count; i++)
+ data->calib_offsets[i] = 0x7fffffff;
+
+ return 0;
+}
+
+int geomagneticd_offsets_check(struct geomagneticd_data *data)
+{
+ int count;
+ int i;
+
+ if (data == NULL)
+ return -EINVAL;
+
+ count = sizeof(data->hard_offsets) / sizeof(int);
+
+ // 0x7f is an invalid value for hard offsets
+ for (i = 0; i < count; i++)
+ if (data->hard_offsets[i] == 0x7f)
+ return 0;
+
+ count = sizeof(data->calib_offsets) / sizeof(int);
+
+ // 0x0x7fffffff is an invalid value for calib offsets
+ for (i = 0; i < count; i++)
+ if (data->calib_offsets[i] == 0x7fffffff)
+ return 0;
+
+ return 1;
+}
+
+/*
+ * Geomagneticd
+ */
+
+int geomagneticd_magnetic_extrema_init(struct geomagneticd_data *data)
+{
+ int count;
+ int i;
+
+ if (data == NULL)
+ return -EINVAL;
+
+ count = sizeof(data->calib_offsets) / sizeof(int);
+
+ // Approximate the previous extrema from the calib offsets
+ for (i = 0; i < count; i++) {
+ data->magnetic_extrema[0][i] = -(data->calib_offsets[i] + 45) * 1000 + 5000;
+ data->magnetic_extrema[1][i] = (data->calib_offsets[i] + 45) * 1000 - 5000;
+ }
+
+ return 0;
+}
+
+int geomagneticd_magnetic_extrema(struct geomagneticd_data *data, int index,
+ int value)
+{
+ if (data == NULL || index < 0 || index >= 3)
+ return -EINVAL;
+
+ if (value == 0)
+ return 0;
+
+ // Update the extrema from the current value if needed
+ if (value < data->magnetic_extrema[0][index] || data->magnetic_extrema[0][index] == 0)
+ data->magnetic_extrema[0][index] = value;
+ if (value > data->magnetic_extrema[1][index] || data->magnetic_extrema[1][index] == 0)
+ data->magnetic_extrema[1][index] = value;
+
+ return 0;
+}
+
+int geomagneticd_calib_offsets(struct geomagneticd_data *data)
+{
+ int calib_offsets[3];
+ int offsets[2];
+ int update;
+ int count;
+ int rc;
+ int i;
+
+ if (data == NULL)
+ return -EINVAL;
+
+ // Calculating the offset is only meaningful when enough values were
+ // obtained. There is no need to calculate it too often either.
+ if (data->count % 10 != 0)
+ return 0;
+
+ update = 0;
+
+ count = sizeof(data->calib_offsets) / sizeof(int);
+
+ // Calculate the calib offset for each axis to have values in [-45;45] uT
+ for (i = 0; i < count; i++) {
+ offsets[0] = data->magnetic_extrema[0][i] + 45 * 1000;
+ offsets[1] = data->magnetic_extrema[1][i] - 45 * 1000;
+ calib_offsets[i] = (offsets[0] + offsets[1]) / 2;
+
+ if (calib_offsets[i] != data->calib_offsets[i]) {
+ data->calib_offsets[i] = calib_offsets[i];
+ update = 1;
+ }
+ }
+
+ if (update) {
+ data->accuracy = 1;
+
+ rc = geomagneticd_offsets_write(data);
+ if (rc < 0) {
+ ALOGE("%s: Unable to write offsets", __func__);
+ return -1;
+ }
+
+ rc = geomagneticd_config_write(data);
+ if (rc < 0) {
+ ALOGE("%s: Unable to write config", __func__);
+ return -1;
+ }
+ }
+
+ return 0;
+}
+
+int geomagneticd_poll(struct geomagneticd_data *data)
+{
+ struct input_event input_event;
+ struct pollfd poll_fd;
+ int rc;
+
+ if (data == NULL)
+ return -EINVAL;
+
+ if (data->input_fd < 0)
+ return -1;
+
+ memset(&poll_fd, 0, sizeof(poll_fd));
+ poll_fd.fd = data->input_fd;
+ poll_fd.events = POLLIN;
+
+ while (1) {
+ rc = poll(&poll_fd, 1, -1);
+ if (rc < 0) {
+ ALOGE("%s: poll failure", __func__);
+ goto error;
+ }
+
+ if (!(poll_fd.revents & POLLIN))
+ continue;
+
+ poll_fd.revents = 0;
+
+ rc = read(data->input_fd, &input_event, sizeof(input_event));
+ if (rc < (int) sizeof(input_event)) {
+ ALOGE("%s: Unable to read input event", __func__);
+ continue;
+ }
+
+ // Update the extrema from the current value
+ if(input_event.type == EV_ABS) {
+ switch (input_event.code) {
+ case ABS_X:
+ geomagneticd_magnetic_extrema(data, 0, input_event.value);
+ break;
+ case ABS_Y:
+ geomagneticd_magnetic_extrema(data, 1, input_event.value);
+ break;
+ case ABS_Z:
+ geomagneticd_magnetic_extrema(data, 2, input_event.value);
+ break;
+ }
+ }
+
+ if (input_event.type == EV_SYN) {
+ // Sometimes, the hard offsets cannot be read at startup
+ // so we need to do it now
+ if (!geomagneticd_offsets_check(data)) {
+ rc = geomagneticd_offsets_read(data);
+ if (rc < 0) {
+ ALOGE("%s: Unable to read offsets", __func__);
+ continue;
+ }
+
+ // Most likely, the calib offset will be invalid
+ if (geomagneticd_offsets_check(data)) {
+ data->accuracy = 1;
+ geomagneticd_magnetic_extrema_init(data);
+ }
+
+ rc = geomagneticd_config_write(data);
+ if (rc < 0) {
+ ALOGE("%s: Unable to write config", __func__);
+ continue;
+ }
+ }
+
+ data->count++;
+
+ rc = geomagneticd_calib_offsets(data);
+ if (rc < 0) {
+ ALOGE("%s: Unable to calib offsets", __func__);
+ continue;
+ }
+ }
+ }
+
+ rc = 0;
+ goto complete;
+
+error:
+ rc = -1;
+
+complete:
+ return rc;
+}
+
+int main(int argc, char *argv[])
+{
+ struct geomagneticd_data *geomagneticd_data = NULL;
+ char path[PATH_MAX] = { 0 };
+ int input_fd = -1;
+ int rc;
+
+ geomagneticd_data = (struct geomagneticd_data *)
+ calloc(1, sizeof(struct geomagneticd_data));
+
+ input_fd = input_open("geomagnetic_raw");
+ if (input_fd < 0) {
+ ALOGE("%s: Unable to open input", __func__);
+ goto error;
+ }
+
+ rc = sysfs_path_prefix("geomagnetic_raw", (char *) &path);
+ if (rc < 0 || path[0] == '\0') {
+ ALOGE("%s: Unable to open sysfs", __func__);
+ goto error;
+ }
+
+ snprintf(geomagneticd_data->path_offsets, PATH_MAX, "%s/offsets", path);
+
+ geomagneticd_data->input_fd = input_fd;
+
+ geomagneticd_offsets_init(geomagneticd_data);
+
+ // Attempt to read the offsets from the config
+ rc = geomagneticd_config_read(geomagneticd_data);
+ if (rc < 0 || !geomagneticd_offsets_check(geomagneticd_data)) {
+ // Read the offsets from the driver
+ rc = geomagneticd_offsets_read(geomagneticd_data);
+ if (rc < 0) {
+ ALOGE("%s: Unable to read offsets", __func__);
+ goto error;
+ }
+
+ // Most likely, the calib offset will be invalid and the hard
+ // offset may be invalid as well
+ if (geomagneticd_offsets_check(geomagneticd_data)) {
+ geomagneticd_data->accuracy = 1;
+ geomagneticd_magnetic_extrema_init(geomagneticd_data);
+ }
+ } else {
+ // Get the magnetic extrema from the config's offsets
+ geomagneticd_magnetic_extrema_init(geomagneticd_data);
+
+ rc = geomagneticd_offsets_write(geomagneticd_data);
+ if (rc < 0) {
+ ALOGE("%s: Unable to write offsets", __func__);
+ goto error;
+ }
+ }
+
+ rc = geomagneticd_poll(geomagneticd_data);
+ if (rc < 0)
+ goto error;
+
+ rc = 0;
+ goto complete;
+
+error:
+ while (1)
+ sleep(3600);
+
+ rc = 1;
+
+complete:
+ if (input_fd >= 0)
+ close(input_fd);
+
+ if (geomagneticd_data != NULL)
+ free(geomagneticd_data);
+
+ return rc;
+}