diff options
Diffstat (limited to 'sensors/sensors_trout.c')
| -rw-r--r-- | sensors/sensors_trout.c | 599 |
1 files changed, 599 insertions, 0 deletions
diff --git a/sensors/sensors_trout.c b/sensors/sensors_trout.c new file mode 100644 index 00000000..f3c0b3a5 --- /dev/null +++ b/sensors/sensors_trout.c @@ -0,0 +1,599 @@ +/* + * 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; +} + |