| /* |
| * Copyright 2008, The Android Open Source Project |
| * |
| * Licensed under the Apache License, Version 2.0 (the "License"); |
| * you may not use this file except in compliance with the License. |
| * You may obtain a copy of the License at |
| * |
| * http://www.apache.org/licenses/LICENSE-2.0 |
| * |
| * Unless required by applicable law or agreed to in writing, software |
| * distributed under the License is distributed on an "AS IS" BASIS, |
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| * See the License for the specific language governing permissions and |
| * limitations under the License. |
| */ |
| |
| #define LOG_TAG "Sensors" |
| |
| #include <hardware/sensors.h> |
| #include <fcntl.h> |
| #include <errno.h> |
| #include <dirent.h> |
| #include <math.h> |
| #include <poll.h> |
| |
| #include <linux/input.h> |
| #include <linux/akm8976.h> |
| |
| #include <cutils/log.h> |
| #include <cutils/atomic.h> |
| |
| /*****************************************************************************/ |
| |
| #define AKM_DEVICE_NAME "/dev/akm8976_aot" |
| |
| #define SUPPORTED_SENSORS (SENSORS_ORIENTATION | \ |
| SENSORS_ACCELERATION | \ |
| SENSORS_MAGNETIC_FIELD | \ |
| SENSORS_ORIENTATION_RAW) |
| |
| |
| // sensor IDs must be a power of two and |
| // must match values in SensorManager.java |
| #define EVENT_TYPE_ACCEL_X ABS_X |
| #define EVENT_TYPE_ACCEL_Y ABS_Z |
| #define EVENT_TYPE_ACCEL_Z ABS_Y |
| #define EVENT_TYPE_ACCEL_STATUS ABS_WHEEL |
| |
| #define EVENT_TYPE_YAW ABS_RX |
| #define EVENT_TYPE_PITCH ABS_RY |
| #define EVENT_TYPE_ROLL ABS_RZ |
| #define EVENT_TYPE_ORIENT_STATUS ABS_RUDDER |
| |
| #define EVENT_TYPE_MAGV_X ABS_HAT0X |
| #define EVENT_TYPE_MAGV_Y ABS_HAT0Y |
| #define EVENT_TYPE_MAGV_Z ABS_BRAKE |
| |
| #define EVENT_TYPE_TEMPERATURE ABS_THROTTLE |
| #define EVENT_TYPE_STEP_COUNT ABS_GAS |
| |
| // 720 LSG = 1G |
| #define LSG (720.0f) |
| |
| // conversion of acceleration data to SI units (m/s^2) |
| #define CONVERT_A (GRAVITY_EARTH / LSG) |
| #define CONVERT_A_X (CONVERT_A) |
| #define CONVERT_A_Y (-CONVERT_A) |
| #define CONVERT_A_Z (CONVERT_A) |
| |
| // conversion of magnetic data to uT units |
| #define CONVERT_M (1.0f/16.0f) |
| #define CONVERT_M_X (CONVERT_M) |
| #define CONVERT_M_Y (CONVERT_M) |
| #define CONVERT_M_Z (CONVERT_M) |
| |
| #define SENSOR_STATE_MASK (0x7FFF) |
| |
| /*****************************************************************************/ |
| |
| static int sAkmFD = -1; |
| static uint32_t sActiveSensors = 0; |
| |
| /*****************************************************************************/ |
| |
| /* |
| * We use a Least Mean Squares filter to smooth out the output of the yaw |
| * sensor. |
| * |
| * The goal is to estimate the output of the sensor based on previous acquired |
| * samples. |
| * |
| * We approximate the input by a line with the equation: |
| * Z(t) = a * t + b |
| * |
| * We use the Least Mean Squares method to calculate a and b so that the |
| * distance between the line and the measured COUNT inputs Z(t) is minimal. |
| * |
| * In practice we only need to compute b, which is the value we're looking for |
| * (it's the estimated Z at t=0). However, to improve the latency a little bit, |
| * we're going to discard a certain number of samples that are too far from |
| * the estimated line and compute b again with the new (trimmed down) samples. |
| * |
| * notes: |
| * 'a' is the slope of the line, and physicaly represent how fast the input |
| * is changing. In our case, how fast the yaw is changing, that is, how fast the |
| * user is spinning the device (in degre / nanosecond). This value should be |
| * zero when the device is not moving. |
| * |
| * The minimum distance between the line and the samples (which we are not |
| * explicitely computing here), is an indication of how bad the samples are |
| * and gives an idea of the "quality" of the estimation (well, really of the |
| * sensor values). |
| * |
| */ |
| |
| /* sensor rate in me */ |
| #define SENSORS_RATE_MS 20 |
| /* timeout (constant value) in ms */ |
| #define SENSORS_TIMEOUT_MS 100 |
| /* # of samples to look at in the past for filtering */ |
| #define COUNT 24 |
| /* prediction ratio */ |
| #define PREDICTION_RATIO (1.0f/3.0f) |
| /* prediction time in seconds (>=0) */ |
| #define PREDICTION_TIME ((SENSORS_RATE_MS*COUNT/1000.0f)*PREDICTION_RATIO) |
| |
| static float mV[COUNT*2]; |
| static float mT[COUNT*2]; |
| static int mIndex; |
| |
| static inline |
| float normalize(float x) |
| { |
| x *= (1.0f / 360.0f); |
| if (fabsf(x) >= 0.5f) |
| x = x - ceilf(x + 0.5f) + 1.0f; |
| if (x < 0) |
| x += 1.0f; |
| x *= 360.0f; |
| return x; |
| } |
| |
| static void LMSInit(void) |
| { |
| memset(mV, 0, sizeof(mV)); |
| memset(mT, 0, sizeof(mT)); |
| mIndex = COUNT; |
| } |
| |
| static float LMSFilter(int64_t time, int v) |
| { |
| const float ns = 1.0f / 1000000000.0f; |
| const float t = time*ns; |
| float v1 = mV[mIndex]; |
| if ((v-v1) > 180) { |
| v -= 360; |
| } else if ((v1-v) > 180) { |
| v += 360; |
| } |
| /* Manage the circular buffer, we write the data twice spaced by COUNT |
| * values, so that we don't have to memcpy() the array when it's full */ |
| mIndex++; |
| if (mIndex >= COUNT*2) |
| mIndex = COUNT; |
| mV[mIndex] = v; |
| mT[mIndex] = t; |
| mV[mIndex-COUNT] = v; |
| mT[mIndex-COUNT] = t; |
| |
| float A, B, C, D, E; |
| float a, b; |
| int i; |
| |
| A = B = C = D = E = 0; |
| for (i=0 ; i<COUNT-1 ; i++) { |
| const int j = mIndex - 1 - i; |
| const float Z = mV[j]; |
| const float T = 0.5f*(mT[j] + mT[j+1]) - t; |
| float dT = mT[j] - mT[j+1]; |
| dT *= dT; |
| A += Z*dT; |
| B += T*(T*dT); |
| C += (T*dT); |
| D += Z*(T*dT); |
| E += dT; |
| } |
| b = (A*B + C*D) / (E*B + C*C); |
| a = (E*b - A) / C; |
| float f = b + PREDICTION_TIME*a; |
| |
| //LOGD("A=%f, B=%f, C=%f, D=%f, E=%f", A,B,C,D,E); |
| //LOGD("%lld %d %f %f", time, v, f, a); |
| |
| f = normalize(f); |
| return f; |
| } |
| |
| /*****************************************************************************/ |
| |
| static int open_input() |
| { |
| /* scan all input drivers and look for "compass" */ |
| int fd = -1; |
| const char *dirname = "/dev/input"; |
| char devname[PATH_MAX]; |
| char *filename; |
| DIR *dir; |
| struct dirent *de; |
| dir = opendir(dirname); |
| if(dir == NULL) |
| return -1; |
| strcpy(devname, dirname); |
| filename = devname + strlen(devname); |
| *filename++ = '/'; |
| while((de = readdir(dir))) { |
| if(de->d_name[0] == '.' && |
| (de->d_name[1] == '\0' || |
| (de->d_name[1] == '.' && de->d_name[2] == '\0'))) |
| continue; |
| strcpy(filename, de->d_name); |
| fd = open(devname, O_RDONLY); |
| if (fd>=0) { |
| char name[80]; |
| if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) { |
| name[0] = '\0'; |
| } |
| if (!strcmp(name, "compass")) { |
| LOGD("using %s (name=%s)", devname, name); |
| break; |
| } |
| close(fd); |
| fd = -1; |
| } |
| } |
| closedir(dir); |
| |
| if (fd < 0) { |
| LOGE("Couldn't find or open 'compass' driver (%s)", strerror(errno)); |
| } |
| return fd; |
| } |
| |
| static int open_akm() |
| { |
| if (sAkmFD <= 0) { |
| sAkmFD = open(AKM_DEVICE_NAME, O_RDONLY); |
| LOGD("%s, fd=%d", __PRETTY_FUNCTION__, sAkmFD); |
| LOGE_IF(sAkmFD<0, "Couldn't open %s (%s)", |
| AKM_DEVICE_NAME, strerror(errno)); |
| if (sAkmFD >= 0) { |
| sActiveSensors = 0; |
| } |
| } |
| return sAkmFD; |
| } |
| |
| static void close_akm() |
| { |
| if (sAkmFD > 0) { |
| LOGD("%s, fd=%d", __PRETTY_FUNCTION__, sAkmFD); |
| close(sAkmFD); |
| sAkmFD = -1; |
| } |
| } |
| |
| static void enable_disable(int fd, uint32_t sensors, uint32_t mask) |
| { |
| if (fd<0) return; |
| short flags; |
| |
| if (sensors & SENSORS_ORIENTATION_RAW) { |
| sensors |= SENSORS_ORIENTATION; |
| mask |= SENSORS_ORIENTATION; |
| } else if (mask & SENSORS_ORIENTATION_RAW) { |
| mask |= SENSORS_ORIENTATION; |
| } |
| |
| if (mask & SENSORS_ORIENTATION) { |
| flags = (sensors & SENSORS_ORIENTATION) ? 1 : 0; |
| if (ioctl(fd, ECS_IOCTL_APP_SET_MFLAG, &flags) < 0) { |
| LOGE("ECS_IOCTL_APP_SET_MFLAG error (%s)", strerror(errno)); |
| } |
| } |
| if (mask & SENSORS_ACCELERATION) { |
| flags = (sensors & SENSORS_ACCELERATION) ? 1 : 0; |
| if (ioctl(fd, ECS_IOCTL_APP_SET_AFLAG, &flags) < 0) { |
| LOGE("ECS_IOCTL_APP_SET_AFLAG error (%s)", strerror(errno)); |
| } |
| } |
| if (mask & SENSORS_TEMPERATURE) { |
| flags = (sensors & SENSORS_TEMPERATURE) ? 1 : 0; |
| if (ioctl(fd, ECS_IOCTL_APP_SET_TFLAG, &flags) < 0) { |
| LOGE("ECS_IOCTL_APP_SET_TFLAG error (%s)", strerror(errno)); |
| } |
| } |
| #ifdef ECS_IOCTL_APP_SET_MVFLAG |
| if (mask & SENSORS_MAGNETIC_FIELD) { |
| flags = (sensors & SENSORS_MAGNETIC_FIELD) ? 1 : 0; |
| if (ioctl(fd, ECS_IOCTL_APP_SET_MVFLAG, &flags) < 0) { |
| LOGE("ECS_IOCTL_APP_SET_MVFLAG error (%s)", strerror(errno)); |
| } |
| } |
| #endif |
| } |
| |
| static uint32_t read_sensors_state(int fd) |
| { |
| if (fd<0) return 0; |
| short flags; |
| uint32_t sensors = 0; |
| // read the actual value of all sensors |
| if (!ioctl(fd, ECS_IOCTL_APP_GET_MFLAG, &flags)) { |
| if (flags) sensors |= SENSORS_ORIENTATION; |
| else sensors &= ~SENSORS_ORIENTATION; |
| } |
| if (!ioctl(fd, ECS_IOCTL_APP_GET_AFLAG, &flags)) { |
| if (flags) sensors |= SENSORS_ACCELERATION; |
| else sensors &= ~SENSORS_ACCELERATION; |
| } |
| if (!ioctl(fd, ECS_IOCTL_APP_GET_TFLAG, &flags)) { |
| if (flags) sensors |= SENSORS_TEMPERATURE; |
| else sensors &= ~SENSORS_TEMPERATURE; |
| } |
| #ifdef ECS_IOCTL_APP_SET_MVFLAG |
| if (!ioctl(fd, ECS_IOCTL_APP_GET_MVFLAG, &flags)) { |
| if (flags) sensors |= SENSORS_MAGNETIC_FIELD; |
| else sensors &= ~SENSORS_MAGNETIC_FIELD; |
| } |
| #endif |
| return sensors; |
| } |
| |
| /*****************************************************************************/ |
| |
| uint32_t sensors_control_init() |
| { |
| return SUPPORTED_SENSORS; |
| } |
| |
| int sensors_control_open() |
| { |
| return open_input(); |
| } |
| |
| uint32_t sensors_control_activate(uint32_t sensors, uint32_t mask) |
| { |
| mask &= SUPPORTED_SENSORS; |
| uint32_t active = sActiveSensors; |
| uint32_t new_sensors = (active & ~mask) | (sensors & mask); |
| uint32_t changed = active ^ new_sensors; |
| if (changed) { |
| int fd = open_akm(); |
| if (fd < 0) return 0; |
| |
| if (!active && new_sensors) { |
| // force all sensors to be updated |
| changed = SUPPORTED_SENSORS; |
| } |
| |
| enable_disable(fd, new_sensors, changed); |
| |
| if (active && !new_sensors) { |
| // close the driver |
| close_akm(); |
| } |
| sActiveSensors = active = new_sensors; |
| LOGD("sensors=%08x, real=%08x", |
| sActiveSensors, read_sensors_state(fd)); |
| } |
| return active; |
| } |
| |
| int sensors_control_delay(int32_t ms) |
| { |
| #ifdef ECS_IOCTL_APP_SET_DELAY |
| if (sAkmFD <= 0) { |
| return -1; |
| } |
| short delay = ms; |
| if (!ioctl(sAkmFD, ECS_IOCTL_APP_SET_DELAY, &delay)) { |
| return -errno; |
| } |
| return 0; |
| #else |
| return -1; |
| #endif |
| } |
| |
| /*****************************************************************************/ |
| |
| #define MAX_NUM_SENSORS 8 |
| static int sInputFD = -1; |
| static const int ID_O = 0; |
| static const int ID_A = 1; |
| static const int ID_T = 2; |
| static const int ID_M = 3; |
| static const int ID_OR = 7; // orientation raw |
| static sensors_data_t sSensors[MAX_NUM_SENSORS]; |
| static uint32_t sPendingSensors; |
| |
| int sensors_data_open(int fd) |
| { |
| int i; |
| LMSInit(); |
| memset(&sSensors, 0, sizeof(sSensors)); |
| for (i=0 ; i<MAX_NUM_SENSORS ; i++) { |
| // by default all sensors have high accuracy |
| // (we do this because we don't get an update if the value doesn't |
| // change). |
| sSensors[i].vector.status = SENSOR_STATUS_ACCURACY_HIGH; |
| } |
| sPendingSensors = 0; |
| sInputFD = dup(fd); |
| LOGD("sensors_data_open: fd = %d", sInputFD); |
| return 0; |
| } |
| |
| int sensors_data_close() |
| { |
| close(sInputFD); |
| sInputFD = -1; |
| return 0; |
| } |
| |
| static int pick_sensor(sensors_data_t* values) |
| { |
| uint32_t mask = SENSORS_MASK; |
| while(mask) { |
| uint32_t i = 31 - __builtin_clz(mask); |
| mask &= ~(1<<i); |
| if (sPendingSensors & (1<<i)) { |
| sPendingSensors &= ~(1<<i); |
| *values = sSensors[i]; |
| values->sensor = (1<<i); |
| LOGD_IF(0, "%d [%f, %f, %f]", (1<<i), |
| values->vector.x, |
| values->vector.y, |
| values->vector.z); |
| return (1<<i); |
| } |
| } |
| LOGE("No sensor to return!!! sPendingSensors=%08x", sPendingSensors); |
| // we may end-up in a busy loop, slow things down, just in case. |
| usleep(100000); |
| return -1; |
| } |
| |
| int sensors_data_poll(sensors_data_t* values, uint32_t sensors_of_interest) |
| { |
| struct input_event event; |
| int nread; |
| int64_t t; |
| |
| int fd = sInputFD; |
| if (fd <= 0) |
| return -1; |
| |
| // there are pending sensors, returns them now... |
| if (sPendingSensors) { |
| return pick_sensor(values); |
| } |
| |
| uint32_t new_sensors = 0; |
| struct pollfd fds; |
| fds.fd = fd; |
| fds.events = POLLIN; |
| fds.revents = 0; |
| |
| // wait until we get a complete event for an enabled sensor |
| while (1) { |
| nread = 0; |
| if (sensors_of_interest & SENSORS_ORIENTATION) { |
| /* We do some special processing if the orientation sensor is |
| * activated. In particular the yaw value is filtered with a |
| * LMS filter. Since the kernel only sends an event when the |
| * value changes, we need to wake up at regular intervals to |
| * generate an output value (the output value may not be |
| * constant when the input value is constant) |
| */ |
| int err = poll(&fds, 1, SENSORS_TIMEOUT_MS); |
| if (err == 0) { |
| struct timespec time; |
| time.tv_sec = time.tv_nsec = 0; |
| clock_gettime(CLOCK_MONOTONIC, &time); |
| |
| /* generate an output value */ |
| t = time.tv_sec*1000000000LL+time.tv_nsec; |
| new_sensors |= SENSORS_ORIENTATION; |
| sSensors[ID_O].orientation.yaw = |
| LMSFilter(t, sSensors[ID_O].orientation.yaw); |
| |
| /* generate a fake sensors event */ |
| event.type = EV_SYN; |
| event.time.tv_sec = time.tv_sec; |
| event.time.tv_usec = time.tv_nsec/1000; |
| nread = sizeof(event); |
| } |
| } |
| if (nread == 0) { |
| /* read the next event */ |
| nread = read(fd, &event, sizeof(event)); |
| } |
| if (nread == sizeof(event)) { |
| uint32_t v; |
| if (event.type == EV_ABS) { |
| //LOGD("type: %d code: %d value: %-5d time: %ds", |
| // event.type, event.code, event.value, |
| // (int)event.time.tv_sec); |
| switch (event.code) { |
| |
| case EVENT_TYPE_ACCEL_X: |
| new_sensors |= SENSORS_ACCELERATION; |
| sSensors[ID_A].acceleration.x = event.value * CONVERT_A_X; |
| break; |
| case EVENT_TYPE_ACCEL_Y: |
| new_sensors |= SENSORS_ACCELERATION; |
| sSensors[ID_A].acceleration.y = event.value * CONVERT_A_Y; |
| break; |
| case EVENT_TYPE_ACCEL_Z: |
| new_sensors |= SENSORS_ACCELERATION; |
| sSensors[ID_A].acceleration.z = event.value * CONVERT_A_Z; |
| break; |
| |
| case EVENT_TYPE_MAGV_X: |
| new_sensors |= SENSORS_MAGNETIC_FIELD; |
| sSensors[ID_M].magnetic.x = event.value * CONVERT_M_X; |
| break; |
| case EVENT_TYPE_MAGV_Y: |
| new_sensors |= SENSORS_MAGNETIC_FIELD; |
| sSensors[ID_M].magnetic.y = event.value * CONVERT_M_Y; |
| break; |
| case EVENT_TYPE_MAGV_Z: |
| new_sensors |= SENSORS_MAGNETIC_FIELD; |
| sSensors[ID_M].magnetic.z = event.value * CONVERT_M_Z; |
| break; |
| |
| case EVENT_TYPE_YAW: |
| new_sensors |= SENSORS_ORIENTATION | SENSORS_ORIENTATION_RAW; |
| t = event.time.tv_sec*1000000000LL + |
| event.time.tv_usec*1000; |
| sSensors[ID_O].orientation.yaw = |
| (sensors_of_interest & SENSORS_ORIENTATION) ? |
| LMSFilter(t, event.value) : event.value; |
| sSensors[ID_OR].orientation.yaw = event.value; |
| break; |
| case EVENT_TYPE_PITCH: |
| new_sensors |= SENSORS_ORIENTATION | SENSORS_ORIENTATION_RAW; |
| sSensors[ID_O].orientation.pitch = event.value; |
| sSensors[ID_OR].orientation.pitch = event.value; |
| break; |
| case EVENT_TYPE_ROLL: |
| new_sensors |= SENSORS_ORIENTATION | SENSORS_ORIENTATION_RAW; |
| sSensors[ID_O].orientation.roll = event.value; |
| sSensors[ID_OR].orientation.roll = event.value; |
| break; |
| |
| case EVENT_TYPE_TEMPERATURE: |
| new_sensors |= SENSORS_TEMPERATURE; |
| sSensors[ID_T].temperature = event.value; |
| break; |
| |
| case EVENT_TYPE_STEP_COUNT: |
| // step count (only reported in MODE_FFD) |
| // we do nothing with it for now. |
| break; |
| case EVENT_TYPE_ACCEL_STATUS: |
| // accuracy of the calibration (never returned!) |
| //LOGD("G-Sensor status %d", event.value); |
| break; |
| case EVENT_TYPE_ORIENT_STATUS: |
| // accuracy of the calibration |
| v = (uint32_t)(event.value & SENSOR_STATE_MASK); |
| LOGD_IF(sSensors[ID_O].orientation.status != (uint8_t)v, |
| "M-Sensor status %d", v); |
| sSensors[ID_O].orientation.status = (uint8_t)v; |
| sSensors[ID_OR].orientation.status = (uint8_t)v; |
| break; |
| } |
| } else if (event.type == EV_SYN) { |
| if (new_sensors) { |
| sPendingSensors = new_sensors; |
| int64_t t = event.time.tv_sec*1000000000LL + |
| event.time.tv_usec*1000; |
| while (new_sensors) { |
| uint32_t i = 31 - __builtin_clz(new_sensors); |
| new_sensors &= ~(1<<i); |
| sSensors[i].time = t; |
| } |
| return pick_sensor(values); |
| } |
| } |
| } |
| } |
| } |
| |
| uint32_t sensors_data_get_sensors() { |
| return SUPPORTED_SENSORS; |
| } |
| |