From 0f8e8ccd66a668202eb8f651f0019d9cdda64165 Mon Sep 17 00:00:00 2001 From: Christian Balster Date: Wed, 20 May 2015 13:19:48 +0200 Subject: i9300: libsensors: add real accuracy reporting for magnetometer Change-Id: I3dd56f4d7fb0de8feca2422c165fe0c3cd92761a --- libsensors/akm8975.c | 30 +++++++++++++++++++++++++----- libsensors/input.c | 1 + 2 files changed, 26 insertions(+), 5 deletions(-) diff --git a/libsensors/akm8975.c b/libsensors/akm8975.c index 420c790..d8b1c46 100644 --- a/libsensors/akm8975.c +++ b/libsensors/akm8975.c @@ -56,7 +56,8 @@ struct akm8975_data { int akfs_get_magnetic_field(struct akm8975_data *akm8975_data, short *magnetic_data) { AK8975PRMS *params; - int rc; + int rc, aocret; + float radius; if (akm8975_data == NULL || magnetic_data == NULL) return -EINVAL; @@ -80,7 +81,7 @@ int akfs_get_magnetic_field(struct akm8975_data *akm8975_data, short *magnetic_d // AOC for magnetometer // Offset estimation is done in this function - AKFS_AOC(¶ms->m_aocv, params->mfv_hdata, ¶ms->mfv_ho); + aocret = AKFS_AOC(¶ms->m_aocv, params->mfv_hdata, ¶ms->mfv_ho); // Subtract offset // Then, a magnetic vector, the unit is uT, is stored in mfv_hvbuf. @@ -99,9 +100,25 @@ int akfs_get_magnetic_field(struct akm8975_data *akm8975_data, short *magnetic_d return -1; } + // Check the size of magnetic vector + radius = sqrtf( + (params->mfv_hvec.u.x * params->mfv_hvec.u.x) + + (params->mfv_hvec.u.y * params->mfv_hvec.u.y) + + (params->mfv_hvec.u.z * params->mfv_hvec.u.z)); + + // Sanity check result and set accuracy + if ((radius > MAGNETIC_FIELD_EARTH_MAX + 10) || (radius < MAGNETIC_FIELD_EARTH_MIN - 10)) { + params->mi_hstatus = SENSOR_STATUS_UNRELIABLE; + } else if(params->mi_hstatus == SENSOR_STATUS_UNRELIABLE) { + params->mi_hstatus = SENSOR_STATUS_ACCURACY_MEDIUM; + } else if (aocret == AKFS_SUCCESS) { + params->mi_hstatus = SENSOR_STATUS_ACCURACY_HIGH; + } + akm8975_data->magnetic.x = params->mfv_hvec.u.x; akm8975_data->magnetic.y = params->mfv_hvec.u.y; akm8975_data->magnetic.z = params->mfv_hvec.u.z; + akm8975_data->magnetic.status = params->mi_hstatus; return 0; } @@ -223,6 +240,8 @@ void *akm8975_thread(void *thread_data) write(uinput_fd, &event, sizeof(event)); input_event_set(&event, EV_REL, REL_Z, (int) (data->magnetic.z * 1000)); write(uinput_fd, &event, sizeof(event)); + input_event_set(&event, EV_REL, REL_MISC, (int) data->magnetic.status); + write(uinput_fd, &event, sizeof(event)); input_event_set(&event, EV_SYN, 0, 0); write(uinput_fd, &event, sizeof(event)); @@ -446,7 +465,7 @@ int akm8975_activate(struct smdk4x12_sensors_handlers *handlers) // Initialize for AOC AKFS_InitAOC(&akfs_params->m_aocv); // Initialize magnetic status - akfs_params->mi_hstatus = 0; + akfs_params->mi_hstatus = SENSOR_STATUS_UNRELIABLE; handlers->activated = 1; pthread_mutex_unlock(&data->mutex); @@ -547,8 +566,6 @@ int akm8975_get_data(struct smdk4x12_sensors_handlers *handlers, event->sensor = handlers->handle; event->type = handlers->handle; - event->magnetic.status = SENSOR_STATUS_ACCURACY_MEDIUM; - do { rc = read(input_fd, &input_event, sizeof(input_event)); if (rc < (int) sizeof(input_event)) @@ -565,6 +582,9 @@ int akm8975_get_data(struct smdk4x12_sensors_handlers *handlers, case REL_Z: event->magnetic.z = akm8975_convert(input_event.value); break; + case REL_MISC: + event->magnetic.status = input_event.value; + break; default: continue; } diff --git a/libsensors/input.c b/libsensors/input.c index 5a58b45..cdd82c9 100644 --- a/libsensors/input.c +++ b/libsensors/input.c @@ -88,6 +88,7 @@ int uinput_rel_create(const char *name) rc |= ioctl(uinput_fd, UI_SET_RELBIT, REL_X); rc |= ioctl(uinput_fd, UI_SET_RELBIT, REL_Y); rc |= ioctl(uinput_fd, UI_SET_RELBIT, REL_Z); + rc |= ioctl(uinput_fd, UI_SET_RELBIT, REL_MISC); rc |= ioctl(uinput_fd, UI_SET_EVBIT, EV_SYN); if (rc < 0) { -- cgit v1.1