AkmSensor.cpp

/*
 * Copyright (C) 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.
 */

#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <unistd.h>
#include <dirent.h>
#include <sys/select.h>
#include <linux/akm8975.h>
#include <cutils/log.h>
#include "AkmSensor.h"
#include "sensors.h"

#define	SENSOR_STATE_MASK	(0x7FFF)

#define AKM_DEVICE_NAME		"/dev/akm8975_aot"

#define	ID_A	SENSORS_ACCELERATION_HANDLE
#define	ID_M	SENSORS_MAGNETIC_FIELD_HANDLE
#define ID_O	SENSORS_ORIENTATION_HANDLE

#define EVENT_TYPE_ACCEL_X			REL_X
#define EVENT_TYPE_ACCEL_Y			REL_Y
#define EVENT_TYPE_ACCEL_Z			REL_Z

#define EVENT_TYPE_YAW				REL_RX
#define EVENT_TYPE_PITCH			REL_RY
#define EVENT_TYPE_ROLL				REL_RZ
#define EVENT_TYPE_ORIENT_STATUS	REL_HWHEEL

#define EVENT_TYPE_MAGV_X			REL_DIAL
#define EVENT_TYPE_MAGV_Y			REL_WHEEL
#define EVENT_TYPE_MAGV_Z			REL_MISC

// 1024 LSG = 1G
#define LSG							(1024.0f)
#define MAX_RANGE_A					(2*GRAVITY_EARTH)
// 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 CONVERT_O					(1.0f/64.0f)
#define CONVERT_O_Y					(CONVERT_O)
#define CONVERT_O_P					(CONVERT_O)
#define CONVERT_O_R					(-CONVERT_O)

/*****************************************************************************/

AkmSensor::AkmSensor()
	: SensorBase(AKM_DEVICE_NAME, "compass"),
	mEnabled(0),
	mPendingMask(0),
	mInputReader(32)
{
	memset(mPendingEvents, 0, sizeof(mPendingEvents));

	mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
	mPendingEvents[Accelerometer].sensor = ID_A;
	mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
	mPendingEvents[Accelerometer].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;

	mPendingEvents[MagneticField].version = sizeof(sensors_event_t);
	mPendingEvents[MagneticField].sensor = ID_M;
	mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD;
	mPendingEvents[MagneticField].magnetic.status = SENSOR_STATUS_ACCURACY_HIGH;

	mPendingEvents[Orientation  ].version = sizeof(sensors_event_t);
	mPendingEvents[Orientation  ].sensor = ID_O;
	mPendingEvents[Orientation  ].type = SENSOR_TYPE_ORIENTATION;
	mPendingEvents[Orientation  ].orientation.status = SENSOR_STATUS_ACCURACY_HIGH;

	for (int i=0 ; i<numSensors ; i++)
		mDelays[i] = 200000000; // 200 ms by default

	// read the actual value of all sensors if they're enabled already
	struct input_absinfo absinfo;
	short flags = 0;

	open_device();

	if (!ioctl(dev_fd, ECS_IOCTL_APP_GET_AFLAG, &flags)) {
		if (flags)  {
			mEnabled |= 1<<Accelerometer;
		}
	}
	if (!ioctl(dev_fd, ECS_IOCTL_APP_GET_MVFLAG, &flags)) {
		if (flags)  {
			mEnabled |= 1<<MagneticField;
		}
	}
	if (!ioctl(dev_fd, ECS_IOCTL_APP_GET_MFLAG, &flags)) {
		if (flags)  {
			mEnabled |= 1<<Orientation;
		}
	}
	if (!mEnabled) {
		close_device();
	}
}

AkmSensor::~AkmSensor() {
}

int AkmSensor::enable(int32_t handle, int en)
{
	int what = -1;
	switch (handle) {
		case ID_A: what = Accelerometer; break;
		case ID_M: what = MagneticField; break;
		case ID_O: what = Orientation;   break;
	}

	if (uint32_t(what) >= numSensors)
		return -EINVAL;

	int newState  = en ? 1 : 0;
	int err = 0;

	if ((uint32_t(newState)<<what) != (mEnabled & (1<<what))) {
		if (!mEnabled) {
			open_device();
		}
		int cmd;
		switch (what) {
			case Accelerometer: cmd = ECS_IOCTL_APP_SET_AFLAG;  break;
			case MagneticField: cmd = ECS_IOCTL_APP_SET_MVFLAG; break;
			case Orientation:   cmd = ECS_IOCTL_APP_SET_MFLAG;  break;
		}
		short flags = newState;
		err = ioctl(dev_fd, cmd, &flags);
		err = err<0 ? -errno : 0;
		ALOGE_IF(err, "ECS_IOCTL_APP_SET_XXX failed (%s)", strerror(-err));
		if (!err) {
			mEnabled &= ~(1<<what);
			mEnabled |= (uint32_t(flags)<<what);
			update_delay();
		}
		if (!mEnabled) {
			close_device();
		}
	}
	return err;
}

int AkmSensor::setDelay(int32_t handle, int64_t ns)
{
#ifdef ECS_IOCTL_APP_SET_DELAY
	int what = -1;
	switch (handle) {
		case ID_A: what = Accelerometer; break;
		case ID_M: what = MagneticField; break;
		case ID_O: what = Orientation;   break;
	}

	if (uint32_t(what) >= numSensors)
		return -EINVAL;

	if (ns < 0)
		return -EINVAL;

	mDelays[what] = ns;
	return update_delay();
#else
	return -1;
#endif
}

int AkmSensor::update_delay()
{
	if (mEnabled) {
		uint64_t wanted = -1LLU;
		for (int i=0 ; i<numSensors ; i++) {
			if (mEnabled & (1<<i)) {
				uint64_t ns = mDelays[i];
				wanted = wanted < ns ? wanted : ns;
			}
		}
		short delay = int64_t(wanted) / 1000000;
		if (ioctl(dev_fd, ECS_IOCTL_APP_SET_DELAY, &delay)) {
			return -errno;
		}
	}
	return 0;
}

int AkmSensor::readEvents(sensors_event_t* data, int count)
{
	if (count < 1)
		return -EINVAL;

	ssize_t n = mInputReader.fill(data_fd);
	if (n < 0)
		return n;

	int numEventReceived = 0;
	input_event const* event;

	while (count && mInputReader.readEvent(&event)) {
		int type = event->type;
		if (type == EV_REL) {
			processEvent(event->code, event->value);
			mInputReader.next();
		} else if (type == EV_SYN) {
			int64_t time = timevalToNano(event->time);
			for (int j=0 ; count && mPendingMask && j<numSensors ; j++) {
				if (mPendingMask & (1<<j)) {
					mPendingMask &= ~(1<<j);
					mPendingEvents[j].timestamp = time;
					if (mEnabled & (1<<j)) {
						*data++ = mPendingEvents[j];
						count--;
						numEventReceived++;
					}
				}
			}
			if (!mPendingMask) {
				mInputReader.next();
			}
		} else {
			ALOGE("AkmSensor: unknown event (type=%d, code=%d)",
					type, event->code);
			mInputReader.next();
		}
	}

	return numEventReceived;
}

void AkmSensor::processEvent(int code, int value)
{
	switch (code) {
		case EVENT_TYPE_ACCEL_X:
			mPendingMask |= 1<<Accelerometer;
			mPendingEvents[Accelerometer].acceleration.x = value * CONVERT_A_X;
			break;
		case EVENT_TYPE_ACCEL_Y:
			mPendingMask |= 1<<Accelerometer;
			mPendingEvents[Accelerometer].acceleration.y = value * CONVERT_A_Y;
			break;
		case EVENT_TYPE_ACCEL_Z:
			mPendingMask |= 1<<Accelerometer;
			mPendingEvents[Accelerometer].acceleration.z = value * CONVERT_A_Z;
			break;

		case EVENT_TYPE_MAGV_X:
			mPendingMask |= 1<<MagneticField;
			mPendingEvents[MagneticField].magnetic.x = value * CONVERT_M_X;
			break;
		case EVENT_TYPE_MAGV_Y:
			mPendingMask |= 1<<MagneticField;
			mPendingEvents[MagneticField].magnetic.y = value * CONVERT_M_Y;
			break;
		case EVENT_TYPE_MAGV_Z:
			mPendingMask |= 1<<MagneticField;
			mPendingEvents[MagneticField].magnetic.z = value * CONVERT_M_Z;
			break;

		case EVENT_TYPE_YAW:
			mPendingMask |= 1<<Orientation;
			mPendingEvents[Orientation].orientation.azimuth = value * CONVERT_O_Y;
			break;
		case EVENT_TYPE_PITCH:
			mPendingMask |= 1<<Orientation;
			mPendingEvents[Orientation].orientation.pitch = value * CONVERT_O_P;
			break;
		case EVENT_TYPE_ROLL:
			mPendingMask |= 1<<Orientation;
			mPendingEvents[Orientation].orientation.roll = value * CONVERT_O_R;
			break;
		case EVENT_TYPE_ORIENT_STATUS:
			mPendingMask |= 1<<Orientation;
			mPendingEvents[Orientation].orientation.status =
					uint8_t(value & SENSOR_STATE_MASK);
			break;
	}
}