From 7d903c6c944fe0d10a619d4c6c9b21e7f08705a8 Mon Sep 17 00:00:00 2001 From: Patrick Porlan Date: Fri, 11 Apr 2014 11:10:47 +0200 Subject: [PATCH] STPK-1429 Add Intel Sensor Hub conversion routines This will be used with several of the sensors we have on the XPS 12. This code is derived from the Intel Sensor Hub (libsensors) HAL module. Issue: STPK-1429 Change-Id: I44fafd68aa24b8ead615385ebee3569821235df4 Signed-off-by: Patrick Porlan --- transform.c | 191 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 191 insertions(+) diff --git a/transform.c b/transform.c index b390635..e21066a 100644 --- a/transform.c +++ b/transform.c @@ -11,6 +11,119 @@ #include "transform.h" +/*----------------------------------------------------------------------------*/ + +/* Macros related to Intel Sensor Hub */ + +#define GRAVITY 9.80665f + +/* 720 LSG = 1G */ +#define LSG (1024.0f) +#define NUMOFACCDATA (8.0f) + +/* conversion of acceleration data to SI units (m/s^2) */ +#define CONVERT_A (GRAVITY_EARTH / LSG / NUMOFACCDATA) +#define CONVERT_A_X(x) ((float(x)/1000) * (GRAVITY * -1.0)) +#define CONVERT_A_Y(x) ((float(x)/1000) * (GRAVITY * 1.0)) +#define CONVERT_A_Z(x) ((float(x)/1000) * (GRAVITY * 1.0)) + +/* conversion of magnetic data to uT units */ +#define CONVERT_M (1.0f/6.6f) +#define CONVERT_M_X (-CONVERT_M) +#define CONVERT_M_Y (-CONVERT_M) +#define CONVERT_M_Z (CONVERT_M) + +/* conversion of orientation data to degree units */ +#define CONVERT_O (1.0f/64.0f) +#define CONVERT_O_A (CONVERT_O) +#define CONVERT_O_P (CONVERT_O) +#define CONVERT_O_R (-CONVERT_O) + +/*conversion of gyro data to SI units (radian/sec) */ +#define CONVERT_GYRO ((2000.0f/32767.0f)*((float)M_PI / 180.0f)) +#define CONVERT_GYRO_X (-CONVERT_GYRO) +#define CONVERT_GYRO_Y (-CONVERT_GYRO) +#define CONVERT_GYRO_Z (CONVERT_GYRO) + +#define BIT(x) (1 << (x)) + +inline unsigned int set_bit_range(int start, int end) +{ + int i; + unsigned int value = 0; + + for (i = start; i < end; ++i) + value |= BIT(i); + return value; +} + +inline float convert_from_vtf_format(int size, int exponent, unsigned int value) +{ + int divider=1; + int i; + float sample; + int 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); + } +} + +// Platform sensor orientation +#define DEF_ORIENT_ACCEL_X -1 +#define DEF_ORIENT_ACCEL_Y -1 +#define DEF_ORIENT_ACCEL_Z -1 + +#define DEF_ORIENT_GYRO_X 1 +#define DEF_ORIENT_GYRO_Y 1 +#define DEF_ORIENT_GYRO_Z 1 + +// 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) +#define CONVERT_A_G_VTF16E14_Y(s,d,x) (DEF_ORIENT_ACCEL_Y *\ + convert_from_vtf_format(s,d,x)*GRAVITY) +#define CONVERT_A_G_VTF16E14_Z(s,d,x) (DEF_ORIENT_ACCEL_Z *\ + convert_from_vtf_format(s,d,x)*GRAVITY) + +// Degree/sec to radian/sec +#define CONVERT_G_D_VTF16E14_X(s,d,x) (DEF_ORIENT_GYRO_X *\ + convert_from_vtf_format(s,d,x) * \ + ((float)M_PI/180.0f)) +#define CONVERT_G_D_VTF16E14_Y(s,d,x) (DEF_ORIENT_GYRO_Y *\ + convert_from_vtf_format(s,d,x) * \ + ((float)M_PI/180.0f)) +#define CONVERT_G_D_VTF16E14_Z(s,d,x) (DEF_ORIENT_GYRO_Z *\ + convert_from_vtf_format(s,d,x) * \ + ((float)M_PI/180.0f)) + +// Milli gauss to micro tesla +#define CONVERT_M_MG_VTF16E14_X(s,d,x) (convert_from_vtf_format(s,d,x)/10) +#define CONVERT_M_MG_VTF16E14_Y(s,d,x) (convert_from_vtf_format(s,d,x)/10) +#define CONVERT_M_MG_VTF16E14_Z(s,d,x) (convert_from_vtf_format(s,d,x)/10) + +#define DATA_BYTES 2 +#define ACC_EXPONENT -2 +#define GYRO_EXPONENT -1 +#define MAGN_EXPONENT 0 +#define INC_EXPONENT -1 +#define ROT_EXPONENT -8 + +/*----------------------------------------------------------------------------*/ + static int64_t sample_as_int64(unsigned char* sample, struct datum_info_t* type) { uint16_t u16; @@ -104,11 +217,89 @@ static float transform_sample_default(int s, int c, unsigned char* sample_data) static void finalize_sample_ISH(int s, struct sensors_event_t* data) { + int i = sensor_info[s].catalog_index; + int sensor_type = sensor_catalog[i].type; + float pitch, roll, yaw; + + if (sensor_type == SENSOR_TYPE_ORIENTATION) { + + pitch = data->data[0]; + roll = data->data[1]; + yaw = data->data[2]; + + data->data[0] = 360.0 - yaw; + data->data[1] = -pitch; + data->data[2] = -roll; + } } static float transform_sample_ISH(int s, int c, unsigned char* sample_data) { + struct datum_info_t* sample_type = &sensor_info[s].channel[c].type_info; + int val = (int) sample_as_int64(sample_data, sample_type); + int i = sensor_info[s].catalog_index; + int sensor_type = sensor_catalog[i].type; + + switch (sensor_type) { + case SENSOR_TYPE_ACCELEROMETER: + switch (c) { + case 0: + return CONVERT_A_G_VTF16E14_X( + DATA_BYTES, ACC_EXPONENT, val); + + case 1: + return CONVERT_A_G_VTF16E14_Y( + DATA_BYTES, ACC_EXPONENT, val); + + case 2: + return CONVERT_A_G_VTF16E14_Z( + DATA_BYTES, ACC_EXPONENT, val); + } + break; + + + case SENSOR_TYPE_GYROSCOPE: + switch (c) { + case 0: + return CONVERT_G_D_VTF16E14_X( + DATA_BYTES, GYRO_EXPONENT, val); + + case 1: + return CONVERT_G_D_VTF16E14_Y( + DATA_BYTES, GYRO_EXPONENT, val); + + case 2: + return CONVERT_G_D_VTF16E14_Z( + DATA_BYTES, GYRO_EXPONENT, val); + } + break; + + case SENSOR_TYPE_MAGNETIC_FIELD: + switch (c) { + case 0: + return CONVERT_M_MG_VTF16E14_X( + DATA_BYTES, MAGN_EXPONENT, val); + + case 1: + return CONVERT_M_MG_VTF16E14_Y( + DATA_BYTES, MAGN_EXPONENT, val); + + case 2: + return CONVERT_M_MG_VTF16E14_Z( + DATA_BYTES, MAGN_EXPONENT, val); + } + break; + + case SENSOR_TYPE_ORIENTATION: + return convert_from_vtf_format(DATA_BYTES, INC_EXPONENT, + val); + + case SENSOR_TYPE_ROTATION_VECTOR: + return convert_from_vtf_format(DATA_BYTES, ROT_EXPONENT, + val); + } + return 0; } -- 2.11.0