From: Chih-Wei Huang Date: Sun, 28 Dec 2014 08:47:17 +0000 (+0800) Subject: libsensors: make the implementation be compatible with new IIO ABI X-Git-Tag: android-x86-6.0-r1~2^2 X-Git-Url: http://git.osdn.net/view?p=android-x86%2Fhardware-intel-libsensors.git;a=commitdiff_plain;h=463d5aa217479c4643f88db987ccc152ec011d65;hp=8ee28422c817a615d075214d7c770df6f8cefbbb libsensors: make the implementation be compatible with new IIO ABI Also improve debug messages. --- diff --git a/HidSensor_Accel3D.cpp b/HidSensor_Accel3D.cpp index fae0b57..5b8d924 100644 --- a/HidSensor_Accel3D.cpp +++ b/HidSensor_Accel3D.cpp @@ -75,11 +75,11 @@ int AccelSensor::processEvent(unsigned char *raw_data, size_t raw_data_len){ (raw_data + 2), *(raw_data + 3), *(raw_data + 4), *(raw_data + 5)); sample = (struct accel_3d_sample*)raw_data; mPendingEvent.data[0] = mPendingEvent.acceleration.x = - CONVERT_A_G_VTF16E14_X(GetChannelBytesUsedSize(CHANNEL_X), GetExponentValue(), sample->accel_x); + CONVERT_A_G_VTF16E14_X(GetChannelBytesUsedSize(CHANNEL_X), GetScaleValue(), sample->accel_x); mPendingEvent.data[1] = mPendingEvent.acceleration.y = - CONVERT_A_G_VTF16E14_Y(GetChannelBytesUsedSize(CHANNEL_Y), GetExponentValue(), sample->accel_y); + CONVERT_A_G_VTF16E14_Y(GetChannelBytesUsedSize(CHANNEL_Y), GetScaleValue(), sample->accel_y); mPendingEvent.data[2] = mPendingEvent.acceleration.z = - CONVERT_A_G_VTF16E14_Z(GetChannelBytesUsedSize(CHANNEL_Z), GetExponentValue(), sample->accel_z); + CONVERT_A_G_VTF16E14_Z(GetChannelBytesUsedSize(CHANNEL_Z), GetScaleValue(), sample->accel_z); ALOGV("ACCEL 3D Sample %fm/s2 %fm/s2 %fm/s2\n", mPendingEvent.acceleration.x, mPendingEvent.acceleration.y, mPendingEvent.acceleration.z); diff --git a/HidSensor_Compass3D.cpp b/HidSensor_Compass3D.cpp index 7174d27..64b9ef3 100644 --- a/HidSensor_Compass3D.cpp +++ b/HidSensor_Compass3D.cpp @@ -68,12 +68,13 @@ int CompassSensor::processEvent(unsigned char *raw_data, size_t raw_data_len){ } sample = (struct compass_3d_sample*)raw_data; + float sc = GetScaleValue(); mPendingEvent.data[0] = mPendingEvent.magnetic.x = CONVERT_M_MG_VTF16E14_X - (GetChannelBytesUsedSize(CHANNEL_X), GetExponentValue(), sample->compass_x); + (GetChannelBytesUsedSize(CHANNEL_X), sc, sample->compass_x); mPendingEvent.data[1] = mPendingEvent.magnetic.y = CONVERT_M_MG_VTF16E14_Y - (GetChannelBytesUsedSize(CHANNEL_Y), GetExponentValue(), sample->compass_y); + (GetChannelBytesUsedSize(CHANNEL_Y), sc, sample->compass_y); mPendingEvent.data[2] = mPendingEvent.magnetic.z = CONVERT_M_MG_VTF16E14_Z - (GetChannelBytesUsedSize(CHANNEL_Z), GetExponentValue(), sample->compass_z); + (GetChannelBytesUsedSize(CHANNEL_Z), sc, sample->compass_z); ALOGV("COMPASS 3D Sample %fuT %fuT %fuT\n", mPendingEvent.magnetic.x, mPendingEvent.magnetic.y, mPendingEvent.magnetic.z); diff --git a/HidSensor_Gyro3D.cpp b/HidSensor_Gyro3D.cpp index 979b27d..d719ab7 100644 --- a/HidSensor_Gyro3D.cpp +++ b/HidSensor_Gyro3D.cpp @@ -69,36 +69,14 @@ int GyroSensor::processEvent(unsigned char *raw_data, size_t raw_data_len){ return - 1; } sample = (struct gyro_3d_sample*)raw_data; - switch (GetUnitValue()) { - case HID_USAGE_SENSOR_UNITS_NOT_SPECIFIED: - case HID_USAGE_SENSOR_UNITS_DEGREES_PER_SECOND: - mPendingEvent.data[0] = mPendingEvent.gyro.x = CONVERT_G_D_VTF16E14_X - (GetChannelBytesUsedSize(CHANNEL_X), GetExponentValue(), sample->gyro_x) - ; - mPendingEvent.data[1] = mPendingEvent.gyro.y = CONVERT_G_D_VTF16E14_Y - (GetChannelBytesUsedSize(CHANNEL_Y), GetExponentValue(), sample->gyro_y) - ; - mPendingEvent.data[2] = mPendingEvent.gyro.z = CONVERT_G_D_VTF16E14_Z - (GetChannelBytesUsedSize(CHANNEL_Z), GetExponentValue(), sample->gyro_z) - ; - break; - case HID_USAGE_SENSOR_UNITS_RADIANS_PER_SECOND: - mPendingEvent.data[0] = mPendingEvent.gyro.x = CONVERT_FROM_VTF16 - (GetChannelBytesUsedSize(CHANNEL_X), GetExponentValue(), sample->gyro_x) - ; - mPendingEvent.data[1] = mPendingEvent.gyro.y = CONVERT_FROM_VTF16 - (GetChannelBytesUsedSize(CHANNEL_Y), GetExponentValue(), sample->gyro_y) - ; - mPendingEvent.data[2] = mPendingEvent.gyro.z = CONVERT_FROM_VTF16 - (GetChannelBytesUsedSize(CHANNEL_Z), GetExponentValue(), sample->gyro_z) - ; - break; - default: - ALOGE("Gyro Unit is not supported"); - break; - } - - ALOGV("GYRO 3D Sample %fm/s2 %fm/s2 %fm/s2\n", mPendingEvent.gyro.x, + float sc = GetScaleValue(); + mPendingEvent.data[0] = mPendingEvent.gyro.x = CONVERT_FROM_VTF16 + (GetChannelBytesUsedSize(CHANNEL_X), sc, sample->gyro_x); + mPendingEvent.data[1] = mPendingEvent.gyro.y = CONVERT_FROM_VTF16 + (GetChannelBytesUsedSize(CHANNEL_Y), sc, sample->gyro_y); + mPendingEvent.data[2] = mPendingEvent.gyro.z = CONVERT_FROM_VTF16 + (GetChannelBytesUsedSize(CHANNEL_Z), sc, sample->gyro_z); + ALOGD("GYRO 3D Sample %fm/s2 %fm/s2 %fm/s2\n", mPendingEvent.gyro.x, mPendingEvent.gyro.y, mPendingEvent.gyro.z); ALOGV("<<%s", __func__); return 0; diff --git a/OrientationSensor.cpp b/OrientationSensor.cpp index 1696b87..5941bde 100644 --- a/OrientationSensor.cpp +++ b/OrientationSensor.cpp @@ -63,10 +63,10 @@ int OrientationSensor::processEvent(unsigned char *data, size_t len) float vals[3]; unsigned int *sample = (unsigned int*)data; - long ex = GetExponentValue(); + float sc = GetScaleValue(); for (int i=0; i<3; i++) { int sz = GetChannelBytesUsedSize(i); - vals[i] = convert_from_vtf_format(sz, ex, sample[i]); + vals[i] = convert_from_vtf_format(sz, sc, sample[i]); } // When held in the Android convention frame (X to the right, Y diff --git a/RotVecSensor.cpp b/RotVecSensor.cpp index 33919a7..6ba1dad 100644 --- a/RotVecSensor.cpp +++ b/RotVecSensor.cpp @@ -35,9 +35,9 @@ const struct sensor_t RotVecSensor::sSensorInfo_rotvec = { }; RotVecSensor::RotVecSensor() - : SensorIIODev("dev_rotation", // name - "in_rot_scale", // units sysfs node - "in_rot_offset", // exponent sysfs node + : SensorIIODev("magn_3d", // name + "in_magn_scale", // units sysfs node + "in_magn_offset",// exponent sysfs node "in_rot_", // channel_prefix 10) // retry count { @@ -58,11 +58,6 @@ int RotVecSensor::processEvent(unsigned char *data, size_t len) 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). @@ -70,10 +65,10 @@ int RotVecSensor::processEvent(unsigned char *data, size_t len) // copy out the raw data. unsigned int *sample = (unsigned int*)data; - long ex = GetExponentValue(); - for (int i=0; i<4; i++) { + float sc = GetScaleValue(); + for (int i=0; i < (len / sizeof(*sample)); i++) { int sz = GetChannelBytesUsedSize(i); - mPendingEvent.data[i] = convert_from_vtf_format(sz, ex, sample[i]); + mPendingEvent.data[i] = convert_from_vtf_format(sz, sc, sample[i]); } if (mSynthCompass) diff --git a/SensorIIODev.cpp b/SensorIIODev.cpp index 51d85e8..1d7f57b 100644 --- a/SensorIIODev.cpp +++ b/SensorIIODev.cpp @@ -27,17 +27,17 @@ static const std::string IIO_DIR = "/sys/bus/iio/devices"; static const int DEF_BUFFER_LEN = 2; static const int DEF_HYST_VALUE = 0; -SensorIIODev::SensorIIODev(const std::string& dev_name, const std::string& units, - const std::string& exponent, +SensorIIODev::SensorIIODev(const std::string& dev_name, const std::string& scale, + const std::string& offset, const std::string& channel_prefix, int retry_cnt) : SensorBase(""), initialized(false), - unit_expo_str(exponent), - unit_str(units), + scale_str(scale), + offset_str(offset), device_name(dev_name), channel_prefix_str(channel_prefix), - unit_expo_value(0), - units_value(0), + scale(0.0), + offset(0.0), retry_count(retry_cnt), raw_buffer(NULL), mRefCount(0), @@ -46,25 +46,6 @@ SensorIIODev::SensorIIODev(const std::string& dev_name, const std::string& units ALOGV("%s", __func__); } -SensorIIODev::SensorIIODev(const std::string& dev_name, const std::string& units, - const std::string& exponent, - const std::string& channel_prefix) - : SensorBase(""), - initialized(false), - unit_expo_str(exponent), - unit_str(units), - device_name(dev_name), - channel_prefix_str(channel_prefix), - unit_expo_value(0), - units_value(0), - retry_count(1), - raw_buffer(NULL), - mRefCount(0) -{ - - ALOGV("%s", __func__); -} - int SensorIIODev::discover() { int cnt; @@ -80,7 +61,7 @@ int SensorIIODev::discover() if(*sampmin) sample_delay_min_ms = strtol(sampmin, NULL, 10); - ALOGD(">>%s discover", __func__); + ALOGD(">>discover %s", device_name.c_str()); for (cnt = 0; cnt < retry_count; cnt++) { status = ParseIIODirectory(device_name); if (status >= 0){ @@ -88,12 +69,11 @@ int SensorIIODev::discover() initialized = true; filename << "/dev/iio:device" << device_number; mDevPath = filename.str(); - ALOGV("mDevPath %s", mDevPath.c_str()); + ALOGI("found %s at %s", device_name.c_str(), mDevPath.c_str()); ret = 0; break; - } - else{ - ALOGE("Sensor IIO Init failed, retry left:%d\n", retry_count-cnt); + } else { + ALOGE("%s not found, retry left: %d", device_name.c_str(), retry_count-cnt); mDevPath = ""; ret = -1; } @@ -145,7 +125,7 @@ int SensorIIODev::startStop(int enabled) int ret =0; double sensitivity; - ALOGD(">>%s enabled:%d", __func__, enabled); + ALOGD(">>%s enabled: %d", device_name.c_str(), enabled); if (enabled){ if ((ret = discover()) < 0) { @@ -163,9 +143,9 @@ int SensorIIODev::startStop(int enabled) EnableBuffer(1); EnableBuffer(0); - if (ReadHIDExponentValue(&unit_expo_value) < 0) + if (ReadHIDScaleValue(&scale) < 0) goto err_ret; - if (ReadHIDMeasurmentUnit(&units_value) < 0) + if (ReadHIDOffsetValue(&offset) < 0) goto err_ret; if (SetDataReadyTrigger(GetDeviceNumber(), true) < 0) goto err_ret; @@ -178,6 +158,7 @@ int SensorIIODev::startStop(int enabled) goto err_ret; if (AllocateRxBuffer() < 0) goto err_ret; + ALOGI("%s: scale=%f offset=%f", device_name.c_str(), scale, offset); } else{ if (SetDataReadyTrigger(GetDeviceNumber(), false) < 0) @@ -195,7 +176,7 @@ int SensorIIODev::startStop(int enabled) err_ret: close(); - ALOGE("SesnorIIO: Enable failed\n"); + ALOGE("SesnorIIO: %s Enable failed", device_name.c_str()); return -1; } @@ -205,7 +186,7 @@ int SensorIIODev::setDelay(int64_t delay_ns){ ALOGV(">>%s %ld", __func__, delay_ns); if (IsDeviceInitialized() == false){ - ALOGE("Device was not initialized \n"); + ALOGE("Device %s was not initialized", device_name.c_str()); return -EFAULT; } if (ms){ @@ -221,14 +202,14 @@ int SensorIIODev::setInitialState(){ return 0; } -long SensorIIODev::GetUnitValue() +float SensorIIODev::GetScaleValue() { - return units_value; + return scale; } -long SensorIIODev::GetExponentValue() +float SensorIIODev::GetOffsetValue() { - return unit_expo_value; + return offset; } bool SensorIIODev::IsDeviceInitialized(){ @@ -439,8 +420,6 @@ int SensorIIODev::BuildChannelList(){ ifs.close(); iio_channel.enabled = 1; - iio_channel.scale = 1.0; - iio_channel.offset = 0; iio_channel.name = files[i].substr(0, files[i].length() - 3); @@ -520,7 +499,7 @@ int SensorIIODev::ParseIIODirectory(const std::string& name){ device_number = dev_num = FindDeviceNumberFromName(name, "iio:device"); if (dev_num < 0){ - ALOGE("Failed to find device %s\n", (char*)name.c_str()); + ALOGE("Failed to find device %s", (char*)name.c_str()); return -EFAULT; } @@ -561,7 +540,7 @@ int SensorIIODev::ParseIIODirectory(const std::string& name){ } datum_size = GetSizeFromChannels(); - ALOGV("Datum Size %d", datum_size); + ALOGD("%s Datum Size %d", device_name.c_str(), datum_size); ALOGV("<<%s", __func__); return 0; } @@ -700,12 +679,12 @@ int SensorIIODev::readEvents(sensors_event_t *data, int count){ return numEventReceived; } -int SensorIIODev::ReadHIDMeasurmentUnit(long *unit){ +static int ReadHIDSysValue(int dev, const std::string& sys_str, float *value){ std::stringstream filename; int size; std::string long_str; - filename << IIO_DIR << "/" << "iio:device" << device_number << "/" << unit_str; + filename << IIO_DIR << "/" << "iio:device" << dev << "/" << sys_str; std::ifstream its(filename.str().c_str(), std::ifstream::in); if (!its.good()){ @@ -718,34 +697,17 @@ int SensorIIODev::ReadHIDMeasurmentUnit(long *unit){ its.close(); if (long_str.length() > 0){ - *unit = atol(long_str.c_str()); + *value = atof(long_str.c_str()); return 0; } - ALOGE("ReadHIDMeasurmentUnit failed"); + ALOGE("%s: read %s failed", __func__, filename.str().c_str()); return -EINVAL; } -int SensorIIODev::ReadHIDExponentValue(long *exponent){ - std::stringstream filename; - int size; - std::string long_str; - - filename << IIO_DIR << "/" << "iio:device" << device_number << "/" << unit_expo_str; - - std::ifstream its(filename.str().c_str(), std::ifstream::in); - if (!its.good()){ - ALOGE("%s: Can't Open :%s", - __func__, filename.str().c_str()); - its.close(); - return -EINVAL; - } - std::getline(its, long_str); - its.close(); +int SensorIIODev::ReadHIDScaleValue(float *scale){ + return ReadHIDSysValue(device_number, scale_str, scale); +} - if (long_str.length() > 0){ - *exponent = atol(long_str.c_str()); - return 0; - } - ALOGE("ReadHIDExponentValue failed"); - return -EINVAL; +int SensorIIODev::ReadHIDOffsetValue(float *offset){ + return ReadHIDSysValue(device_number, offset_str, offset); } diff --git a/SensorIIODev.h b/SensorIIODev.h index 59e6350..74b6c87 100644 --- a/SensorIIODev.h +++ b/SensorIIODev.h @@ -37,8 +37,6 @@ // Used by SensorIIO device containers struct SensorIIOChannel{ std::string name; - float scale; - float offset; unsigned index; unsigned real_bytes; unsigned bytes; @@ -68,12 +66,12 @@ private: int enable_buffer; int file_id; int datum_size; - std::string unit_expo_str; - std::string unit_str; + std::string scale_str; + std::string offset_str; std::string device_name; std::string channel_prefix_str; - long unit_expo_value; - long units_value; + float scale; + float offset; int retry_count; unsigned char *raw_buffer; int mRefCount; @@ -114,10 +112,10 @@ protected: int DeviceActivate(int dev_num, int state); double DeviceGetSensitivity(int dev_num); int DeviceSetSensitivity(int dev_num, double value); - long GetUnitValue(); - long GetExponentValue(); - int ReadHIDMeasurmentUnit(long *unit); - int ReadHIDExponentValue(long *exponent); + float GetScaleValue(); + float GetOffsetValue(); + int ReadHIDScaleValue(float *scale); + int ReadHIDOffsetValue(float *offset); int GetChannelBytesUsedSize(unsigned int channel_no); virtual int processEvent(unsigned char *raw_data, size_t raw_data_len) = 0; @@ -127,8 +125,7 @@ protected: virtual int setInitialState(); public: - 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); + SensorIIODev(const std::string& dev_name, const std::string& scale, const std::string& offset, const std::string& channel_prefix, int retry_cnt = 1); // start/stop stream without changing "enabled" status. For slaves. int startStop(int enabled); diff --git a/hsb/BoardConfig.cpp b/hsb/BoardConfig.cpp index 9b9352b..57b4bde 100644 --- a/hsb/BoardConfig.cpp +++ b/hsb/BoardConfig.cpp @@ -124,7 +124,7 @@ int BoardConfig::handleToDriver(int handle) return syncompass; case ID_O: return orientation; - default: + default: return -EINVAL; } return -EINVAL; diff --git a/hsb/SensorConfig.h b/hsb/SensorConfig.h index ebf185a..28c0163 100644 --- a/hsb/SensorConfig.h +++ b/hsb/SensorConfig.h @@ -125,28 +125,18 @@ inline unsigned int set_bit_range(int start, int end) return value; } -inline float convert_from_vtf_format(int size, int exponent, unsigned int value) +inline float convert_from_vtf_format(int size, float scale, unsigned int value) { int divider=1; int i; - float sample; - int mul = 1.0; + float mul = 1.0; value = value & set_bit_range(0, size*8); if (value & BIT(size*8-1)) { value = ((1LL << (size*8)) - value); mul = -1.0; } - sample = value * 1.0; - if (exponent < 0) { - exponent = abs(exponent); - for (i = 0; i < exponent; ++i) { - divider = divider*10; - } - return mul * sample/divider; - } else { - return mul * sample * pow(10.0, exponent); - } + return mul * scale * value; } // Platform sensor orientatation @@ -161,11 +151,11 @@ inline float convert_from_vtf_format(int size, int exponent, unsigned int value) // G to m/s2 #define CONVERT_FROM_VTF16(s,d,x) (convert_from_vtf_format(s,d,x)) #define CONVERT_A_G_VTF16E14_X(s,d,x) (DEF_ORIENT_ACCEL_X *\ - convert_from_vtf_format(s,d,x)*GRAVITY) + convert_from_vtf_format(s,d,x)) #define CONVERT_A_G_VTF16E14_Y(s,d,x) (DEF_ORIENT_ACCEL_Y *\ - convert_from_vtf_format(s,d,x)*GRAVITY) + convert_from_vtf_format(s,d,x)) #define CONVERT_A_G_VTF16E14_Z(s,d,x) (DEF_ORIENT_ACCEL_Z *\ - convert_from_vtf_format(s,d,x)*GRAVITY) + convert_from_vtf_format(s,d,x)) // Degree/sec to radian/sec #define CONVERT_G_D_VTF16E14_X(s,d,x) (DEF_ORIENT_GYRO_X *\ diff --git a/sensors.cpp b/sensors.cpp index 360bbec..7b92149 100644 --- a/sensors.cpp +++ b/sensors.cpp @@ -48,7 +48,7 @@ static int open_sensors(const struct hw_module_t* module, const char* id, struct hw_device_t** device); -static int sensors__get_sensors_list(struct sensors_module_t* module, +static int sensors__get_sensors_list(struct sensors_module_t*, struct sensor_t const** list) { *list = BoardConfig::sensorList(); @@ -68,7 +68,8 @@ struct sensors_module_t HAL_MODULE_INFO_SYM = { name: "Samsung Sensor module", author: "Samsung Electronic Company", methods: &sensors_module_methods, - dso: NULL, + dso: NULL, + reserved: {} }, get_sensors_list: sensors__get_sensors_list, };