blob: f3c0b3a51c948291ea8060c650d6be078cedf004 [file] [log] [blame]
/*
* 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;
}