$(common_src_path)/HidSensor_Gyro3D.cpp \
$(common_src_path)/HidSensor_Compass3D.cpp \
$(common_src_path)/HidSensor_ALS.cpp \
+ $(common_src_path)/RotVecSensor.cpp \
+ $(common_src_path)/SynthCompassSensor.cpp \
include external/stlport/libstlport.mk
LOCAL_C_INCLUDES += $(LOCAL_PATH) device/intel/common/libsensors
#include "HidSensor_Gyro3D.h"
#include "HidSensor_Compass3D.h"
#include "HidSensor_ALS.h"
+#include "RotVecSensor.h"
+#include "SynthCompassSensor.h"
static const struct sensor_t sSensorList[] = {
AccelSensor::sSensorInfo_accel3D,
GyroSensor::sSensorInfo_gyro3D,
- CompassSensor::sSensorInfo_compass3D,
+ SynthCompassSensor::sSensorInfo_compass,
ALSSensor::sSensorInfo_als,
+ RotVecSensor::sSensorInfo_rotvec,
};
const struct sensor_t* BoardConfig::sensorList()
sensors[gyro] = new GyroSensor();
sensors[compass] = new CompassSensor();
sensors[light] = new ALSSensor();
+ sensors[rotvec] = new RotVecSensor();
+ sensors[syncompass] = new SynthCompassSensor();
}
int BoardConfig::handleToDriver(int handle)
return gyro;
case ID_L:
return light;
+ case ID_R:
+ return rotvec;
+ case ID_SC:
+ return syncompass;
default:
return -EINVAL;
}
gyro,
compass,
light,
+ rotvec,
+ syncompass,
numSensorDrivers,
numFds,
};
--- /dev/null
+/*
+ * Copyright (C) 2013 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 <cutils/log.h>
+#include "common.h"
+#include "SensorConfig.h"
+#include "SynthCompassSensor.h"
+#include "RotVecSensor.h"
+
+const struct sensor_t RotVecSensor::sSensorInfo_rotvec = {
+ .name = "HID_SENSOR Rotation Vector",
+ .vendor = "Intel",
+ .version = 1,
+ .handle = SENSORS_ROT_VEC_HANDLE,
+ .type = SENSOR_TYPE_ROTATION_VECTOR,
+ .maxRange = 1.0,
+ .resolution = 1./512, // Advertise as 9 bits, no idea about reality
+ .power = 0.1f,
+ .minDelay = 0,
+ .reserved = {},
+};
+
+RotVecSensor::RotVecSensor()
+ : SensorIIODev("dev_rotation", // name
+ "in_rot_scale", // units sysfs node
+ "in_rot_offset", // exponent sysfs node
+ "in_rot_", // channel_prefix
+ 10) // retry count
+{
+ mPendingEvent.version = sizeof(sensors_event_t);
+ mPendingEvent.sensor = ID_R;
+ mPendingEvent.type = SENSOR_TYPE_ROTATION_VECTOR;
+ memset(mPendingEvent.data, 0, sizeof(mPendingEvent.data));
+
+ sample_delay_min_ms = 50; // 20Hz default
+
+ mSynthCompass = NULL;
+}
+
+int RotVecSensor::processEvent(unsigned char *data, size_t len)
+{
+ if (IsDeviceInitialized() == false) {
+ ALOGE("Device was not initialized \n");
+ return -1;
+ }
+
+ if (len < 4*sizeof(unsigned int)) {
+ ALOGE("Insufficient length \n");
+ return -1;
+ }
+
+ // The Intel Sensor Hub emits a normalized x/y/z/w quaternion
+ // which acts to rotate points in the device coordinate system
+ // (left/up/out) to the world coordinate system (north/east/down).
+ // This is pleasingly identical to the Android convention, so just
+ // copy out the raw data.
+
+ unsigned int *sample = (unsigned int*)data;
+ long ex = GetExponentValue();
+ for (int i=0; i<4; i++) {
+ int sz = GetChannelBytesUsedSize(i);
+ mPendingEvent.data[i] = convert_from_vtf_format(sz, ex, sample[i]);
+ }
+
+ if (mSynthCompass)
+ mSynthCompass->setQuaternion(mPendingEvent.data);
+
+ return 0;
+}
--- /dev/null
+/*
+ * Copyright (C) 2013 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.
+ */
+
+#ifndef HIDSENSOR_ROTVEC_H
+#define HIDSENSOR_ROTVEC_H
+
+#include "SensorIIODev.h"
+
+class SynthCompassSensor;
+
+class RotVecSensor : public SensorIIODev {
+
+public:
+ RotVecSensor();
+ virtual int processEvent(unsigned char *data, size_t len);
+ void setSynthCompass(SynthCompassSensor *c) { mSynthCompass = c; }
+
+ static const struct sensor_t sSensorInfo_rotvec;
+
+ private:
+ SynthCompassSensor *mSynthCompass;
+
+ friend class SynthCompassSensor;
+};
+#endif
* See the License for the specific language governing permissions and
* limitations under the License.
*/
+#include <algorithm>
#include <dirent.h>
#include <fcntl.h>
#include <cutils/log.h>
#include "SensorIIODev.h"
#include "Helpers.h"
+using namespace std;
+
static const std::string IIO_DIR = "/sys/bus/iio/devices";
static const int DEF_BUFFER_LEN = 2;
static const int DEF_HYST_VALUE = 0;
units_value(0),
retry_count(retry_cnt),
raw_buffer(NULL),
+ mRefCount(0),
sample_delay_min_ms(0)
{
ALOGV("%s", __func__);
unit_expo_value(0),
units_value(0),
retry_count(1),
- raw_buffer(NULL)
+ raw_buffer(NULL),
+ mRefCount(0)
{
ALOGV("%s", __func__);
int SensorIIODev::enable(int enabled)
{
+ // startStop() handles the internal sensor stream, which may be
+ // used by slaves. The mEnabled flag is what gates the output of
+ // this particular sensor.
+ int ret = startStop(enabled);
+ if (ret)
+ return ret;
+ mEnabled = enabled;
+ return 0;
+}
+
+int SensorIIODev::startStop(int enabled)
+{
+ int alive = mRefCount > 0;
+ mRefCount = max(0, enabled ? mRefCount+1 : mRefCount-1);
+ int alivenew = mRefCount > 0;
+ if (alive == alivenew)
+ return 0;
+
int ret =0;
ALOGD(">>%s enabled:%d", __func__, enabled);
- if (mEnabled == enabled) {
- return 0;
- }
if (enabled){
+ if ((ret = discover()) < 0) {
+ ALOGE("discover failed: %s\n", device_name.c_str());
+ return ret;
+ }
+ if ((ret = open()) < 0) {
+ ALOGE("open failed: %s\n", device_name.c_str());
+ return ret;
+ }
+
// QUIRK: some sensor hubs need to be turned on and off and on
// before sending any information. So we turn it on and off first
// before enabling again later in this function.
goto err_ret;
if (AllocateRxBuffer() < 0)
goto err_ret;
- mEnabled = enabled;
}
else{
- mEnabled = enabled;
if (SetDataReadyTrigger(GetDeviceNumber(), false) < 0)
goto err_ret;
if (EnableBuffer(0) < 0)
if (FreeRxBuffer() < 0)
goto err_ret;
mDevPath = "";
+ close();
}
return 0;
err_ret:
+ close();
ALOGE("SesnorIIO: Enable failed\n");
return -1;
}
if(read_size != datum_size) {
ALOGE("read() error (or short count) from IIO device: %d\n", errno);
} else {
+ // Always call processEvent(), but only emit output if
+ // enabled. We may have slave devices (e.g. we could be a
+ // RotVec serving a SynthCompass without clients of our own).
if (processEvent(raw_buffer, datum_size) >= 0) {
- mPendingEvent.timestamp = getTimestamp();
- *data = mPendingEvent;
- numEventReceived++;
+ if (mEnabled) {
+ mPendingEvent.timestamp = getTimestamp();
+ *data = mPendingEvent;
+ numEventReceived++;
+ }
}
}
return numEventReceived;
long units_value;
int retry_count;
unsigned char *raw_buffer;
+ int mRefCount;
int discover();
int EnableIIODevice();
SensorIIODev(const std::string& dev_name, const std::string& units, const std::string& exponent, const std::string& channel_prefix);
SensorIIODev(const std::string& dev_name, const std::string& units, const std::string& exponent, const std::string& channel_prefix, int retry_cnt);
+ // start/stop stream without changing "enabled" status. For slaves.
+ int startStop(int enabled);
};
#endif
--- /dev/null
+/*
+ * Copyright (C) 2013 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 <cutils/log.h>
+#include "common.h"
+#include "SensorConfig.h"
+#include "SynthCompassSensor.h"
+
+// This is a "2D" faked compass. The Intel sensor hub reports
+// uncalibrated/raw magnetometer data because of Microsoft
+// requirements for Windows sensor data. Android has no calibration
+// built into the frameworks and relies on the HAL to do it. Which is
+// sad, because that calibration is already being done internal to the
+// sensor hub for use by the integrated orientation/quaternion
+// sensors.
+//
+// Instead of doing the calibration again, we're just using that
+// output quaternion by returning a faked "magnetometer" reading
+// pointing directly at magnetic north. This isn't quite correct, as
+// real magnetic fields are 3D vectors with (typically) non-zero
+// elevations. The output from this sensor is always constrained to
+// lie along the ground plane.
+
+#define MAG_FIELD_UT 45 // Microtessla "field strength" reported
+
+const struct sensor_t SynthCompassSensor::sSensorInfo_compass = {
+ .name = "HID_SENSOR Leveled Compass",
+ .vendor = "Intel",
+ .version = 1,
+ .handle = SENSORS_SYNCOMPASS_HANDLE,
+ .type = SENSOR_TYPE_MAGNETIC_FIELD,
+ .maxRange = MAG_FIELD_UT * 2,
+ .resolution = MAG_FIELD_UT/256.0, // IIO reports 8 bit samples
+ .power = 0.1f,
+ .minDelay = 0,
+ .reserved = {},
+};
+
+SynthCompassSensor::SynthCompassSensor()
+{
+ mPendingEvent.version = sizeof(sensors_event_t);
+ mPendingEvent.sensor = ID_SC;
+ mPendingEvent.type = SENSOR_TYPE_MAGNETIC_FIELD;
+ memset(mPendingEvent.data, 0, sizeof(mPendingEvent.data));
+}
+
+// Converts a quaternion (need not be normalized) into a 3x3 rotation
+// matrix. Form and convetions are basically verbatim from Ken
+// Shoemake's quatut.pdf.
+static void quat2mat(const float q[4], float m[9])
+{
+ float x=q[0], y=q[1], z=q[2], w=q[3];
+ float norm = 1/sqrt(x*x+y*y+z*z+w*w);
+ x *= norm; y *= norm; z *= norm; w *= norm;
+ float result[] = { 1-2*(y*y+z*z), 2*(x*y-w*z), 2*(x*z+w*y),
+ 2*(x*y+w*z), 1-2*(x*x+z*z), 2*(y*z-w*x),
+ 2*(x*z-w*y), 2*(y*z+w*x), 1-2*(x*x+y*y) };
+ memcpy(m, result, 9*sizeof(float));
+}
+
+// Multiplies a 3-vector by a 3x3 matrix in row-major order
+static void matmul(const float m[9], const float v[3], float out[3])
+{
+ float x=v[0], y=v[1], z=v[2];
+ out[0] = x*m[0] + y*m[1] + z*m[2];
+ out[1] = x*m[3] + y*m[4] + z*m[5];
+ out[2] = x*m[6] + y*m[7] + z*m[8];
+}
+
+int SynthCompassSensor::enable(int en)
+{
+ if (!mRotVec)
+ return -1;
+ mRotVec->startStop(en);
+ mEnabled = en;
+ return 0;
+}
+
+int SynthCompassSensor::readEvents(sensors_event_t *data, int count)
+{
+ if (!mHasPendingEvent)
+ return 0;
+
+ mHasPendingEvent = false;
+ mPendingEvent.timestamp = getTimestamp();
+ *data = mPendingEvent;
+ return 1;
+}
+
+void SynthCompassSensor::setQuaternion(float qin[4])
+{
+ if (!mEnabled)
+ return;
+
+ // The quat defines a rotation from device coordinates to world
+ // coordinates, so invert it and generate a matrix that takes
+ // world coords to device space.
+ float q[4];
+ memcpy(q, qin, sizeof(float)*4);
+ q[0] *= -1; q[1] *= -1; q[2] *= -1;
+
+ float mat[9];
+ quat2mat(q, mat);
+
+ // "North" is alone the Y axis in world coordiantes, convert to
+ // device space
+ static const float v[] = { 0, MAG_FIELD_UT, 0 };
+ matmul(mat, v, mPendingEvent.data);
+ mPendingEvent.timestamp = getTimestamp();
+
+ mHasPendingEvent = true;
+}
--- /dev/null
+/*
+ * Copyright (C) 2013 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.
+ */
+
+#ifndef HIDSENSOR_SYNTHCOMPASS_H
+#define HIDSENSOR_SYNTHCOMPASS_H
+
+#include "SensorIIODev.h"
+#include "RotVecSensor.h"
+
+class RotVecSensor;
+
+class SynthCompassSensor : public SensorBase {
+
+public:
+ SynthCompassSensor();
+
+ void setRotVecSensor(RotVecSensor *s) { mRotVec = s; }
+
+ // From the RotationVector sensor
+ void setQuaternion(float q[4]);
+
+ // From SensorBase:
+ virtual int open() { return mRotVec != NULL; }
+ virtual void close() {}
+ virtual int enable(int en);
+ virtual int readEvents(sensors_event_t *data, int count);
+ virtual int setDelay(int64_t ns) { return mRotVec ? mRotVec->setDelay(ns) : -1; }
+
+ static const struct sensor_t sSensorInfo_compass;
+
+ private:
+ RotVecSensor *mRotVec;
+};
+#endif
#define ID_GY (5)
#define ID_PR (6)
#define ID_T (7)
-#define MAX_SENSOR_CNT (ID_T+1)
+#define ID_R (8) // rotation vector
+#define ID_SC (9) // "faked" level compass
+#define MAX_SENSOR_CNT (ID_SC+1)
#define SENSORS_ACCELERATION (1<<ID_A)
#define SENSORS_MAGNETIC_FIELD (1<<ID_M)
#define SENSORS_GYROSCOPE (1<<ID_GY)
#define SENSORS_PRESSURE (1<<ID_PR)
#define SENSORS_TEMPERATURE (1<<ID_T)
+#define SENSORS_ROT_VEC (1<<ID_R)
+#define SENSORS_SYNCOMPASS (1<<ID_SC)
#define SENSORS_ACCELERATION_HANDLE 0
#define SENSORS_MAGNETIC_FIELD_HANDLE 1
#define SENSORS_GYROSCOPE_HANDLE 5
#define SENSORS_PRESSURE_HANDLE 6
#define SENSORS_TEMPERATURE_HANDLE 7
+#define SENSORS_ROT_VEC_HANDLE 8
+#define SENSORS_SYNCOMPASS_HANDLE 9
/* The next one is used to set the timeout used for polling the file
* descriptors.
#include "SensorConfig.h"
#include "BoardConfig.h"
+#include "RotVecSensor.h"
+#include "SynthCompassSensor.h"
+
/*****************************************************************************/
/* The SENSORS Module */
}
}
+ // Mild hack: the synthetic compass is implemented as a slave of
+ // the rotation vector, so we have to tell them about each other
+ // explicitly in lieu of an architecture that lets them probe.
+ SensorBase *rv = mSensors[rotvec], *sc = mSensors[syncompass];
+ if (rv && sc) {
+ ((RotVecSensor*)rv)->setSynthCompass((SynthCompassSensor*)sc);
+ ((SynthCompassSensor*)sc)->setRotVecSensor((RotVecSensor*)rv);
+ }
+
int wakeFds[2];
int result = pipe(wakeFds);
ALOGE_IF(result<0, "error creating wake pipe (%s)", strerror(errno));
int err = 0;
if (enabled) {
- err = s->discover();
- if (err < 0) {
- return err;
- }
- if (s->open() > 0) {
- if ((err = s->enable(1)) < 0)
- s->close();
- }
+ if ((err = s->enable(1)) < 0)
+ s->close();
} else {
s->enable(0);
s->close();
// see if we have some leftover from the last poll()
for (int i=0 ; count && i<numSensorDrivers ; i++) {
SensorBase* const sensor(mSensors[i]);
+ mPollFds[i].fd = sensor->fd();
if (sensor && ((mPollFds[i].revents & POLLIN) || (sensor->hasPendingEvents()))) {
int nb = sensor->readEvents(data, count);
if (nb < count) {
}
}
+ // Some sensors ("slaves") may have generated events based on
+ // the input to the sensors above. So check again.
+ for (int i=0 ; count && i<numSensorDrivers ; i++) {
+ SensorBase *sensor = mSensors[i];
+ if (sensor->hasPendingEvents()) {
+ int nb = sensor->readEvents(data, count);
+ if (nb > 0) {
+ count -= nb;
+ nbEvents += nb;
+ data += nb;
+ }
+ }
+ }
+
if (count) {
// we still have some room, so try to see if we can get
// some events immediately or just wait if we don't have
OSDN Git Service
