├── MODULE_LICENSE_APACHE2 ├── README.md ├── linux ├── system │ └── graphics.h ├── cutils │ ├── native_handle.h │ └── properties.h ├── linux │ └── android_alarm.h ├── utils │ ├── Atomic.h │ └── Log.h └── log.c ├── filtering.h ├── transform.h ├── control.h ├── matrix-ops.h ├── enumeration.h ├── utils.h ├── Makefile ├── activity_event_utils.h ├── discovery.c ├── calibration.h ├── description.h ├── matrix-ops.c ├── Android.mk ├── gyro-calibration.c ├── utils.c ├── entry.c ├── accel-calibration.c ├── NOTICE ├── LICENSE ├── filtering.c ├── common.h ├── README ├── activity.c ├── sens.c ├── compass-calibration.c ├── transform.c └── description.c /MODULE_LICENSE_APACHE2: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | DISCONTINUATION OF PROJECT. 2 | 3 | This project will no longer be maintained by Intel. 4 | 5 | Intel has ceased development and contributions including, but not limited to, maintenance, bug fixes, new releases, or updates, to this project. 6 | 7 | Intel no longer accepts patches to this project. 8 | 9 | If you have an ongoing need to use this project, are interested in independently developing it, or would like to maintain patches for the open source software community, please create your own fork of this project. 10 | -------------------------------------------------------------------------------- /linux/system/graphics.h: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | -------------------------------------------------------------------------------- /linux/cutils/native_handle.h: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | -------------------------------------------------------------------------------- /linux/linux/android_alarm.h: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #ifndef _LINUX_ANDROID_ALARM_H 18 | #define _LINUX_ANDROID_ALARM_H 19 | 20 | #define ANDROID_ALARM_ELAPSED_REALTIME 0 21 | #define ANDROID_ALARM_GET_TIME(x) 0 22 | 23 | #endif /* _LINUX_ANDROID_ALARM_H */ 24 | -------------------------------------------------------------------------------- /linux/cutils/properties.h: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #ifndef _LINUX_APROP_H 18 | #define _LINUX_APROP_H 19 | 20 | #define PROP_VALUE_MAX 92 21 | #define PROP_NAME_MAX 32 22 | 23 | static inline int property_get(const char *name, const char *val, const char *x) 24 | { 25 | return 0; 26 | } 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /filtering.h: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #ifndef FILTERING_H 18 | #define FILTERING_H 19 | 20 | void setup_noise_filtering (int s); 21 | void release_noise_filtering_data (int s); 22 | void denoise (int s, sensors_event_t* event); 23 | void record_sample (int s, const sensors_event_t* data); 24 | 25 | #endif 26 | -------------------------------------------------------------------------------- /linux/utils/Atomic.h: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #ifndef _LINUX_ATOMIC_H 18 | #define _LINUX_ATOMIC_H 19 | 20 | int android_atomic_release_cas(int32_t oldvalue, int32_t newvalue, 21 | volatile int32_t* addr) 22 | { 23 | return !__sync_bool_compare_and_swap(addr, oldvalue, newvalue); 24 | } 25 | 26 | #define android_atomic_cmpxchg android_atomic_release_cas 27 | 28 | 29 | #endif /* _LINUX_ATOMIC_H */ 30 | -------------------------------------------------------------------------------- /transform.h: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #ifndef __TRANSFORM_H__ 18 | #define __TRANSFORM_H__ 19 | 20 | #include "common.h" 21 | 22 | #define CONVERT_GAUSS_TO_MICROTESLA(x) ((x) * 100) 23 | #define CONVERT_MICROTESLA_TO_GAUSS(x) ((x) / 100) 24 | 25 | void select_transform (int s); 26 | float acquire_immediate_float_value (int s, int c); 27 | uint64_t acquire_immediate_uint64_value (int s, int c); 28 | 29 | #endif 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /control.h: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #ifndef __CONTROL_H__ 18 | #define __CONTROL_H__ 19 | 20 | int sensor_activate (int handle, int enabled, int from_virtual); 21 | int sensor_set_delay (int handle, int64_t ns); 22 | int sensor_poll (sensors_event_t* data, int count); 23 | int sensor_flush (int handle); 24 | 25 | int allocate_control_data (void); 26 | void delete_control_data (void); 27 | 28 | void build_sensor_report_maps(int dev_num); 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /matrix-ops.h: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #ifndef __MATRIX_OPS__ 18 | #define __MATRIX_OPS__ 19 | 20 | void transpose (int rows, int cols, double m[rows][cols], double m_trans[cols][rows]); 21 | void multiply (int m, int n, int p, double m1[m][n], double m2[n][p], double result[m][p]); 22 | void invert (int s, double m[s][s], double m_inv[s][s]); 23 | void multiply_scalar_inplace(int rows, int cols, double m[rows][cols], double scalar); 24 | void assign (int rows, int cols, double m[rows][cols], double m1[rows][cols]); 25 | void substract (int rows, int cols, double m1[rows][cols], double m2[rows][cols], double res[rows][cols]); 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /enumeration.h: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #ifndef __ENUMERATION_H__ 18 | #define __ENUMERATION_H__ 19 | 20 | #include "common.h" 21 | 22 | int get_sensors_list (struct sensors_module_t* module, 23 | struct sensor_t const** list); 24 | 25 | void enumerate_sensors (void); 26 | void delete_enumeration_data (void); 27 | 28 | /* 29 | * These are fine-grained type definitions that are used internally, in the sensor array, but mapped to an Android sensor type in the processing pipeline. 30 | * The sensor array uses these, not the desc array. 31 | */ 32 | #define SENSOR_TYPE_INTERNAL_ILLUMINANCE -1 /* Global illuminance, in lux */ 33 | #define SENSOR_TYPE_INTERNAL_INTENSITY -2 /* Global intensity, in sensor specific units */ 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /linux/log.c: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | 20 | static int log; 21 | 22 | int __android_log_print(int prio, const char *tag, const char *fmt, ...) 23 | { 24 | va_list args; 25 | int loglevel; 26 | 27 | if (!log) { 28 | openlog("android", LOG_CONS, LOG_DAEMON); 29 | log = 1; 30 | } 31 | 32 | switch (prio) { 33 | default: 34 | case 0: 35 | case 1: 36 | loglevel = LOG_INFO; 37 | break; 38 | case 2: 39 | case 3: 40 | loglevel = LOG_DEBUG; 41 | break; 42 | case 4: 43 | loglevel = LOG_NOTICE; 44 | break; 45 | case 5: 46 | loglevel = LOG_WARNING; 47 | break; 48 | case 6: 49 | loglevel = LOG_ERR; 50 | break; 51 | case 7: 52 | loglevel = LOG_EMERG; 53 | break; 54 | } 55 | 56 | va_start(args, fmt); 57 | vsyslog(loglevel, fmt, args); 58 | va_end(args); 59 | 60 | return 0; 61 | } 62 | -------------------------------------------------------------------------------- /utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #ifndef __UTILS_H__ 18 | #define __UTILS_H__ 19 | 20 | #include "common.h" 21 | 22 | int sysfs_read_int (const char path[PATH_MAX], int *value); 23 | int sysfs_write_int (const char path[PATH_MAX], int value); 24 | 25 | int sysfs_read_str (const char path[PATH_MAX], char *buf, int buf_len); 26 | int sysfs_write_str (const char path[PATH_MAX], const char *buf); 27 | 28 | int sysfs_read_float (const char path[PATH_MAX], float *value); 29 | int sysfs_write_float(const char path[PATH_MAX], float value); 30 | 31 | int sysfs_read_uint64(const char path[PATH_MAX], uint64_t *value); 32 | 33 | void set_timestamp (struct timespec *out, int64_t target_ns); 34 | 35 | int64_t get_timestamp_boot (void); 36 | int64_t get_timestamp_thread (void); 37 | int64_t get_timestamp_realtime (void); 38 | int64_t get_timestamp_monotonic (void); 39 | 40 | #endif 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2015 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | USE_IIO_SENSOR_HAL := true 16 | USE_IIO_ACTIVITY_RECOGNITION_HAL := true 17 | include Android.mk 18 | 19 | LIBHARDWARE?=../../../../hardware/libhardware/ 20 | CFLAGS=-DLOG_TAG=\"sens\" -I$(LIBHARDWARE)include/ -I./linux -fPIC -Wall 21 | LDFLAGS=-ldl -lpthread -lm -lrt 22 | 23 | all: sensors.gmin.so sens activity_recognition.gmin.so activity 24 | 25 | linux_src = linux/log.o 26 | 27 | sens: sens.o $(linux_src) 28 | cc -o $@ $^ $(LDFLAGS) 29 | 30 | activity: activity.o $(linux_src) 31 | cc -o $@ $^ $(LDFLAGS) 32 | 33 | sensors.gmin.so: $(patsubst %.c,%.o,$(src_files) $(linux_src)) 34 | cc -o $@ $^ $(LDFLAGS) -shared 35 | 36 | activity_recognition.gmin.so: $(patsubst %.c,%.o,$(activity_src_files) $(linux_src)) 37 | cc -o $@ $^ $(LDFLAGS) -shared 38 | 39 | clean: 40 | -rm $(patsubst %.c,%.o,$(src_files) $(activity_src_files) $(linux_src) sens.c activity.c) sens sensors.gmin.so activity activity_recognition.gmin.so 2>/dev/null 41 | -------------------------------------------------------------------------------- /linux/utils/Log.h: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #ifndef _LINUX_ALOG_H 18 | #define _LINUX_ALOG_H 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | 29 | #ifndef ALOGE 30 | #define ALOGE(...) ((void)ALOG(LOG_ERROR, LOG_TAG, __VA_ARGS__)) 31 | #endif 32 | 33 | #ifndef ALOGI 34 | #define ALOGI(...) ((void)ALOG(LOG_INFO, LOG_TAG, __VA_ARGS__)) 35 | #endif 36 | 37 | #ifndef ALOGD 38 | #define ALOGD(...) ((void)ALOG(LOG_DEBUG, LOG_TAG, __VA_ARGS__)) 39 | #endif 40 | 41 | #ifndef ALOGW 42 | #define ALOGW(...) ((void)ALOG(LOG_WARN, LOG_TAG, __VA_ARGS__)) 43 | #endif 44 | 45 | #ifndef ALOGV 46 | #define ALOGV(...) ((void)ALOG(LOG_VERBOSE, LOG_TAG, __VA_ARGS__)) 47 | #endif 48 | 49 | #ifndef ALOG 50 | #define ALOG(priority, tag, ...) \ 51 | LOG_PRI(ANDROID_##priority, tag, __VA_ARGS__) 52 | #endif 53 | 54 | #ifndef LOG_PRI 55 | #define LOG_PRI(priority, tag, ...) \ 56 | android_printLog(priority, tag, __VA_ARGS__) 57 | #endif 58 | 59 | #define android_printLog(prio, tag, fmt...) \ 60 | __android_log_print(prio, tag, fmt) 61 | 62 | typedef enum android_LogPriority { 63 | ANDROID_LOG_UNKNOWN = 0, 64 | ANDROID_LOG_DEFAULT, /* only for SetMinPriority() */ 65 | ANDROID_LOG_VERBOSE, 66 | ANDROID_LOG_DEBUG, 67 | ANDROID_LOG_INFO, 68 | ANDROID_LOG_WARN, 69 | ANDROID_LOG_ERROR, 70 | ANDROID_LOG_FATAL, 71 | ANDROID_LOG_SILENT, /* only for SetMinPriority(); must be last */ 72 | } android_LogPriority; 73 | 74 | extern int __android_log_print(int prio, const char *tag, const char *fmt, ...); 75 | 76 | #endif 77 | 78 | -------------------------------------------------------------------------------- /activity_event_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #ifndef __ACTIVITY_EVENT_UTILS_H__ 18 | #define __ACTIVITY_EVENT_UTILS_H__ 19 | 20 | #include 21 | 22 | #include "utils.h" 23 | 24 | #define MAX_ACTIVITIES 6 25 | #define MAX_EVENTS_PER_ACTIVITY 2 26 | 27 | typedef unsigned bool; 28 | #define true 1 29 | #define false 0 30 | 31 | /* For each activity in activity_recognition.h we can monitor 2 events at most : 32 | * ENTER and EXIT */ 33 | struct activity_event_info { 34 | struct activity_event *event[MAX_EVENTS_PER_ACTIVITY]; 35 | int modifier; 36 | int sensor_catalog_index; 37 | int channel_index; 38 | int dev_num; 39 | int event_fd; 40 | int event_count; 41 | bool monitored[MAX_EVENTS_PER_ACTIVITY]; 42 | }; 43 | 44 | struct control_event_data { 45 | uint8_t enable; 46 | uint8_t activity; 47 | uint8_t event; 48 | }; 49 | 50 | /** 51 | * Creates a control event identifier: 52 | * [unused] EVENT ACTIVITY ENABLE 53 | * 63 ... 24 23 ... 16 15 ... 8 7 ... 0 54 | * @enable: Says if the pair needs to be enabled or disabled (0 or 1) 55 | * @activity: What activity are we working on - index in the list returned by 56 | * get_supported_activities_list() 57 | * @event: What type of event to asociate with the given activity (one of 58 | * the ACTIVITY_EVENT_* enum) 59 | */ 60 | static inline uint64_t get_control_code(uint8_t enable, uint8_t activity, uint8_t event) 61 | { 62 | return ((uint64_t)enable << 56) | 63 | ((uint64_t)activity << 48) | 64 | ((uint64_t)event << 40); 65 | } 66 | 67 | /** 68 | * Parses the given control identifier and retrieves the control data. 69 | * @control_code: the unified control data 70 | * @control_data: extracted data from the control code 71 | */ 72 | static inline void get_control_data(uint64_t control_code, 73 | struct control_event_data *control_data) 74 | { 75 | control_data->enable = (uint8_t)(control_code >> 56); 76 | control_data->activity = (uint8_t)(control_code >> 48 & 0xFF); 77 | control_data->event = (uint8_t)(control_code >> 40 & 0xFF); 78 | } 79 | 80 | #endif 81 | -------------------------------------------------------------------------------- /discovery.c: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include "common.h" 25 | 26 | void discover_sensors(int dev_num, char *sysfs_base_path, char map[catalog_size], 27 | void (*discover_sensor)(int, char*, char*)) 28 | { 29 | char sysfs_dir[PATH_MAX]; 30 | DIR *dir; 31 | struct dirent *d; 32 | unsigned int i; 33 | 34 | memset(map, 0, catalog_size); 35 | 36 | snprintf(sysfs_dir, sizeof(sysfs_dir), sysfs_base_path, dev_num); 37 | 38 | dir = opendir(sysfs_dir); 39 | if (!dir) { 40 | return; 41 | } 42 | 43 | /* Enumerate entries in this iio device's base folder */ 44 | 45 | while ((d = readdir(dir))) { 46 | if (!strncmp(d->d_name, ".", sizeof(".")) || !strncmp(d->d_name, "..", sizeof(".."))) 47 | continue; 48 | 49 | /* If the name matches a catalog entry, flag it */ 50 | for (i = 0; i < catalog_size; i++) { 51 | 52 | /* No discovery for virtual sensors */ 53 | if (sensor_catalog[i].is_virtual) 54 | continue; 55 | discover_sensor(i, d->d_name, map); 56 | } 57 | } 58 | 59 | closedir(dir); 60 | } 61 | 62 | void check_event_sensors(int i, char *sysfs_file, char map[catalog_size]) 63 | { 64 | int j, k; 65 | 66 | for (j = 0; j < sensor_catalog[i].num_channels; j++) 67 | for (k = 0; k < sensor_catalog[i].channel[j].num_events; k++) { 68 | if (!strcmp(sysfs_file, sensor_catalog[i].channel[j].event[k].ev_en_path)) { 69 | map[i] = 1; 70 | return; 71 | } 72 | } 73 | } 74 | 75 | void check_poll_sensors (int i, char *sysfs_file, char map[catalog_size]) 76 | { 77 | int c; 78 | 79 | for (c = 0; c < sensor_catalog[i].num_channels; c++) 80 | if (!strcmp(sysfs_file, sensor_catalog[i].channel[c].raw_path) || 81 | !strcmp(sysfs_file, sensor_catalog[i].channel[c].input_path)) { 82 | map[i] = 1; 83 | break; 84 | } 85 | } 86 | 87 | void check_trig_sensors (int i, char *sysfs_file, char map[catalog_size]) 88 | { 89 | 90 | if (!strcmp(sysfs_file, sensor_catalog[i].channel[0].en_path)) { 91 | map[i] = 1; 92 | return; 93 | } 94 | } 95 | -------------------------------------------------------------------------------- /calibration.h: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #ifndef __CALIBRATION_H__ 18 | #define __CALIBRATION_H__ 19 | 20 | #include "common.h" 21 | 22 | #define MAGN_DS_SIZE 32 23 | 24 | 25 | typedef struct { 26 | /* hard iron offsets */ 27 | double offset[3][1]; 28 | 29 | /* soft iron matrix */ 30 | double w_invert[3][3]; 31 | 32 | /* geomagnetic strength */ 33 | double bfield; 34 | 35 | /* selection data */ 36 | float sample[MAGN_DS_SIZE][3]; 37 | unsigned int sample_count; 38 | float average[3]; 39 | } 40 | compass_cal_t; 41 | 42 | 43 | typedef struct { 44 | float bias_x, bias_y, bias_z; 45 | int count; 46 | float min_x, min_y, min_z; 47 | float max_x, max_y, max_z; 48 | } 49 | gyro_cal_t; 50 | 51 | /* Accelerometer bias estimation and compensation */ 52 | 53 | #define BUCKET_COUNT 3 54 | #define BUCKET_TOLERANCE 2.5 /* Maximum monitoring distance from value of interest, in m/s² */ 55 | #define SLICES 100 /* We currently have 3 buckets per axis, and 100 slices per bucket ; then we distribute incoming samples among slices */ 56 | 57 | typedef struct 58 | { 59 | int version; 60 | int bucket_count; 61 | int slices; 62 | float bucket_tolerance; 63 | 64 | uint64_t bucket[3][BUCKET_COUNT][SLICES]; /* How many samples fell in each of the slices, per axis */ 65 | uint64_t bucket_usage[3][BUCKET_COUNT]; /* How many samples fell in each of the buckets (= sum of the slice counts) */ 66 | 67 | /* Estimated bias, according to accumulated data */ 68 | float accel_bias_x; 69 | float accel_bias_y; 70 | float accel_bias_z; 71 | 72 | uint64_t last_estimation_ts; 73 | } 74 | accel_cal_t; 75 | 76 | 77 | typedef double mat_input_t[MAGN_DS_SIZE][3]; 78 | 79 | 80 | void calibrate_compass (int s, sensors_event_t* event); 81 | void compass_read_data (int s); 82 | void compass_store_data (int s); 83 | 84 | void calibrate_gyro (int s, sensors_event_t* event); 85 | void gyro_cal_init (int s); 86 | void gyro_store_data (int s); 87 | 88 | void calibrate_accel (int s, sensors_event_t* event); 89 | void accel_cal_init (int s); 90 | void accel_cal_store (int s); 91 | 92 | #endif 93 | -------------------------------------------------------------------------------- /description.h: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #ifndef __DESCRIPTION_H__ 18 | #define __DESCRIPTION_H__ 19 | 20 | #include "common.h" 21 | 22 | #define QUIRK_ALREADY_DECODED 0x01 /* Sensor quirks have been read */ 23 | #define QUIRK_INITIAL_RATE 0x02 /* Force initial sensor sampling rate */ 24 | #define QUIRK_FIELD_ORDERING 0x04 /* Do field remapping for this sensor */ 25 | #define QUIRK_TERSE_DRIVER 0x08 /* Force duplicate events generation */ 26 | #define QUIRK_NOISY 0x10 /* High noise level on readings */ 27 | #define QUIRK_FORCE_CONTINUOUS 0x20 /* Force usage of continuous trigger */ 28 | #define QUIRK_BIASED 0x40 /* Biased sensor, requires compensation */ 29 | #define QUIRK_SPOTTY 0x80 /* Driver may lose events */ 30 | #define QUIRK_NO_EVENT_MODE 0x100 /* Disable event mode */ 31 | #define QUIRK_NO_TRIG_MODE 0x200 /* Disable trigger mode */ 32 | #define QUIRK_NO_POLL_MODE 0x400 /* Disable poll mode */ 33 | #define QUIRK_MOUNTING_MATRIX 0x800 /* Mounting information present */ 34 | #define QUIRK_HRTIMER 0x1000 /* We may use a hrtimer if there is no other trigger */ 35 | #define QUIRK_SECONDARY 0x2000 /* List after other sensors of the same type */ 36 | 37 | #ifdef __LP64__ 38 | typedef uint64_t flag_t; 39 | typedef int64_t max_delay_t; 40 | #else 41 | typedef uint32_t flag_t; 42 | typedef int32_t max_delay_t; 43 | #endif 44 | 45 | char* sensor_get_name (int s); 46 | char* sensor_get_vendor (int s); 47 | int sensor_get_version (int s); 48 | float sensor_get_max_range (int s); 49 | void sensor_update_max_range (int s); 50 | float sensor_get_resolution (int s); 51 | float sensor_get_power (int s); 52 | flag_t sensor_get_flags (int s); 53 | int32_t sensor_get_min_delay (int s); 54 | max_delay_t sensor_get_max_delay (int s); 55 | float sensor_get_illumincalib (int s); 56 | uint32_t sensor_get_quirks (int s); 57 | int sensor_get_prop (int s, const char* sel, int* val); 58 | int sensor_get_fl_prop (int s, const char* sel, float* val); 59 | int sensor_get_order (int s,unsigned char map[MAX_CHANNELS]); 60 | int sensor_get_mounting_matrix(int s,float mounting_matrix[9]); 61 | int sensor_get_available_frequencies(int s); 62 | int sensor_get_cal_steps (int s); 63 | char* sensor_get_string_type (int s); 64 | int sensor_get_st_prop (int s, const char* sel, char val[MAX_NAME_SIZE]); 65 | 66 | #endif 67 | -------------------------------------------------------------------------------- /matrix-ops.c: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include "matrix-ops.h" 18 | #include 19 | 20 | 21 | void transpose (int rows, int cols, double m[rows][cols], double m_trans[cols][rows]) 22 | { 23 | int i,j; 24 | 25 | for (i = 0; i < rows; i++) 26 | for (j = 0; j < cols; j++) 27 | m_trans[j][i] = m[i][j]; 28 | } 29 | 30 | 31 | void multiply (int m, int n, int p, double m1[m][n], double m2[n][p], double result[m][p]) 32 | { 33 | int i,j,k; 34 | 35 | for (i = 0; i < m; i++) 36 | for (k = 0; k < p; k++) { 37 | result [i][k] = 0; 38 | for (j = 0; j < n; j++) 39 | result [i][k] += m1[i][j] * m2 [j][k]; 40 | } 41 | } 42 | 43 | 44 | void invert (int s, double m[s][s], double m_inv[s][s]) 45 | { 46 | double t; 47 | int swap,i,j,k; 48 | double tmp[s][s]; 49 | 50 | for (i = 0; i < s; i++) 51 | for (j = 0; j < s; j++) 52 | m_inv[i][j] = 0; 53 | 54 | for (i = 0; i < s; i++) 55 | m_inv[i][i] = 1; 56 | 57 | assign(s,s,m,tmp); 58 | 59 | for (i = 0; i < s; i++) { 60 | swap = i; 61 | for (j = i+1; j < s; j++) { 62 | if (fabs(tmp[i][j]) > fabs(tmp[i][i])) 63 | swap = j; 64 | } 65 | 66 | if (swap != i) { 67 | /* swap rows */ 68 | for (k = 0; k < s; k++) { 69 | t = tmp[k][i]; 70 | tmp[k][i] = tmp[k][swap]; 71 | tmp[k][swap] = t; 72 | 73 | t = m_inv[k][i]; 74 | m_inv[k][i] = m_inv[k][swap]; 75 | m_inv[k][swap] = t; 76 | } 77 | } 78 | 79 | t = 1 / tmp[i][i]; 80 | 81 | for (k = 0 ; k < s ; k++) { 82 | tmp[k][i] *= t; 83 | m_inv[k][i] *= t; 84 | } 85 | 86 | for (j = 0 ; j < s ; j++) 87 | if (j != i) { 88 | t = tmp[i][j]; 89 | for (k = 0 ; k < s; k++) { 90 | tmp[k][j] -= tmp[k][i] * t; 91 | m_inv[k][j] -= m_inv[k][i] * t; 92 | } 93 | } 94 | } 95 | } 96 | 97 | 98 | void multiply_scalar_inplace(int rows, int cols, double m[rows][cols], double scalar) 99 | { 100 | int i,j; 101 | 102 | for (i = 0; i < rows; i++) 103 | for (j = 0; j < cols; j++) 104 | m[i][j] = m[i][j] * scalar; 105 | } 106 | 107 | 108 | void assign (int rows, int cols, double m[rows][cols], double m1[rows][cols]) 109 | { 110 | int i,j; 111 | 112 | for (i = 0; i < rows; i++) 113 | for (j = 0; j < cols; j++) 114 | m1[i][j] = m[i][j]; 115 | } 116 | 117 | void substract (int rows, int cols, double m1[rows][cols], double m2[rows][cols], double res[rows][cols]) 118 | { 119 | int i,j; 120 | 121 | for (i = 0; i < rows; i++) 122 | for (j = 0; j < cols; j++) 123 | res[i][j] = m1[i][j] - m2[i][j]; 124 | } 125 | -------------------------------------------------------------------------------- /Android.mk: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2015 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | # IIO sensors HAL module implementation, compiled as hw/iio-sensors-hal.so 16 | 17 | ifeq ($(USE_IIO_SENSOR_HAL),true) 18 | 19 | LOCAL_PATH := $(call my-dir) 20 | 21 | include $(CLEAR_VARS) 22 | 23 | src_path := . 24 | src_files := $(src_path)/entry.c \ 25 | $(src_path)/enumeration.c \ 26 | $(src_path)/control.c \ 27 | $(src_path)/description.c \ 28 | $(src_path)/utils.c \ 29 | $(src_path)/transform.c \ 30 | $(src_path)/compass-calibration.c \ 31 | $(src_path)/matrix-ops.c \ 32 | $(src_path)/gyro-calibration.c \ 33 | $(src_path)/filtering.c \ 34 | $(src_path)/discovery.c \ 35 | $(src_path)/accel-calibration.c \ 36 | 37 | LOCAL_C_INCLUDES += $(LOCAL_PATH) vendor/intel/hardware/iio-sensors 38 | ifeq ($(HAL_AUTODETECT),true) 39 | LOCAL_MODULE := iio-sensors-hal 40 | else 41 | LOCAL_MODULE := sensors.$(TARGET_BOARD_PLATFORM) 42 | endif 43 | LOCAL_MODULE_OWNER := intel 44 | LOCAL_MODULE_RELATIVE_PATH := hw 45 | LOCAL_MODULE_TAGS := optional 46 | LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\" -fvisibility=hidden 47 | ifeq ($(NO_IIO_EVENTS),true) 48 | LOCAL_CFLAGS += -D__NO_EVENTS__ 49 | endif 50 | LOCAL_LDFLAGS := -Wl,--gc-sections 51 | LOCAL_SHARED_LIBRARIES := liblog libcutils libdl 52 | LOCAL_PRELINK_MODULE := false 53 | LOCAL_SRC_FILES := $(src_files) 54 | LOCAL_PROPRIETARY_MODULE := true 55 | include $(BUILD_SHARED_LIBRARY) 56 | 57 | include $(CLEAR_VARS) 58 | LOCAL_C_INCLUDES += $(LOCAL_PATH) vendor/intel/hardware/iio-sensors 59 | LOCAL_MODULE := sens 60 | LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\" -fvisibility=hidden 61 | LOCAL_SHARED_LIBRARIES := liblog libcutils libdl 62 | LOCAL_SRC_FILES := sens.c 63 | LOCAL_MODULE_TAGS := eng 64 | LOCAL_MODULE_PATH := $(TARGET_OUT_VENDOR_EXECUTABLES) 65 | include $(BUILD_EXECUTABLE) 66 | 67 | endif 68 | 69 | 70 | # Activity HAL module implementation 71 | 72 | ifeq ($(USE_IIO_ACTIVITY_RECOGNITION_HAL),true) 73 | 74 | include $(CLEAR_VARS) 75 | 76 | src_path := . 77 | activity_src_files := $(src_path)/activity_event_entry.c \ 78 | $(src_path)/discovery.c \ 79 | $(src_path)/utils.c \ 80 | 81 | LOCAL_C_INCLUDES += $(LOCAL_PATH) vendor/intel/hardware/iio-sensors 82 | LOCAL_MODULE := activity_recognition.$(TARGET_BOARD_PLATFORM) 83 | LOCAL_MODULE_OWNER := intel 84 | LOCAL_MODULE_RELATIVE_PATH := hw 85 | LOCAL_MODULE_TAGS := optional 86 | LOCAL_CFLAGS := -DLOG_TAG=\"Activity\" -fvisibility=hidden 87 | LOCAL_LDFLAGS := -Wl,--gc-sections 88 | LOCAL_SHARED_LIBRARIES := liblog libcutils 89 | LOCAL_PRELINK_MODULE := false 90 | LOCAL_SRC_FILES := $(activity_src_files) 91 | LOCAL_PROPRIETARY_MODULE := true 92 | include $(BUILD_SHARED_LIBRARY) 93 | 94 | include $(CLEAR_VARS) 95 | LOCAL_C_INCLUDES += $(LOCAL_PATH) vendor/intel/hardware/iio-sensors 96 | LOCAL_MODULE := activity 97 | LOCAL_CFLAGS := -DLOG_TAG=\"Activity\" -fvisibility=hidden 98 | LOCAL_SHARED_LIBRARIES := liblog libcutils libdl 99 | LOCAL_SRC_FILES := activity.c 100 | LOCAL_MODULE_TAGS := eng 101 | LOCAL_MODULE_PATH := $(TARGET_OUT_VENDOR_EXECUTABLES) 102 | include $(BUILD_EXECUTABLE) 103 | 104 | endif 105 | -------------------------------------------------------------------------------- /gyro-calibration.c: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "common.h" 23 | #include "calibration.h" 24 | 25 | 26 | /* Gyro defines */ 27 | #define GYRO_MAX_ERR 0.05 28 | #define GYRO_DS_SIZE 100 29 | #define GYRO_CALIBRATION_PATH "/data/gyro.conf" 30 | #define GYRO_CAL_VERSION 1.0 31 | 32 | 33 | static void reset (gyro_cal_t* cal_data) 34 | { 35 | cal_data->count = 0; 36 | 37 | cal_data->bias_x = cal_data->bias_y = cal_data->bias_z = 0; 38 | 39 | cal_data->min_x = cal_data->min_y = cal_data->min_z = 1.0; 40 | cal_data->max_x = cal_data->max_y = cal_data->max_z =-1.0; 41 | } 42 | 43 | 44 | void gyro_cal_init (int s) 45 | { 46 | int ret; 47 | gyro_cal_t* cal_data = (gyro_cal_t*) sensor[s].cal_data; 48 | FILE* data_file; 49 | float version; 50 | 51 | sensor[s].cal_level = 0; 52 | 53 | if (cal_data == NULL) 54 | return; 55 | 56 | reset(cal_data); 57 | 58 | data_file = fopen (GYRO_CALIBRATION_PATH, "r"); 59 | if (data_file == NULL) 60 | return; 61 | 62 | ret = fscanf(data_file, "%f %f %f %f", &version, 63 | &cal_data->bias_x, &cal_data->bias_y, &cal_data->bias_z); 64 | 65 | if (ret != 4 || version != GYRO_CAL_VERSION) { 66 | reset(cal_data); 67 | ALOGE("Gyro calibration - init failed!\n"); 68 | } 69 | 70 | fclose(data_file); 71 | } 72 | 73 | 74 | void gyro_store_data (int s) 75 | { 76 | int ret; 77 | gyro_cal_t* cal_data = (gyro_cal_t*) sensor[s].cal_data; 78 | FILE* data_file; 79 | 80 | if (cal_data == NULL) 81 | return; 82 | 83 | data_file = fopen (GYRO_CALIBRATION_PATH, "w"); 84 | 85 | if (data_file == NULL) 86 | return; 87 | 88 | ret = fprintf(data_file, "%f %f %f %f", GYRO_CAL_VERSION, 89 | cal_data->bias_x, cal_data->bias_y, cal_data->bias_z); 90 | 91 | if (ret < 0) 92 | ALOGE ("Gyro calibration - store data failed!"); 93 | 94 | fclose(data_file); 95 | } 96 | 97 | 98 | static int gyro_collect (float x, float y, float z, gyro_cal_t* cal_data) 99 | { 100 | /* Analyze gyroscope data */ 101 | 102 | if (fabs(x) >= 1 || fabs(y) >= 1 || fabs(z) >= 1) { 103 | 104 | /* We're supposed to be standing still ; start over */ 105 | reset(cal_data); 106 | 107 | return 0; /* Uncalibrated */ 108 | } 109 | 110 | if (cal_data->count < GYRO_DS_SIZE) { 111 | 112 | if (x < cal_data->min_x) 113 | cal_data->min_x = x; 114 | 115 | if (y < cal_data->min_y) 116 | cal_data->min_y = y; 117 | 118 | if (z < cal_data->min_z) 119 | cal_data->min_z = z; 120 | 121 | if (x > cal_data->max_x) 122 | cal_data->max_x = x; 123 | 124 | if (y > cal_data->max_y) 125 | cal_data->max_y = y; 126 | 127 | if (z > cal_data->max_z) 128 | cal_data->max_z = z; 129 | 130 | if (fabs(cal_data->max_x - cal_data->min_x) <= GYRO_MAX_ERR && 131 | fabs(cal_data->max_y - cal_data->min_y) <= GYRO_MAX_ERR && 132 | fabs(cal_data->max_z - cal_data->min_z) <= GYRO_MAX_ERR) 133 | cal_data->count++; /* One more conformant sample */ 134 | else 135 | reset(cal_data); /* Out of spec sample ; start over */ 136 | 137 | return 0; /* Still uncalibrated */ 138 | } 139 | 140 | /* We got enough stable samples to estimate gyroscope bias */ 141 | cal_data->bias_x = (cal_data->max_x + cal_data->min_x) / 2; 142 | cal_data->bias_y = (cal_data->max_y + cal_data->min_y) / 2; 143 | cal_data->bias_z = (cal_data->max_z + cal_data->min_z) / 2; 144 | 145 | return 1; /* Calibrated! */ 146 | } 147 | 148 | 149 | void calibrate_gyro (int s, sensors_event_t* event) 150 | { 151 | gyro_cal_t* cal_data = (gyro_cal_t*) sensor[s].cal_data; 152 | 153 | if (cal_data == NULL) 154 | return; 155 | 156 | /* Attempt gyroscope calibration if we have not reached this state */ 157 | if (sensor[s].cal_level == 0) 158 | sensor[s].cal_level = gyro_collect(event->data[0], event->data[1], 159 | event->data[2], cal_data); 160 | 161 | 162 | event->data[0] = event->data[0] - cal_data->bias_x; 163 | event->data[1] = event->data[1] - cal_data->bias_y; 164 | event->data[2] = event->data[2] - cal_data->bias_z; 165 | 166 | if (sensor[s].cal_level) 167 | event->gyro.status = SENSOR_STATUS_ACCURACY_HIGH; 168 | } 169 | -------------------------------------------------------------------------------- /utils.c: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "common.h" 26 | #include "utils.h" 27 | 28 | #include 29 | 30 | /* Note: 31 | * 32 | * Some of these calls are going to fail, because not all sensors expose all 33 | * possible sysfs attributes. As an optimization we may want to cache which 34 | * ones are valid and immediately return in error for inexistent entries. 35 | */ 36 | int sysfs_read(const char path[PATH_MAX], void *buf, int buf_len) 37 | { 38 | int fd, len; 39 | 40 | if (!path[0] || !buf || buf_len < 1) 41 | return -1; 42 | 43 | fd = open(path, O_RDONLY); 44 | 45 | if (fd == -1) { 46 | ALOGV("Cannot open %s (%s)\n", path, strerror(errno)); 47 | return -1; 48 | } 49 | 50 | len = read(fd, buf, buf_len); 51 | 52 | close(fd); 53 | 54 | if (len == -1) 55 | ALOGW("Cannot read from %s (%s)\n", path, strerror(errno)); 56 | else 57 | ALOGV("Read %d bytes from %s\n", len, path); 58 | 59 | return len; 60 | } 61 | 62 | 63 | int sysfs_write(const char path[PATH_MAX], const void *buf, const int buf_len) 64 | { 65 | int fd, len; 66 | 67 | if (!path[0] || !buf || buf_len < 1) 68 | return -1; 69 | 70 | fd = open(path, O_WRONLY); 71 | 72 | if (fd == -1) { 73 | ALOGV("Cannot open %s (%s)\n", path, strerror(errno)); 74 | return -1; 75 | } 76 | 77 | len = write(fd, buf, buf_len); 78 | 79 | close(fd); 80 | 81 | if (len == -1) 82 | ALOGW("Cannot write to %s (%s)\n", path, strerror(errno)); 83 | else if (len != buf_len) 84 | ALOGW("Cannot write %d bytes to %s (%d)\n", buf_len, path, len); 85 | else 86 | ALOGV("Wrote %d bytes to %s\n", buf_len, path); 87 | 88 | return len; 89 | } 90 | 91 | 92 | static void str2int(const char* buf, void *v) 93 | { 94 | *(int*)v = atoi(buf); 95 | } 96 | 97 | 98 | static void str2float(const char* buf, void *v) 99 | { 100 | *(float*)v = strtof(buf, NULL); 101 | } 102 | 103 | 104 | static void str2uint64(const char* buf, void *v) 105 | { 106 | *(uint64_t*)v = atoll(buf); 107 | } 108 | 109 | 110 | int sysfs_read_num(const char path[PATH_MAX], void *v, 111 | void (*str2num)(const char* buf, void *v)) 112 | { 113 | char buf[20]; 114 | int len = sysfs_read_str(path, buf, sizeof(buf)); 115 | 116 | if (len <= 0) { 117 | ALOGW("Cannot read number from %s (%s)\n", path, 118 | strerror(errno)); 119 | return -1; 120 | } 121 | 122 | str2num(buf, v); 123 | return 0; 124 | } 125 | 126 | 127 | int sysfs_read_int(const char path[PATH_MAX], int *value) 128 | { 129 | return sysfs_read_num(path, value, str2int); 130 | } 131 | 132 | 133 | int sysfs_read_float(const char path[PATH_MAX], float *value) 134 | { 135 | return sysfs_read_num(path, value, str2float); 136 | } 137 | 138 | 139 | int sysfs_read_uint64(const char path[PATH_MAX], uint64_t *value) 140 | { 141 | return sysfs_read_num(path, value, str2uint64); 142 | } 143 | 144 | 145 | int sysfs_write_int(const char path[PATH_MAX], int value) 146 | { 147 | char buf[20]; 148 | int len = snprintf(buf, sizeof(buf), "%d", value); 149 | 150 | if (len <= 0) { 151 | ALOGE("Unexpected condition in sysfs_write_int\n"); 152 | return -1; 153 | } 154 | 155 | return sysfs_write(path, buf, len); 156 | } 157 | 158 | int sysfs_write_float(const char path[PATH_MAX], float value) 159 | { 160 | char buf[20]; 161 | int len = snprintf(buf, sizeof(buf), "%g", value); 162 | 163 | if (len <= 0) { 164 | ALOGE("Unexpected condition in sysfs_write_float\n"); 165 | return -1; 166 | } 167 | 168 | return sysfs_write(path, buf, len); 169 | } 170 | 171 | 172 | int sysfs_write_str(const char path[PATH_MAX], const char *str) 173 | { 174 | if (!str || !str[0]) 175 | return -1; 176 | 177 | return sysfs_write(path, str, strlen(str)); 178 | } 179 | 180 | 181 | int sysfs_read_str(const char path[PATH_MAX], char *buf, int buf_len) 182 | { 183 | int len; 184 | 185 | if (!buf || buf_len < 1) 186 | return -1; 187 | 188 | len = sysfs_read(path, buf, buf_len); 189 | 190 | if (len == -1) { 191 | ALOGW("Cannot read string from %s (%s)\n", path, 192 | strerror(errno)); 193 | return -1; 194 | } 195 | 196 | buf[len == 0 ? 0 : len - 1] = '\0'; 197 | 198 | ALOGV("Read %s from %s\n", buf, path); 199 | 200 | return len; 201 | } 202 | 203 | 204 | int64_t get_timestamp (clockid_t clock_id) 205 | { 206 | struct timespec ts = {0}; 207 | 208 | if (!clock_gettime(clock_id, &ts)) 209 | return 1000000000LL * ts.tv_sec + ts.tv_nsec; 210 | else /* in this case errno is set appropriately */ 211 | return -1; 212 | } 213 | 214 | int64_t get_timestamp_realtime (void) 215 | { 216 | return get_timestamp(CLOCK_REALTIME); 217 | } 218 | 219 | int64_t get_timestamp_boot (void) 220 | { 221 | return get_timestamp(CLOCK_BOOTTIME); 222 | } 223 | 224 | int64_t get_timestamp_thread (void) 225 | { 226 | return get_timestamp(CLOCK_THREAD_CPUTIME_ID); 227 | } 228 | 229 | int64_t get_timestamp_monotonic (void) 230 | { 231 | return get_timestamp(CLOCK_MONOTONIC); 232 | } 233 | 234 | void set_timestamp (struct timespec *out, int64_t target_ns) 235 | { 236 | out->tv_sec = target_ns / 1000000000LL; 237 | out->tv_nsec = target_ns % 1000000000LL; 238 | } 239 | 240 | -------------------------------------------------------------------------------- /entry.c: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | #include "enumeration.h" 20 | #include "control.h" 21 | #include "description.h" 22 | #include "utils.h" 23 | 24 | #include 25 | 26 | /* This is the IIO Sensors HAL module entry points file */ 27 | 28 | static int init_count; 29 | 30 | static int activate (__attribute__((unused)) struct sensors_poll_device_t* dev, 31 | int handle, int enabled) 32 | { 33 | int64_t entry_ts; 34 | int ret; 35 | int elapsed_ms; 36 | 37 | if (init_count == 0 || handle < 0 || handle >= sensor_count) 38 | return -EINVAL; 39 | 40 | entry_ts = get_timestamp_thread(); 41 | 42 | /* 43 | * The Intel sensor hub seems to have trouble enabling sensors before 44 | * a sampling rate has been configured, and setting the sampling rate 45 | * after it's been enabled does not seem to revive affected sensors. 46 | * The issue does not show up with an up to date ISH firmware but as the 47 | * updater is a Windows only tool and is not widely available, implement 48 | * a workaround for this behavior. We set the initial sampling rate to 49 | * 10 events per second when the sensor is enabled for the first time. 50 | */ 51 | 52 | if (enabled && sensor_get_quirks(handle) & QUIRK_INITIAL_RATE) { 53 | ALOGI("Forcing initial sampling rate\n"); 54 | sensor_activate(handle, 1, 0); 55 | sensor_set_delay(handle, 100000000); /* Start with 100 ms */ 56 | sensor_activate(handle, 0, 0); 57 | 58 | /* Clear flag for this sensor as do this only once */ 59 | sensor[handle].quirks ^= QUIRK_INITIAL_RATE; 60 | } 61 | 62 | ret = sensor_activate(handle, enabled, 0); 63 | 64 | elapsed_ms = (int) ((get_timestamp_thread() - entry_ts) / 1000000); 65 | 66 | if (elapsed_ms) { 67 | if (enabled) 68 | ALOGI("Activation of sensor %s took %d ms\n", sensor[handle].friendly_name, elapsed_ms); 69 | else 70 | ALOGI("Deactivation of sensor %s took %d ms\n", sensor[handle].friendly_name, elapsed_ms); 71 | } 72 | 73 | return ret; 74 | } 75 | 76 | 77 | static int set_delay (__attribute__((unused)) struct sensors_poll_device_t* dev, 78 | int handle, int64_t ns) 79 | { 80 | int i; 81 | 82 | if (init_count == 0 || handle < 0 || handle >= sensor_count) 83 | return -EINVAL; 84 | 85 | /* 86 | * If this sensor relies on other sensors, try to propagate the 87 | * requested sampling rate to the base sensors. 88 | */ 89 | 90 | for (i=0; i 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "common.h" 23 | #include "calibration.h" 24 | #include "utils.h" 25 | 26 | /* 27 | * This implements a crude way of estimating the accelerometer manufacturing mounting bias. We monitor x y and z distribution around a few strategic spots that 28 | * should represent most hits when the device is stable (on earth!). We then try to derive an estimation of the accelerometer bias for each of these axes from 29 | * that data, continuously. This is a very rough method that should only be used as a last resort for now. 30 | */ 31 | 32 | static float bucket_center[BUCKET_COUNT] = { -9.8, 0, 9.8 }; /* The spots we are most interested in */ 33 | 34 | #define ACCEL_CALIB_DATA_VERSION 1 /* Update this whenever the stored data structure changes */ 35 | 36 | #define ACCEL_CALIBRATION_PATH "/data/accel.conf" /* Location of saved calibration data */ 37 | 38 | #define REFRESH_INTERVAL 1000*1000*1000 /* Recompute bias estimation every second */ 39 | 40 | 41 | static void ascribe_sample (accel_cal_t* cal_data, int channel, float value) 42 | { 43 | /* Check if this falls within one of our ranges of interest */ 44 | float range_min; 45 | float range_max; 46 | int i; 47 | int slice; 48 | 49 | for (i=0; i= range_min && value <= range_max) { 54 | /* Find suitable bucket */ 55 | slice = (int) ((value-range_min) / (range_max-range_min) * (SLICES-1)); 56 | 57 | /* Increment counters */ 58 | cal_data->bucket[channel][i][slice]++; 59 | cal_data->bucket_usage[channel][i]++; 60 | return; 61 | } 62 | } 63 | } 64 | 65 | 66 | static float estimate_bias (accel_cal_t* cal_data, int channel) 67 | { 68 | /* 69 | * The long term distribution within the bucket, for each of the buckets, should be centered (samples evenly distributed). 70 | * Try to determine the position in the bucket that separates it in two portions holding as many samples, then compute an estimated bias for that axis 71 | * (channel) based on that data. 72 | */ 73 | 74 | int i; 75 | uint64_t half_of_the_samples; 76 | uint64_t count; 77 | float median; 78 | float estimated_bucket_bias[BUCKET_COUNT] = {0}; 79 | uint64_t bias_weight[BUCKET_COUNT]; 80 | uint64_t total_weight; 81 | float range_min; 82 | float range_max; 83 | float estimated_bias; 84 | int slice; 85 | 86 | for (i=0; ibucket_usage[channel][i] / 2; 88 | count = 0; 89 | 90 | for (slice = 0; slice < SLICES; slice++) { 91 | count += cal_data->bucket[channel][i][slice]; 92 | 93 | if (count >= half_of_the_samples) { 94 | range_min = bucket_center[i] - BUCKET_TOLERANCE; 95 | range_max = bucket_center[i] + BUCKET_TOLERANCE; 96 | 97 | median = range_min + ((float) slice) / (SLICES-1) * (range_max-range_min); 98 | 99 | estimated_bucket_bias[i] = median - bucket_center[i]; 100 | 101 | bias_weight[i] = count; 102 | break; 103 | } 104 | } 105 | } 106 | 107 | /* Weight each of the estimated bucket bias values based on the number of samples collected */ 108 | 109 | total_weight = 0; 110 | 111 | for (i=0; idata[0]; 137 | y = event->data[1]; 138 | z = event->data[2]; 139 | 140 | /* Analyze sample */ 141 | ascribe_sample(cal_data, 0, x); 142 | ascribe_sample(cal_data, 1, y); 143 | ascribe_sample(cal_data, 2, z); 144 | 145 | current_ts = get_timestamp_boot(); 146 | 147 | /* Estimate bias using accumulated data, from time to time*/ 148 | if (current_ts >= cal_data->last_estimation_ts + REFRESH_INTERVAL) { 149 | cal_data->last_estimation_ts = current_ts; 150 | 151 | cal_data->accel_bias_x = estimate_bias(cal_data, 0); 152 | cal_data->accel_bias_y = estimate_bias(cal_data, 1); 153 | cal_data->accel_bias_z = estimate_bias(cal_data, 2); 154 | } 155 | 156 | ALOGV("Compensating for estimated accelerometer bias: x=%g, y=%g, z=%g\n", cal_data->accel_bias_x, cal_data->accel_bias_y, cal_data->accel_bias_z); 157 | 158 | /* Apply compensation */ 159 | event->data[0] = x - cal_data->accel_bias_x; 160 | event->data[1] = y - cal_data->accel_bias_y; 161 | event->data[2] = z - cal_data->accel_bias_z; 162 | } 163 | 164 | 165 | void accel_cal_init (int s) 166 | { 167 | int fd; 168 | int n; 169 | 170 | accel_cal_t* cal_data = (accel_cal_t*) sensor[s].cal_data; 171 | 172 | if (cal_data == NULL) 173 | return; 174 | 175 | if (cal_data->last_estimation_ts) 176 | return; /* No need to overwrite perfectly good data at reenable time */ 177 | 178 | fd = open(ACCEL_CALIBRATION_PATH, O_RDONLY); 179 | 180 | if (fd != -1) { 181 | n = read(fd, cal_data, sizeof(accel_cal_t)); 182 | 183 | close(fd); 184 | 185 | if (n == sizeof(accel_cal_t) && 186 | cal_data->version == ((ACCEL_CALIB_DATA_VERSION << 16) + sizeof(accel_cal_t)) && 187 | cal_data->bucket_count == BUCKET_COUNT && 188 | cal_data->slices == SLICES && 189 | cal_data->bucket_tolerance == BUCKET_TOLERANCE) { 190 | cal_data->last_estimation_ts = 0; 191 | return; /* We successfully loaded previously saved accelerometer calibration data */ 192 | } 193 | } 194 | 195 | /* Fall back to initial values */ 196 | memset(cal_data, 0, sizeof(accel_cal_t)); 197 | 198 | /* Store the parameters that are used with that data set, so we can check them against future version of the code to prevent inadvertent reuse */ 199 | cal_data->version = (ACCEL_CALIB_DATA_VERSION << 16) + sizeof(accel_cal_t); 200 | cal_data->bucket_count = BUCKET_COUNT; 201 | cal_data->slices = SLICES; 202 | cal_data->bucket_tolerance = BUCKET_TOLERANCE; 203 | } 204 | 205 | 206 | void accel_cal_store (int s) 207 | { 208 | int fd; 209 | accel_cal_t* cal_data = (accel_cal_t*) sensor[s].cal_data; 210 | 211 | if (cal_data == NULL) 212 | return; 213 | 214 | fd = open(ACCEL_CALIBRATION_PATH, O_WRONLY | O_TRUNC | O_CREAT, S_IRUSR); 215 | 216 | if (fd != -1) { 217 | write(fd, cal_data, sizeof(accel_cal_t)); 218 | close(fd); 219 | } 220 | } 221 | -------------------------------------------------------------------------------- /NOTICE: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015 Intel Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | Apache License 17 | Version 2.0, January 2004 18 | http://www.apache.org/licenses/ 19 | 20 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 21 | 22 | 1. Definitions. 23 | 24 | "License" shall mean the terms and conditions for use, reproduction, 25 | and distribution as defined by Sections 1 through 9 of this document. 26 | 27 | "Licensor" shall mean the copyright owner or entity authorized by 28 | the copyright owner that is granting the License. 29 | 30 | "Legal Entity" shall mean the union of the acting entity and all 31 | other entities that control, are controlled by, or are under common 32 | control with that entity. For the purposes of this definition, 33 | "control" means (i) the power, direct or indirect, to cause the 34 | direction or management of such entity, whether by contract or 35 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 36 | outstanding shares, or (iii) beneficial ownership of such entity. 37 | 38 | "You" (or "Your") shall mean an individual or Legal Entity 39 | exercising permissions granted by this License. 40 | 41 | "Source" form shall mean the preferred form for making modifications, 42 | including but not limited to software source code, documentation 43 | source, and configuration files. 44 | 45 | "Object" form shall mean any form resulting from mechanical 46 | transformation or translation of a Source form, including but 47 | not limited to compiled object code, generated documentation, 48 | and conversions to other media types. 49 | 50 | "Work" shall mean the work of authorship, whether in Source or 51 | Object form, made available under the License, as indicated by a 52 | copyright notice that is included in or attached to the work 53 | (an example is provided in the Appendix below). 54 | 55 | "Derivative Works" shall mean any work, whether in Source or Object 56 | form, that is based on (or derived from) the Work and for which the 57 | editorial revisions, annotations, elaborations, or other modifications 58 | represent, as a whole, an original work of authorship. For the purposes 59 | of this License, Derivative Works shall not include works that remain 60 | separable from, or merely link (or bind by name) to the interfaces of, 61 | the Work and Derivative Works thereof. 62 | 63 | "Contribution" shall mean any work of authorship, including 64 | the original version of the Work and any modifications or additions 65 | to that Work or Derivative Works thereof, that is intentionally 66 | submitted to Licensor for inclusion in the Work by the copyright owner 67 | or by an individual or Legal Entity authorized to submit on behalf of 68 | the copyright owner. For the purposes of this definition, "submitted" 69 | means any form of electronic, verbal, or written communication sent 70 | to the Licensor or its representatives, including but not limited to 71 | communication on electronic mailing lists, source code control systems, 72 | and issue tracking systems that are managed by, or on behalf of, the 73 | Licensor for the purpose of discussing and improving the Work, but 74 | excluding communication that is conspicuously marked or otherwise 75 | designated in writing by the copyright owner as "Not a Contribution." 76 | 77 | "Contributor" shall mean Licensor and any individual or Legal Entity 78 | on behalf of whom a Contribution has been received by Licensor and 79 | subsequently incorporated within the Work. 80 | 81 | 2. Grant of Copyright License. Subject to the terms and conditions of 82 | this License, each Contributor hereby grants to You a perpetual, 83 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 84 | copyright license to reproduce, prepare Derivative Works of, 85 | publicly display, publicly perform, sublicense, and distribute the 86 | Work and such Derivative Works in Source or Object form. 87 | 88 | 3. Grant of Patent License. Subject to the terms and conditions of 89 | this License, each Contributor hereby grants to You a perpetual, 90 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 91 | (except as stated in this section) patent license to make, have made, 92 | use, offer to sell, sell, import, and otherwise transfer the Work, 93 | where such license applies only to those patent claims licensable 94 | by such Contributor that are necessarily infringed by their 95 | Contribution(s) alone or by combination of their Contribution(s) 96 | with the Work to which such Contribution(s) was submitted. If You 97 | institute patent litigation against any entity (including a 98 | cross-claim or counterclaim in a lawsuit) alleging that the Work 99 | or a Contribution incorporated within the Work constitutes direct 100 | or contributory patent infringement, then any patent licenses 101 | granted to You under this License for that Work shall terminate 102 | as of the date such litigation is filed. 103 | 104 | 4. Redistribution. You may reproduce and distribute copies of the 105 | Work or Derivative Works thereof in any medium, with or without 106 | modifications, and in Source or Object form, provided that You 107 | meet the following conditions: 108 | 109 | (a) You must give any other recipients of the Work or 110 | Derivative Works a copy of this License; and 111 | 112 | (b) You must cause any modified files to carry prominent notices 113 | stating that You changed the files; and 114 | 115 | (c) You must retain, in the Source form of any Derivative Works 116 | that You distribute, all copyright, patent, trademark, and 117 | attribution notices from the Source form of the Work, 118 | excluding those notices that do not pertain to any part of 119 | the Derivative Works; and 120 | 121 | (d) If the Work includes a "NOTICE" text file as part of its 122 | distribution, then any Derivative Works that You distribute must 123 | include a readable copy of the attribution notices contained 124 | within such NOTICE file, excluding those notices that do not 125 | pertain to any part of the Derivative Works, in at least one 126 | of the following places: within a NOTICE text file distributed 127 | as part of the Derivative Works; within the Source form or 128 | documentation, if provided along with the Derivative Works; or, 129 | within a display generated by the Derivative Works, if and 130 | wherever such third-party notices normally appear. The contents 131 | of the NOTICE file are for informational purposes only and 132 | do not modify the License. You may add Your own attribution 133 | notices within Derivative Works that You distribute, alongside 134 | or as an addendum to the NOTICE text from the Work, provided 135 | that such additional attribution notices cannot be construed 136 | as modifying the License. 137 | 138 | You may add Your own copyright statement to Your modifications and 139 | may provide additional or different license terms and conditions 140 | for use, reproduction, or distribution of Your modifications, or 141 | for any such Derivative Works as a whole, provided Your use, 142 | reproduction, and distribution of the Work otherwise complies with 143 | the conditions stated in this License. 144 | 145 | 5. Submission of Contributions. Unless You explicitly state otherwise, 146 | any Contribution intentionally submitted for inclusion in the Work 147 | by You to the Licensor shall be under the terms and conditions of 148 | this License, without any additional terms or conditions. 149 | Notwithstanding the above, nothing herein shall supersede or modify 150 | the terms of any separate license agreement you may have executed 151 | with Licensor regarding such Contributions. 152 | 153 | 6. Trademarks. This License does not grant permission to use the trade 154 | names, trademarks, service marks, or product names of the Licensor, 155 | except as required for reasonable and customary use in describing the 156 | origin of the Work and reproducing the content of the NOTICE file. 157 | 158 | 7. Disclaimer of Warranty. Unless required by applicable law or 159 | agreed to in writing, Licensor provides the Work (and each 160 | Contributor provides its Contributions) on an "AS IS" BASIS, 161 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 162 | implied, including, without limitation, any warranties or conditions 163 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 164 | PARTICULAR PURPOSE. You are solely responsible for determining the 165 | appropriateness of using or redistributing the Work and assume any 166 | risks associated with Your exercise of permissions under this License. 167 | 168 | 8. Limitation of Liability. In no event and under no legal theory, 169 | whether in tort (including negligence), contract, or otherwise, 170 | unless required by applicable law (such as deliberate and grossly 171 | negligent acts) or agreed to in writing, shall any Contributor be 172 | liable to You for damages, including any direct, indirect, special, 173 | incidental, or consequential damages of any character arising as a 174 | result of this License or out of the use or inability to use the 175 | Work (including but not limited to damages for loss of goodwill, 176 | work stoppage, computer failure or malfunction, or any and all 177 | other commercial damages or losses), even if such Contributor 178 | has been advised of the possibility of such damages. 179 | 180 | 9. Accepting Warranty or Additional Liability. While redistributing 181 | the Work or Derivative Works thereof, You may choose to offer, 182 | and charge a fee for, acceptance of support, warranty, indemnity, 183 | or other liability obligations and/or rights consistent with this 184 | License. However, in accepting such obligations, You may act only 185 | on Your own behalf and on Your sole responsibility, not on behalf 186 | of any other Contributor, and only if You agree to indemnify, 187 | defend, and hold each Contributor harmless for any liability 188 | incurred by, or claims asserted against, such Contributor by reason 189 | of your accepting any such warranty or additional liability. 190 | 191 | END OF TERMS AND CONDITIONS 192 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /filtering.c: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include "common.h" 22 | #include "filtering.h" 23 | #include "description.h" 24 | 25 | typedef struct 26 | { 27 | float* buff; 28 | unsigned int idx; 29 | unsigned int count; 30 | unsigned int sample_size; 31 | } 32 | filter_median_t; 33 | 34 | 35 | typedef struct 36 | { 37 | int max_samples; /* Maximum averaging window size */ 38 | int num_fields; /* Number of fields per sample (usually 3) */ 39 | float *history; /* Working buffer containing recorded samples */ 40 | float *history_sum; /* The current sum of the history elements */ 41 | int history_size; /* Number of recorded samples */ 42 | int history_entries; /* How many of these are initialized */ 43 | int history_index; /* Index of sample to evict next time */ 44 | } 45 | filter_average_t; 46 | 47 | 48 | static unsigned int partition (float* list, unsigned int left, unsigned int right, unsigned int pivot_index) 49 | { 50 | unsigned int i; 51 | unsigned int store_index = left; 52 | float aux; 53 | float pivot_value = list[pivot_index]; 54 | 55 | /* Swap list[pivotIndex] and list[right] */ 56 | aux = list[pivot_index]; 57 | list[pivot_index] = list[right]; 58 | list[right] = aux; 59 | 60 | for (i = left; i < right; i++) 61 | { 62 | if (list[i] < pivot_value) 63 | { 64 | /* Swap list[store_index] and list[i] */ 65 | aux = list[store_index]; 66 | list[store_index] = list[i]; 67 | list[i] = aux; 68 | store_index++; 69 | } 70 | } 71 | 72 | /* Swap list[right] and list[store_index] */ 73 | aux = list[right]; 74 | list[right] = list[store_index]; 75 | list[store_index] = aux; 76 | return store_index; 77 | } 78 | 79 | 80 | static float median (float* queue, unsigned int size) 81 | { 82 | /* http://en.wikipedia.org/wiki/Quickselect */ 83 | 84 | unsigned int left = 0; 85 | unsigned int right = size - 1; 86 | unsigned int pivot_index; 87 | unsigned int median_index = (right / 2); 88 | float temp[size]; 89 | 90 | memcpy(temp, queue, size * sizeof(float)); 91 | 92 | /* If the list has only one element return it */ 93 | if (left == right) 94 | return temp[left]; 95 | 96 | while (left < right) { 97 | pivot_index = (left + right) / 2; 98 | pivot_index = partition(temp, left, right, pivot_index); 99 | if (pivot_index == median_index) 100 | return temp[median_index]; 101 | else if (pivot_index > median_index) 102 | right = pivot_index - 1; 103 | else 104 | left = pivot_index + 1; 105 | } 106 | 107 | return temp[left]; 108 | } 109 | 110 | 111 | static void denoise_median_init (int s, unsigned int num_fields, unsigned int max_samples) 112 | { 113 | filter_median_t* f_data = (filter_median_t*) malloc(sizeof(filter_median_t)); 114 | 115 | f_data->buff = (float*) calloc(max_samples, sizeof(float) * num_fields); 116 | f_data->sample_size = max_samples; 117 | f_data->count = 0; 118 | f_data->idx = 0; 119 | sensor[s].filter = f_data; 120 | } 121 | 122 | 123 | static void denoise_average_init (int s, unsigned int num_fields, unsigned int max_samples) 124 | { 125 | filter_average_t* filter = (filter_average_t*) malloc(sizeof(filter_average_t)); 126 | 127 | if (filter) { 128 | memset(filter, 0, sizeof(filter_average_t)); 129 | filter->max_samples = max_samples; 130 | filter->num_fields = num_fields; 131 | } 132 | 133 | sensor[s].filter = filter; 134 | } 135 | 136 | 137 | static void denoise_median_reset (sensor_info_t* info) 138 | { 139 | filter_median_t* f_data = (filter_median_t*) info->filter; 140 | 141 | if (!f_data) 142 | return; 143 | 144 | f_data->count = 0; 145 | f_data->idx = 0; 146 | } 147 | 148 | 149 | static void denoise_median (sensor_info_t* info, sensors_event_t* data, unsigned int num_fields) 150 | { 151 | unsigned int field, offset; 152 | 153 | filter_median_t* f_data = (filter_median_t*) info->filter; 154 | if (!f_data) 155 | return; 156 | 157 | /* If we are at event count 1 reset the indices */ 158 | if (info->event_count == 1) 159 | denoise_median_reset(info); 160 | 161 | if (f_data->count < f_data->sample_size) 162 | f_data->count++; 163 | 164 | for (field = 0; field < num_fields; field++) { 165 | offset = f_data->sample_size * field; 166 | f_data->buff[offset + f_data->idx] = data->data[field]; 167 | 168 | data->data[field] = median(f_data->buff + offset, f_data->count); 169 | } 170 | 171 | f_data->idx = (f_data->idx + 1) % f_data->sample_size; 172 | } 173 | 174 | 175 | static void denoise_average (sensor_info_t* si, sensors_event_t* data) 176 | { 177 | /* 178 | * Smooth out incoming data using a moving average over a number of 179 | * samples. We accumulate one second worth of samples, or max_samples, 180 | * depending on which is lower. 181 | */ 182 | 183 | int f; 184 | int sampling_rate = (int) si->sampling_rate; 185 | int history_size; 186 | int history_full = 0; 187 | filter_average_t* filter; 188 | 189 | /* Don't denoise anything if we have less than two samples per second */ 190 | if (sampling_rate < 2) 191 | return; 192 | 193 | filter = (filter_average_t*) si->filter; 194 | 195 | if (!filter) 196 | return; 197 | 198 | /* Restrict window size to the min of sampling_rate and max_samples */ 199 | if (sampling_rate > filter->max_samples) 200 | history_size = filter->max_samples; 201 | else 202 | history_size = sampling_rate; 203 | 204 | /* Reset history if we're operating on an incorrect window size */ 205 | if (filter->history_size != history_size) { 206 | filter->history_size = history_size; 207 | filter->history_entries = 0; 208 | filter->history_index = 0; 209 | filter->history = (float*) realloc(filter->history, filter->history_size * filter->num_fields * sizeof(float)); 210 | if (filter->history) { 211 | filter->history_sum = (float*) realloc(filter->history_sum, filter->num_fields * sizeof(float)); 212 | if (filter->history_sum) 213 | memset(filter->history_sum, 0, filter->num_fields * sizeof(float)); 214 | } 215 | } 216 | 217 | if (!filter->history || !filter->history_sum) 218 | return; /* Unlikely, but still... */ 219 | 220 | /* Update initialized samples count */ 221 | if (filter->history_entries < filter->history_size) 222 | filter->history_entries++; 223 | else 224 | history_full = 1; 225 | 226 | /* Record new sample and calculate the moving sum */ 227 | for (f=0; f < filter->num_fields; f++) { 228 | /** A field is going to be overwritten if history is full, so decrease the history sum */ 229 | if (history_full) 230 | filter->history_sum[f] -= filter->history[filter->history_index * filter->num_fields + f]; 231 | 232 | filter->history[filter->history_index * filter->num_fields + f] = data->data[f]; 233 | filter->history_sum[f] += data->data[f]; 234 | 235 | /* For now simply compute a mobile mean for each field and output filtered data */ 236 | data->data[f] = filter->history_sum[f] / filter->history_entries; 237 | } 238 | 239 | /* Update our rolling index (next evicted cell) */ 240 | filter->history_index = (filter->history_index + 1) % filter->history_size; 241 | } 242 | 243 | 244 | void setup_noise_filtering (int s) 245 | { 246 | char filter_buf[MAX_NAME_SIZE]; 247 | int num_fields; 248 | char* cursor; 249 | int window_size = 0; 250 | 251 | /* By default, don't apply filtering */ 252 | sensor[s].filter_type = FILTER_TYPE_NONE; 253 | 254 | /* Restrict filtering to a few sensor types for now */ 255 | switch (sensor[s].type) { 256 | case SENSOR_TYPE_ACCELEROMETER: 257 | case SENSOR_TYPE_GYROSCOPE: 258 | case SENSOR_TYPE_MAGNETIC_FIELD: 259 | num_fields = 3 /* x,y,z */; 260 | break; 261 | 262 | default: 263 | return; /* No filtering */ 264 | } 265 | 266 | /* If noisy, start with default filter for sensor type */ 267 | if (sensor[s].quirks & QUIRK_NOISY) 268 | switch (sensor[s].type) { 269 | case SENSOR_TYPE_GYROSCOPE: 270 | sensor[s].filter_type = FILTER_TYPE_MEDIAN; 271 | break; 272 | 273 | case SENSOR_TYPE_MAGNETIC_FIELD: 274 | sensor[s].filter_type = FILTER_TYPE_MOVING_AVERAGE; 275 | break; 276 | } 277 | 278 | /* Use whatever was specified if there's an explicit configuration choice for this sensor */ 279 | 280 | filter_buf[0] = '\0'; 281 | sensor_get_st_prop(s, "filter", filter_buf); 282 | 283 | cursor = strstr(filter_buf, "median"); 284 | if (cursor) 285 | sensor[s].filter_type = FILTER_TYPE_MEDIAN; 286 | else { 287 | cursor = strstr(filter_buf, "average"); 288 | if (cursor) 289 | sensor[s].filter_type = FILTER_TYPE_MOVING_AVERAGE; 290 | } 291 | 292 | /* Check if an integer is part of the string, and use it as window size */ 293 | if (cursor) { 294 | while (*cursor && !isdigit(*cursor)) 295 | cursor++; 296 | 297 | if (*cursor) 298 | window_size = atoi(cursor); 299 | } 300 | 301 | switch (sensor[s].filter_type) { 302 | 303 | case FILTER_TYPE_MEDIAN: 304 | denoise_median_init(s, num_fields, window_size ? window_size : 5); 305 | break; 306 | 307 | case FILTER_TYPE_MOVING_AVERAGE: 308 | denoise_average_init(s, num_fields, window_size ? window_size: 20); 309 | break; 310 | } 311 | } 312 | 313 | 314 | void denoise (int s, sensors_event_t* data) 315 | { 316 | switch (sensor[s].filter_type) { 317 | 318 | case FILTER_TYPE_MEDIAN: 319 | denoise_median(&sensor[s], data, 3); 320 | break; 321 | 322 | case FILTER_TYPE_MOVING_AVERAGE: 323 | denoise_average(&sensor[s], data); 324 | break; 325 | } 326 | } 327 | 328 | 329 | void release_noise_filtering_data (int s) 330 | { 331 | void *buf; 332 | 333 | if (!sensor[s].filter) 334 | return; 335 | 336 | switch (sensor[s].filter_type) { 337 | 338 | case FILTER_TYPE_MEDIAN: 339 | buf = ((filter_median_t*) sensor[s].filter)->buff; 340 | if (buf) 341 | free(buf); 342 | break; 343 | 344 | case FILTER_TYPE_MOVING_AVERAGE: 345 | buf = ((filter_average_t*) sensor[s].filter)->history; 346 | if (buf) 347 | free(buf); 348 | 349 | buf = ((filter_average_t*) sensor[s].filter)->history_sum; 350 | if (buf) 351 | free(buf); 352 | break; 353 | } 354 | 355 | free(sensor[s].filter); 356 | sensor[s].filter = NULL; 357 | } 358 | 359 | 360 | #define GLOBAL_HISTORY_SIZE 100 361 | 362 | typedef struct 363 | { 364 | int sensor; 365 | int motion_trigger; 366 | sensors_event_t data; 367 | } 368 | recorded_sample_t; 369 | 370 | /* 371 | * This is a circular buffer holding the last GLOBAL_HISTORY_SIZE events, covering the entire sensor collection. It is intended as a way to correlate 372 | * data coming from active sensors, no matter the sensor type, over a recent window of time. The array is not sorted ; we simply evict the oldest cell 373 | * (by insertion time) and replace its contents. Timestamps don't necessarily grow monotonically as they tell the data acquisition type, and that there 374 | * can be a delay between acquisition and insertion into this table. 375 | */ 376 | 377 | static recorded_sample_t global_history[GLOBAL_HISTORY_SIZE]; 378 | 379 | static int initialized_entries; /* How many of these are initialized */ 380 | static int insertion_index; /* Index of sample to evict next time */ 381 | 382 | 383 | void record_sample (int s, const sensors_event_t* event) 384 | { 385 | recorded_sample_t *cell; 386 | int i; 387 | 388 | /* Don't record duplicate samples, as they are not useful for filters */ 389 | if (sensor[s].report_pending == DATA_DUPLICATE) 390 | return; 391 | 392 | if (initialized_entries == GLOBAL_HISTORY_SIZE) { 393 | i = insertion_index; 394 | insertion_index = (insertion_index+1) % GLOBAL_HISTORY_SIZE; 395 | } else { 396 | i = initialized_entries; 397 | initialized_entries++; 398 | } 399 | 400 | cell = &global_history[i]; 401 | 402 | cell->sensor = s; 403 | 404 | cell->motion_trigger = (sensor[s].selected_trigger == sensor[s].motion_trigger_name); 405 | 406 | memcpy(&cell->data, event, sizeof(sensors_event_t)); 407 | } 408 | -------------------------------------------------------------------------------- /common.h: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #ifndef __COMMON_H__ 18 | #define __COMMON_H__ 19 | 20 | #include 21 | 22 | #define MAX_DEVICES 9 /* Check iio devices 0 to MAX_DEVICES-1 */ 23 | #define MAX_SENSORS 12 /* We can handle as many sensors */ 24 | #define MAX_CHANNELS 4 /* We can handle as many channels per sensor */ 25 | #define MAX_EVENTS 2 /* We can handle as many events per channel */ 26 | #define MAX_TRIGGERS 8 /* Check for triggers 0 to MAX_TRIGGERS-1 */ 27 | 28 | #define DEV_FILE_PATH "/dev/iio:device%d" 29 | #define BASE_PATH "/sys/bus/iio/devices/iio:device%d/" 30 | #define TRIGGER_FILE_PATH "/sys/bus/iio/devices/trigger%d/name" 31 | #define IIO_DEVICES "/sys/bus/iio/devices/" 32 | 33 | #define CHANNEL_PATH BASE_PATH "scan_elements/" 34 | #define ENABLE_PATH BASE_PATH "buffer/enable" 35 | #define BUFFER_LENGTH_PATH BASE_PATH "buffer/length" 36 | #define NAME_PATH BASE_PATH "name" 37 | #define TRIGGER_PATH BASE_PATH "trigger/current_trigger" 38 | #define EVENTS_PATH BASE_PATH "events/" 39 | #define SENSOR_ENABLE_PATH BASE_PATH "in_%s_en" 40 | #define SENSOR_OFFSET_PATH BASE_PATH "in_%s_offset" 41 | #define SENSOR_SCALE_PATH BASE_PATH "in_%s_scale" 42 | #define SENSOR_SAMPLING_PATH BASE_PATH "in_%s_sampling_frequency" 43 | #define DEVICE_SAMPLING_PATH BASE_PATH "sampling_frequency" 44 | #define DEVICE_AVAIL_FREQ_PATH BASE_PATH "sampling_frequency_available" 45 | #define ILLUMINATION_CALIBPATH BASE_PATH "in_illuminance_calibscale" 46 | #define SENSOR_CALIB_BIAS_PATH BASE_PATH "in_%s_calibbias" 47 | #define MOUNTING_MATRIX_PATH BASE_PATH "mounting_matrix" 48 | 49 | #define CONFIGFS_TRIGGER_PATH "/sys/kernel/config/iio/triggers/" 50 | 51 | #define PROP_BASE "ro.iio.%s.%s" /* Note: PROPERTY_KEY_MAX is small */ 52 | 53 | #define MAX_TYPE_SPEC_LEN 32 /* Channel type spec len; ex: "le:u10/16>>0" */ 54 | #define MAX_SENSOR_REPORT_SIZE 32 /* Sensor report buffer size */ 55 | #define MAX_DEVICE_REPORT_SIZE 32 /* iio device scan buffer size */ 56 | 57 | #define MAX_NAME_SIZE 64 58 | 59 | #define MAX_SENSOR_BASES 3 /* Max number of base sensors a sensor can rely on */ 60 | 61 | #define ARRAY_SIZE(x) sizeof(x)/sizeof(x[0]) 62 | #define REPORTING_MODE(x) ((x) & 0x06) 63 | 64 | #define FILTER_TYPE_NONE 0 65 | #define FILTER_TYPE_MOVING_AVERAGE 1 66 | #define FILTER_TYPE_MEDIAN 2 67 | 68 | #define MODE_AUTO 0 /* autodetect */ 69 | #define MODE_POLL 1 70 | #define MODE_TRIGGER 2 71 | #define MODE_EVENT 3 72 | 73 | 74 | typedef struct 75 | { 76 | const char *type; /* event type; e.g: transition */ 77 | const char *dir; /* event direction; e.g: rising */ 78 | 79 | /* sysfs entries located in /sys/bus/iio/devices/iio:deviceX/events/ */ 80 | const char *ev_en_path; 81 | const char *ev_value_path; 82 | } 83 | event_descriptor_t; 84 | 85 | 86 | typedef struct 87 | { 88 | const char *name; /* channel name ; ex: x */ 89 | 90 | /* sysfs entries located under scan_elements */ 91 | const char *en_path; /* Enabled sysfs file name ; ex: "in_temp_en" */ 92 | const char *type_path; /* _type sysfs file name */ 93 | const char *index_path; /* _index sysfs file name */ 94 | 95 | /* sysfs entries located in /sys/bus/iio/devices/iio:deviceX */ 96 | const char *raw_path; /* _raw sysfs file name */ 97 | const char *input_path; /* _input sysfs file name */ 98 | const char *scale_path; /* _scale sysfs file name */ 99 | 100 | const int num_events; 101 | event_descriptor_t event[MAX_EVENTS]; 102 | } 103 | channel_descriptor_t; 104 | 105 | 106 | typedef struct 107 | { 108 | const char *tag; /* Prefix such as "accel", "gyro", "temp"... */ 109 | const char *shorthand; 110 | const int type; /* Sensor type ; ex: SENSOR_TYPE_ACCELEROMETER */ 111 | const int num_channels; /* Expected iio channels for this sensor */ 112 | const int is_virtual; /* Is the sensor virtual or not */ 113 | channel_descriptor_t channel[MAX_CHANNELS]; 114 | } 115 | sensor_catalog_entry_t; 116 | 117 | 118 | typedef struct 119 | { 120 | char sign; 121 | char endianness; 122 | short realbits; 123 | short storagebits; 124 | short shift; 125 | } 126 | datum_info_t; 127 | 128 | 129 | typedef struct 130 | { 131 | int offset; /* Offset in bytes within the iio character device report */ 132 | int size; /* Field size in bytes */ 133 | float scale; /* Scale for each channel */ 134 | char type_spec[MAX_TYPE_SPEC_LEN]; /* From driver; ex: le:u10/16>>0 */ 135 | datum_info_t type_info; /* Decoded contents of type spec */ 136 | float opt_scale; /* 137 | * Optional correction scale read from a property such as iio.accel.x.scale, allowing late compensation of 138 | * problems such as misconfigured axes ; set to 1 by default. Applied at the end of the scaling process. 139 | */ 140 | int raw_path_present; /* Flag signalling the presence of in___raw file */ 141 | int input_path_present; /* Flag signalling the presence of in__input file */ 142 | } 143 | channel_info_t; 144 | 145 | 146 | typedef struct 147 | { 148 | /* Conversion function called once per channel */ 149 | float (*transform) (int s, int c, unsigned char* sample_data); 150 | 151 | /* Function called once per sample */ 152 | int (*finalize) (int s, sensors_event_t* data); 153 | } 154 | sample_ops_t; 155 | 156 | 157 | /* 158 | * Whenever we have sensor data recorded for a sensor in the associated 159 | * sensor cell, its report_pending field is set to a non-zero value 160 | * indicating how we got this data. 161 | */ 162 | #define DATA_TRIGGER 1 /* From /dev/iio:device fd */ 163 | #define DATA_SYSFS 2 /* Through polling */ 164 | #define DATA_DUPLICATE 3 /* Duplicate of triggered motion sample */ 165 | 166 | 167 | typedef struct 168 | { 169 | char friendly_name[MAX_NAME_SIZE]; /* ex: Accelerometer */ 170 | char internal_name[MAX_NAME_SIZE]; /* ex: accel_3d */ 171 | char vendor_name[MAX_NAME_SIZE]; /* ex: Intel */ 172 | char init_trigger_name[MAX_NAME_SIZE]; /* ex: accel-name-dev1 */ 173 | char motion_trigger_name[MAX_NAME_SIZE];/* ex: accel-any-motion-dev1 */ 174 | char hrtimer_trigger_name[MAX_NAME_SIZE]; /*ex: accel-hr-dev1 */ 175 | int trigger_nr; /* trigger number associated with this device */ 176 | float max_range; 177 | float resolution; 178 | float power; 179 | 180 | /* 181 | * Currently active trigger - either a pointer to the initial (default) trigger name buffer, or a pointer to the motion trigger name buffer, 182 | * or something else (typically NULL or a pointer to some static "\n". This is used to determine if the conditions are met to switch from 183 | * the default trigger to the motion trigger for a sensor, or rather for the interrupt-driven sensors associated to a given iio device. 184 | */ 185 | const char* selected_trigger; 186 | 187 | float offset; /* (cooked = raw + offset) * scale */ 188 | float scale; /* default:1. when set to 0, use channel specific value */ 189 | float illumincalib; /* to set the calibration for the ALS */ 190 | 191 | float requested_rate; /* requested events / second */ 192 | float sampling_rate; /* setup events / second */ 193 | 194 | float min_supported_rate; 195 | float max_supported_rate; 196 | 197 | int dev_num; /* Associated iio dev num, ex: 3 for /dev/iio:device3 */ 198 | 199 | int catalog_index; /* Associated entry within the sensor_catalog array */ 200 | int type; /* Sensor type, such as SENSOR_TYPE_GYROSCOPE */ 201 | 202 | int num_channels; /* Actual channel count ; 0 for poll mode sensors */ 203 | 204 | int mode; /* Usage mode, ex: poll, trigger ... */ 205 | 206 | /* 207 | * The array below indicates where to gather report data for this sensor inside the reports that we read from the iio character device. 208 | * It is updated whenever channels are enabled or disabled on the same device. Channel size indicates the size in bytes of fields, and 209 | * should be zero for disabled channels. The type field indicates how a specific channel data item is structured. 210 | */ 211 | channel_info_t channel[MAX_CHANNELS]; 212 | 213 | /* 214 | * This flag is set if we acquired data from the sensor but did not forward it to upper layers (i.e. Android) yet. If so, report_buffer 215 | * contains that data. Valid values are 0: empty, 1: normal, 2: repeat of last acquired value after timeout. 216 | */ 217 | int report_pending; 218 | 219 | /* This flag is set if we have a meta data event pending */ 220 | int meta_data_pending; 221 | 222 | /* 223 | * Timestamp closely matching the date of sampling, preferably retrieved from a iio channel alongside sample data. Value zero indicates that 224 | * we couldn't get such a closely correlated timestamp, and that one has to be generated before the report gets sent up to Android. 225 | */ 226 | int64_t report_ts; 227 | 228 | /* Buffer containing the last generated sensor report for this sensor */ 229 | unsigned char report_buffer[MAX_SENSOR_REPORT_SIZE]; 230 | 231 | /* Whether or not the above buffer contains data from a device report */ 232 | int report_initialized; 233 | 234 | /* Channel and sample finalization callbacks for this sensor */ 235 | sample_ops_t ops; 236 | 237 | int cal_level; /* 0 means not calibrated */ 238 | 239 | /* 240 | * Depending on the sensor, calibration may take too much time at higher levels. Allow optional capping to a certain level. 241 | */ 242 | int max_cal_level; 243 | 244 | void *cal_data; /* Sensor calibration data, e.g. for magnetometer */ 245 | 246 | void* filter; /* Filtering data for noisy sensors */ 247 | int filter_type;/* FILTER_ specification for this sensor ; default is FILTER_NONE */ 248 | 249 | /* Previously reported value, for on-change sensors */ 250 | union { 251 | float data; 252 | uint64_t data64; 253 | } prev_val; 254 | /* 255 | * Certain sensors expose their readings through sysfs files that have a long response time (100-200 ms for ALS). Rather than block our 256 | * global control loop for several hundred ms each second, offload those lengthy blocking reads to dedicated threads, which will then report 257 | * their data through a fd that we can add to our poll fd set. 258 | */ 259 | int thread_data_fd[2]; 260 | pthread_t acquisition_thread; 261 | 262 | int base_count; /* How many base sensors is the sensor depending on */ 263 | int base[MAX_SENSOR_BASES]; 264 | 265 | uint32_t quirks; /* Bit mask expressing the need for special tweaks */ 266 | 267 | /* Note: we may have to explicitely serialize access to some fields */ 268 | 269 | int is_virtual; /* Composite sensor, exposed from data acquired through other sensors */ 270 | 271 | uint32_t ref_count; /* Dependency count - for a real sensor how many active virtual sensors are depending on it */ 272 | 273 | uint32_t directly_enabled; /* Flag showing if a sensor was enabled directly by Android */ 274 | 275 | /* 276 | * Current sample for a virtual sensor - when a report is ready we'll keep the data here until it's finally processed. Can be modified for 277 | * more than one at a later time. 278 | */ 279 | sensors_event_t sample; 280 | uint64_t event_id; 281 | 282 | /* 283 | * If the QUIRK_FIELD_ORDERING bit is set in quirks, the contents of this array are used in the finalization stage to swap sample fields 284 | * before transmitting them to Android ; they form a mapping between the indices of the input and output arrays: ex: 0123 is identity for 285 | * a sample containing 4 fields. 286 | */ 287 | unsigned char order[MAX_CHANNELS]; 288 | 289 | /* 290 | * If the QUIRK_MOUNTING_MATRIX bit is set in quirks, the contents of this matrix is used to correct the sample values so that it takes 291 | * into account the way the sensor has been mounted on the PCB. 292 | */ 293 | float mounting_matrix[9]; 294 | 295 | /** Count of available frequencies */ 296 | int avail_freqs_count; 297 | 298 | /** Array of available frequencies */ 299 | float* avail_freqs; 300 | 301 | /* 302 | * Event counter - will be used to check if we have a significant sample for noisy sensors. We want to make sure we do not send any wrong 303 | * events before filtering kicks in. We can also use it for statistics. 304 | */ 305 | uint64_t event_count; 306 | 307 | /* Some polled sensors need to be first enabled so that they start 308 | * computing a set of values in hardware (e.g step counter). Enabling 309 | * is done through a sysfs attribute in__en 310 | */ 311 | int needs_enable; 312 | 313 | float semi_arbitrated_rate; /* Arbitrated sampling rate before we considered other sensors co-located on the same iio device */ 314 | } 315 | sensor_info_t; 316 | 317 | 318 | /* Reference a few commonly used variables... */ 319 | extern int sensor_count; 320 | extern struct sensor_t sensor_desc[MAX_SENSORS]; 321 | extern sensor_info_t sensor[MAX_SENSORS]; 322 | extern sensor_catalog_entry_t sensor_catalog[]; 323 | extern unsigned int catalog_size; 324 | 325 | /* Needed both in sensors and activity HALs */ 326 | void check_trig_sensors (int i, char *sysfs_file, char map[catalog_size]); 327 | void check_poll_sensors (int i, char *sysfs_file, char map[catalog_size]); 328 | void check_event_sensors (int i, char *sysfs_file, char map[catalog_size]); 329 | void discover_sensors(int dev_num, char *sysfs_base_path, char map[catalog_size], 330 | void (*discover_sensor)(int, char*, char*)); 331 | 332 | /* 333 | * Macros associating iio sysfs entries to to sensor types ; see 334 | * linux/kernel/drivers/iio/industrialio-core.c and 335 | * hardware/libhardware/include/hardware/sensor.h 336 | */ 337 | 338 | #define DECLARE_VOID_CHANNEL(tag) \ 339 | tag, \ 340 | "", \ 341 | "", \ 342 | "", \ 343 | "", \ 344 | "", \ 345 | "", \ 346 | 347 | #define DECLARE_CHANNEL(tag, spacer, name) \ 348 | name, \ 349 | "in_"tag spacer name"_en", \ 350 | "in_"tag spacer name"_type", \ 351 | "in_"tag spacer name"_index", \ 352 | "in_"tag spacer name"_raw", \ 353 | "in_"tag spacer name"_input", \ 354 | "in_"tag spacer name"_scale", \ 355 | 0, {{0}}, 356 | 357 | #define DECLARE_NAMED_CHANNEL(tag, name) DECLARE_CHANNEL(tag, "_", name) 358 | 359 | #define DECLARE_GENERIC_CHANNEL(tag) DECLARE_CHANNEL(tag, "", "") 360 | 361 | #define DECLARE_EVENT(tag, spacer1, name, spacer2, type, spacer3, dir) \ 362 | type, dir, \ 363 | "in_"tag spacer1 name spacer2 type spacer3 dir"_en", \ 364 | "in_"tag spacer1 name spacer2 type spacer3 dir"_value", \ 365 | 366 | #define DECLARE_GENERIC_EVENT(tag, name, type, dir) \ 367 | DECLARE_EVENT(tag, "_", name, "_", type, "_", dir) 368 | #define DECLARE_NAMED_EVENT(tag, name) \ 369 | DECLARE_EVENT(tag, "_", name, "","","","") 370 | 371 | #endif 372 | -------------------------------------------------------------------------------- /README: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2015 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | iio sensors HAL documentation 16 | _____________________________ 17 | 18 | 19 | PURPOSE OF THE IIO SENSORS HAL 20 | 21 | This library links the Android sensors framework to the set of Linux sensors 22 | drivers that expose a iio interface. 23 | 24 | These layers are mostly documented here: 25 | https://source.android.com/devices/sensors/hal-interface.html 26 | [basic tour of the Android sensors HAL interface] 27 | 28 | http://source.android.com/devices/halref/sensors_8h_source.html 29 | [Android sensor details] 30 | 31 | https://www.kernel.org/doc/Documentation/ABI/testing/sysfs-bus-iio 32 | [overview of the iio interface] 33 | 34 | 35 | DESIGN GOALS 36 | 37 | The iio sensors HAL is designed to drive a complete collection of sensors, 38 | whose types and properties are discovered dynamically. It should be reusable 39 | without modification across a variety of boards, avoiding creating custom 40 | HALs over and over, and allowing quick sensors enabling on new hardware 41 | platforms. It's meant to be small, simple, and have minimal CPU and memory 42 | overhead. 43 | 44 | 45 | FUNCTION 46 | 47 | The HAL discovers the set of available sensors at startup, reports them to 48 | Android, and performs basic operations on them: 49 | 50 | - enable and disable sensors 51 | - set the rate at which sensors should report events to Android 52 | - await for samples and return them to Android in the format it expects 53 | 54 | This is primarily done by reading and writing sysfs files located under 55 | /sys/bus/iio, as well as interacting with /dev/iio:deviceX character devices. 56 | 57 | 58 | SUPPORTED SENSOR TYPES 59 | 60 | As of march 2015 the following sensor types are supported: 61 | 62 | - accelerometer 63 | - gyroscope 64 | - magnetometer 65 | - ambient light sensor 66 | - temperature sensor 67 | - proximity sensor 68 | - step detector 69 | - step counter 70 | 71 | 72 | ENUMERATION 73 | 74 | Basic enumeration happens by scanning /sys/bus/iio/devices. We search each of 75 | the device subfolders for sysfs entries that we can associate to known sensor 76 | types. Each of the iio devices can possibly support several sensors. 77 | Of particular interest is the scan_elements subfolder, which we use to 78 | determine if a specific sensor will be used in trigger mode (interrupt driven) 79 | or in polled mode (sampling happens in response to sysfs reads). 80 | 81 | 82 | EVENTS and POLLING 83 | 84 | The preferred way to retrieve data from a sensor is though the associated iio 85 | character device. That implies that the sensor can report events whenever new 86 | samples become available. The iio sensor HAL opens a fd on each of the 87 | /dev/iio:deviceX files associated to enabled sensors, and add these fds to a 88 | fd set monitored by a single poll call, which blocks the HAL's main sensor data 89 | polling function. From there we read "device reports" that can possibly hold 90 | data for several sensors, and split that in "sensor reports" that we translate 91 | into the format Android expects. 92 | 93 | Another mode of operation is polling mode. It is engaged if no scan_elements 94 | folder is found, or if no iio channels are detected for the sensor. In that 95 | case we start a dedicated data acquisition thread for the sensor, which will 96 | periodically read sysfs entries (either _raw or _input) to get sensor data. 97 | This data is then transmitted to the main poll thread through Unix pipes, 98 | whose reader end fds get added to the monitored fd set. 99 | 100 | 101 | TRIGGERS 102 | 103 | Triggers are the iio way of selecting when a iio driver should alert userspace 104 | that there is new data available. At least one "data ready" trigger needs to be 105 | exposed. These are expected to fire periodically, at a programmed sampling 106 | rate, whenever the sensor driver acquires a new sample from hardware. 107 | 108 | Another type of trigger we support is motion based ; if a motion trigger is 109 | selected, the driver will avoid sending duplicate samples to userspace. In 110 | practice, the sensors HAL only gets data when there the device position 111 | changes. The iio sensor HAL has an internal "repeat last sample" logic for 112 | sensor types from which Android expects to get data periodically, unbeknown 113 | that layers below are only sending data on motion. We selected to engage this 114 | mode only for low frequencies (~25 Hz or less) as certain games are sensitive 115 | to the thresholding effects that motion triggers yield. 116 | 117 | 118 | FDs and CHANNELS 119 | 120 | The iio sensors HAL always open a fd on the iio device associated to an 121 | enabled sensor. The assumption there is that the hardware should be powered 122 | off unless a fd is open on the associated iio device. That is done even for 123 | polled sensors. 124 | 125 | If the iio device supports several sensors types we can handle, all recognized 126 | channels get enabled at startup, and remain so. Although the iio interface 127 | supports dynamic enabling and disabling of individual channels, doing so 128 | changes the size of the "device reports" we read from the iio device fds, and 129 | creates complicated synchronization issues in the data decoding code path of 130 | the HAL, that we preferred to avoid for the time being. 131 | 132 | 133 | TIMESTAMPS 134 | 135 | Android associates timestamps to samples. These timestamps are expressed as 136 | the time elapsed since the beginning of the boot sequence. 137 | 138 | If possible, we read the iio timestamps from the timestamp channel, alongside 139 | sample data ; these are closely correlated to the actual data acquisition time, 140 | as they come from the driver, and possibly from hardware. 141 | 142 | 143 | ORIENTATION MAPPING 144 | 145 | The sensors HAL is able to interpret optional 'panel' and 'rotation' specifiers 146 | from ACPI DSDT entries for sensors. See: 147 | http://www.spinics.net/lists/linux-acpi/msg51540.html 148 | 149 | It is possible to supersede these values using the .panel and .rotation 150 | properties (both need to be specified, and they are read only once at boot 151 | time). 152 | 153 | 154 | UNIT CONVERSION 155 | 156 | IIO and Android occasionally disagree on the units to use. 157 | That is the case for: 158 | - magnetic field strength: Tesla vs Gauss 159 | - proximity 160 | 161 | The HAL performs appropriate mappings. 162 | 163 | 164 | OPTIONAL PROPERTIES 165 | 166 | We support a variety of properties. Some convey user visible information which 167 | is passed Android. Properties are expressed by sensor type, such as: 168 | ro.iio.accel.name = "Intel Accelerometer" 169 | 170 | On certain boards we may have several sensors of the same type. It's then 171 | possible to specialize the name using the iio sysfs 'name' contents: 172 | ro.iio.temp.bmg160.name = "BMG160 Thermometer". 173 | 174 | If several properties match, the most specific form has higher priority. 175 | 176 | All properties are optional. As of March 2015 the following properties are 177 | supported: 178 | 179 | .name : user visible sensor name, passed to Android 180 | .vendor : user visible sensor manufacturer name, passed to Android 181 | .resolution : sensor measurement resolution, in Android units, passed to 182 | Android 183 | .power : sensor estimated power draw, in mA, presumably at 3.7V 184 | .transform : used to switch to the units used by early ISH drivers; 185 | deprecated 186 | .max_freq : specifies a cap for the sensor sampling rate 187 | .min_freq : specify a floor for the sensor sampling rate 188 | .cal_steps : specify the maximum attempted calibration level for the 189 | magnetometer 190 | .illumincalib : specify a gain for certain ALS drivers ; passed through sysfs 191 | .order : allows reordering channels ; used internally ; deprecated 192 | .quirks : allows specifying various processing options ; see QUIRKS 193 | .panel : allows expressing/superseding the _PLD panel indicator 194 | (4=front, 5=back) 195 | .rotation : allows expressing/superseding the _PLD rotation indicator 196 | (x 45° clockwise) 197 | .scale : scaling/sensitivity hint for the driver, stored through sysfs 198 | .opt_scale : optional scaling applied at a late stage to channel values; 199 | deprecated 200 | .filter : allows selecting one of the available filters, and its strength 201 | 202 | 203 | QUIRKS 204 | 205 | One of the properties we use allows influencing how a specific sensor is used. 206 | It's the 'quirks' property, and allows the HAL to compensate for hardware or 207 | driver idiosyncrasies. Several quirks can be specified using commas to separate 208 | them. 209 | 210 | Available quirks are: 211 | 212 | noisy : engage default filter for the sensor type to smooth out noise 213 | terse : auto-repeat events as if the trigger was a motion trigger, 214 | even though it's not advertised as such 215 | continuous : disable use of motion trigger even if the sensor supports it 216 | init-rate : set sampling rate at 10 Hz after enabling that sensor 217 | biased : the sensor has unusually high bias ; engage high bias detection 218 | and compensation routines 219 | spotty : the sensor may have gaps in its events sequence; adjust 220 | timestamps accordingly 221 | no-poll : specifically disable the iio polling (sysfs) way of getting 222 | data from this driver, even if it's seemingly available 223 | no-trig : specifically disable the iio trigger way of getting data from 224 | this driver, even if it's seemingly available 225 | no-event : specifically disable the iio event way of getting data from 226 | this driver, even if it's seemingly available 227 | 228 | 229 | FILTERING 230 | 231 | Some sensor types are inherently low precision and provide data that is 232 | statistically noisy. If the noisy quirk is specified, we apply a predetermined 233 | filtering strategy depending on the sensor type, to smooth out the noise in 234 | samples before they are passed to Android. That can add latency in the sensor 235 | output. It's also possible to individually set a sensor filter selection and 236 | strength through properties. 237 | 238 | ro.iio.anglvel.filter = average 239 | 240 | or 241 | 242 | ro.iio.anglvel.filter = average, 10 243 | 244 | 245 | CALIBRATION 246 | 247 | Calibration is a different concept from filtering. It has a different meaning 248 | depending on the sensor type. 249 | 250 | 251 | UNCALIBRATED SENSORS 252 | 253 | Android 4.4 (KitKat) introduced the uncalibrated gyroscope and uncalibrated 254 | magnetometer sensor types. They are virtual sensors that decouple the sensor 255 | data from the correction that is applied to it by the HAL, so upper layers can 256 | choose to alter or ignore the correction that got applied at the HAL level. 257 | 258 | 259 | VIRTUAL SENSORS 260 | 261 | The HAL can expose logical sensors, such as the uncalibrated gyroscope, in 262 | addition to the set of iio sensors. These are built on top of base sensors. 263 | The Android framework can add its own virtual sensors too. Those are typically 264 | composite (fusion) sensors, relying on several base sensors for their work. 265 | The current Android code for this, as of Android 5.0, sets the gyroscope at 266 | 200 Hz, the magnetometer at 50 Hz, and the accelerometer at the target 267 | frequency for the virtual sensor. 268 | 269 | 270 | SAMPLING RATE 271 | 272 | Arbitration levels and iio device collocation, virtual sensors, per sensor 273 | rate, Android level arbitration, published rates. 274 | 275 | 276 | TRANSFORMS 277 | 278 | The transform property got used to support early iio drivers for the Intel 279 | Sensor Hub on Haswell machines. It should no longer be used and support for 280 | it may be deleted in the future. 281 | 282 | 283 | SOURCE TREE 284 | 285 | The most central source files are: 286 | 287 | common.h : definitions shared among all files 288 | entry.c : iio sensors HAL entry points 289 | enumeration.c : sensor enumeration routines 290 | control.c : enabling, disabling, sampling rate control 291 | 292 | 293 | THREADS 294 | 295 | The sensors HAL code runs in the context of the calling threads (Android Sensor 296 | Service threads, from the Service Manager process). It spawns one additional 297 | thread per polling sensor in use though. This thread communicates its data to 298 | the single polling thread through a pipe, whose fd is added to the set of fds 299 | the polling thread waits on. 300 | 301 | 302 | BATCHING 303 | 304 | 305 | DRIVER DESIDERATA 306 | 307 | - one iio device per sensor 308 | - interrupt driven 309 | - no jitter 310 | - sampling frequency per sensor 311 | - timestamp channel 312 | - fast stabilization time on enabling 313 | 314 | 315 | MISC 316 | 317 | - ueventd.rc file access rights 318 | - iio-sensors-hal.so (IRDA autodetect) vs sensors.gmin.so (GMIN) 319 | - .conf files, persistency 320 | - code writing convention 321 | 322 | 323 | HISTORICAL PERSPECTIVE 324 | - Star Peak on XPS 12, Harris Beach, T100 325 | - GMIN MRD 7, Anchor 8 326 | - IRDA ECS, ECS 2, CHIPHD, Malata 327 | - SoFIA 328 | - ISH 329 | 330 | 331 | AUTO-DETECTION 332 | 333 | Multi-device targets (coho/cohol) rely on the hardware auto-detection daemon 334 | (hald) to set properties for the enumerated sensors. hald listens for uevents 335 | that get sent by the kernel during system startup, and matches them against a 336 | list of known sensor parts. This list is built from the set of HAL record files 337 | located under /system/etc/hald/hrec.d. Whenever a match is found, the 338 | properties defined in the sensor's record file are set. Additional actions, 339 | such as installing permission files, are possible. For targets that don't rely 340 | on autodetection, sensor properties are set in system init scripts. 341 | 342 | The following commands may be useful (run as root): 343 | 344 | halctl -l : lists detected devices 345 | halctl -g sensors : identifies the selected sensors HAL library 346 | getprop | grep iio : list sensor properties 347 | pm list features | grep sensor : list sensor features, as defined through 348 | permission files 349 | logcat | grep -i sensor : get sensor traces 350 | 351 | 352 | TIPS AND TRICKS 353 | 354 | The iio sensors HAL .so file is stored on a read-only partition, under 355 | /system/lib/hw. Quick testing of code changes can be done using the following 356 | commands: 357 | 358 | mmm 359 | adb root 360 | adb remount 361 | adb pull 362 | adb push 363 | adb reboot 364 | adb shell 365 | 366 | ALOGV traces are compiled out ; you may want to redefine ALOGV in common.h in 367 | order to get them. 368 | -------------------------------------------------------------------------------- /activity.c: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include "activity_event_utils.h" 31 | 32 | /* Return error codes */ 33 | #define ARGV_ERR 1 34 | #define HAL_ACCESS_ERR 1 35 | #define HAL_OPEN_ERR 2 36 | #define HAL_SYMBOL_ERR 3 37 | 38 | #define START_CMD "start" 39 | #define STOP_CMD "stop" 40 | #define REGISTER_CMD "register_callback" 41 | #define ENABLE_CMD "enable" 42 | #define DISABLE_CMD "disable" 43 | #define LIST_CMD "list" 44 | #define FLUSH_CMD "flush" 45 | #define MONITOR_START_CMD "monitor_start" 46 | #define MONITOR_STOP_CMD "monitor_stop" 47 | 48 | #define MAX_CMD_SIZE 512 49 | #define MAX_ARGS 8 50 | 51 | #define CMD_SEPARATOR " " 52 | 53 | #define ACK_FLAG 1 54 | #define MONITOR_START_FLAG 2 55 | #define MONITOR_STOP_FLAG 3 56 | 57 | /* We define a default latency since current HAL does not use this latency */ 58 | #define DEFAULT_LATENCY 0 59 | 60 | static struct activity_recognition_module *hmi; 61 | static struct hw_device_t *device; 62 | 63 | static FILE *client, *monitor_client; 64 | static int monitor_connection = -1; 65 | 66 | #ifdef ANDROID 67 | #define ACTIVITY_SERVER_NAME "/dev/socket/activity-server" 68 | #else 69 | #define ACTIVITY_SERVER_NAME "/tmp/activity-server" 70 | #endif 71 | 72 | #define CLIENT_ERR(f, fmt...) \ 73 | { if (f) { fprintf(f, fmt); fprintf(f, "\n"); } ALOGE(fmt); } 74 | 75 | struct sockaddr_un server_addr = { 76 | .sun_family = AF_UNIX, 77 | .sun_path = ACTIVITY_SERVER_NAME, 78 | }; 79 | 80 | /* Event types are indexed so that a type is positioned at an index represented 81 | * by its value in the ACTIVITY_TYPE_* enum in activity_recognition.h . 82 | */ 83 | static const char* event_types[] = { 84 | "FLUSH_COMPLETE", 85 | "ENTER", 86 | "EXIT", 87 | }; 88 | 89 | static void dummy_activity_callback(const activity_recognition_callback_procs_t *procs __attribute((unused)), 90 | const activity_event_t *events, int count) 91 | { 92 | int i; 93 | 94 | /* Exit if nobody is monitoring */ 95 | if (monitor_client == NULL) 96 | return; 97 | 98 | fprintf(monitor_client, "No. of activity events: %d\n", count); 99 | 100 | for (i = 0; i < count; i++) 101 | fprintf(monitor_client, "\t occurred at %lld (ns)\n", 102 | events[i].activity, 103 | event_types[events[i].event_type], 104 | events[i].timestamp); 105 | } 106 | 107 | static activity_recognition_callback_procs_t dummy_callback_procs = { 108 | .activity_callback = dummy_activity_callback 109 | }; 110 | 111 | /* We register a dummy callback that just prints the events reported from HAL. */ 112 | static void register_activity_callback(void) 113 | { 114 | struct activity_recognition_device *activity_dev = 115 | (struct activity_recognition_device *) device; 116 | 117 | activity_dev->register_activity_callback(activity_dev, &dummy_callback_procs); 118 | } 119 | 120 | static int enable_activity_event(uint32_t handle, uint32_t event_type) 121 | { 122 | struct activity_recognition_device *activity_dev = 123 | (struct activity_recognition_device *) device; 124 | 125 | return activity_dev->enable_activity_event(activity_dev, handle, 126 | event_type, DEFAULT_LATENCY); 127 | } 128 | 129 | static int disable_activity_event(uint32_t handle, uint32_t event_type) 130 | { 131 | struct activity_recognition_device *activity_dev = 132 | (struct activity_recognition_device*) device; 133 | 134 | return activity_dev->disable_activity_event(activity_dev, handle, event_type); 135 | } 136 | 137 | static int flush(void) 138 | { 139 | struct activity_recognition_device *activity_dev = 140 | (struct activity_recognition_device *) device; 141 | 142 | return activity_dev->flush(activity_dev); 143 | } 144 | 145 | static int print_usage(void) 146 | { 147 | fprintf(stderr, "Program usage:\n"); 148 | fprintf(stderr, "\tactivity %s\n", START_CMD); 149 | fprintf(stderr, "\tactivity %s\n", STOP_CMD); 150 | fprintf(stderr, "\tactivity %s\n", REGISTER_CMD); 151 | fprintf(stderr, "\tactivity %s\n", LIST_CMD); 152 | fprintf(stderr, "\tactivity %s \n", ENABLE_CMD); 153 | fprintf(stderr, "\tactivity %s \n", DISABLE_CMD); 154 | fprintf(stderr, "\tactivity %s\n", FLUSH_CMD); 155 | fprintf(stderr, "\tactivity %s\n", MONITOR_START_CMD); 156 | fprintf(stderr, "\tactivity %s\n", MONITOR_STOP_CMD); 157 | fprintf(stderr, "\t* is the index of the activity as shown by running \"activity list\"\n"); 158 | fprintf(stderr, "\t* is one of the following:\n"); 159 | fprintf(stderr, "\t\t%d => activity event ENTER\n", ACTIVITY_EVENT_ENTER); 160 | fprintf(stderr, "\t\t%d => activity event EXIT\n", ACTIVITY_EVENT_EXIT); 161 | fprintf(stderr, "\t\t%d => activity event FLUSH COMPLETE\n", ACTIVITY_EVENT_FLUSH_COMPLETE); 162 | 163 | return ARGV_ERR; 164 | } 165 | 166 | static int parse_cmd(char buffer[]) 167 | { 168 | char *tmp, *args[MAX_ARGS]; 169 | int count; 170 | 171 | tmp = strtok(buffer, CMD_SEPARATOR); 172 | count = 0; 173 | while (tmp) { 174 | args[count++] = tmp; 175 | tmp = strtok(NULL, CMD_SEPARATOR); 176 | } 177 | 178 | if (!count) { 179 | CLIENT_ERR(client, "Invalid command %s", buffer); 180 | return -1; 181 | } 182 | 183 | if (strncmp(args[0], STOP_CMD, sizeof(STOP_CMD)) == 0) { 184 | if (count != 1) { 185 | CLIENT_ERR(client, "Too many arguments. Trimming command down to \ 186 | 'activity %s'", STOP_CMD); 187 | } else { 188 | fprintf(client, "Stopping server\n"); 189 | fflush(client); 190 | } 191 | 192 | unlink(ACTIVITY_SERVER_NAME); 193 | 194 | exit(EXIT_SUCCESS); 195 | } 196 | 197 | if (strncmp(args[0], LIST_CMD, sizeof(LIST_CMD)) == 0) { 198 | const char * const* activities; 199 | int size, i; 200 | 201 | if (count != 1) 202 | CLIENT_ERR(client, "Too many arguments. Trimming command down to \ 203 | 'activity %s'", LIST_CMD); 204 | size = hmi->get_supported_activities_list(hmi, &activities); 205 | if (client) { 206 | fprintf(client, "Activities list:\n"); 207 | for (i = 0; i < size; i++) 208 | fprintf(client, "\t[%d] %s\n", i + 1, activities[i]); 209 | } 210 | 211 | return 0; 212 | } 213 | 214 | if (strncmp(args[0], REGISTER_CMD, sizeof(REGISTER_CMD)) == 0) { 215 | if (count != 1) 216 | CLIENT_ERR(client, "Too many arguments. Trimming command down to \ 217 | 'activity %s'", REGISTER_CMD); 218 | register_activity_callback(); 219 | 220 | return 0; 221 | } 222 | 223 | if (strncmp(args[0], ENABLE_CMD, sizeof(ENABLE_CMD)) == 0) { 224 | if (count > 3) 225 | CLIENT_ERR(client, "Too many arguments. Trimming command down to \ 226 | 'activity %s %s %s'", ENABLE_CMD, args[1], args[2]) 227 | else if (count != 3) { 228 | CLIENT_ERR(client, "Insufficient arguments. Command should be \ 229 | 'activity %s '", ENABLE_CMD); 230 | return -1; 231 | } 232 | 233 | return enable_activity_event(atoi(args[1]), atoi(args[2])); 234 | } 235 | 236 | if (strncmp(args[0], DISABLE_CMD, sizeof(DISABLE_CMD)) == 0) { 237 | if (count > 3) 238 | CLIENT_ERR(client, "Too many arguments. Trimming command down to \ 239 | 'activity %s %s %s'", DISABLE_CMD, args[1], args[2]) 240 | else if (count != 3) { 241 | CLIENT_ERR(client, "Insufficient arguments. Command should be \ 242 | 'activity %s '", DISABLE_CMD); 243 | return -1; 244 | } 245 | 246 | return disable_activity_event(atoi(args[1]), atoi(args[2])); 247 | } 248 | 249 | if (strncmp(args[0], FLUSH_CMD, sizeof(FLUSH_CMD)) == 0) { 250 | if (count != 1) 251 | CLIENT_ERR(client, "Too many arguments. Trimming command down to \ 252 | 'activity %s'", FLUSH_CMD); 253 | 254 | return flush(); 255 | } 256 | 257 | if (strncmp(args[0], MONITOR_START_CMD, sizeof(MONITOR_START_CMD)) == 0) { 258 | if (count != 1) 259 | CLIENT_ERR(client, "Too many arguments. Trimming command \ 260 | down to 'activity %s'", MONITOR_START_CMD); 261 | return MONITOR_START_FLAG; 262 | } 263 | 264 | if (strncmp(args[0], MONITOR_STOP_CMD, sizeof(MONITOR_STOP_CMD)) == 0) { 265 | if (count != 1) 266 | CLIENT_ERR(client, "Too many arguments. Trimming command \ 267 | down to 'activity %s'", MONITOR_STOP_CMD); 268 | return MONITOR_STOP_FLAG; 269 | } 270 | 271 | CLIENT_ERR(client, "Invalid command %s", buffer); 272 | 273 | return -1; 274 | } 275 | 276 | static void stop_monitoring(int *flag) 277 | { 278 | int current_flag = ACK_FLAG; 279 | 280 | /* Send ACK to client */ 281 | write(monitor_connection, ¤t_flag, sizeof(current_flag)); 282 | 283 | /* Cleanup */ 284 | fclose(monitor_client); 285 | close(monitor_connection); 286 | monitor_client = NULL; 287 | monitor_connection = -1; 288 | 289 | *flag = ACK_FLAG; 290 | } 291 | 292 | static void start_monitoring(int conn, int *flag) 293 | { 294 | /* Stop other started monitor if that's the case */ 295 | if (monitor_client != NULL) 296 | stop_monitoring(flag); 297 | 298 | monitor_client = client; 299 | monitor_connection = conn; 300 | *flag = MONITOR_START_FLAG; 301 | 302 | } 303 | 304 | static void start_server(void) 305 | { 306 | int sock, conn, ret; 307 | int flag; 308 | 309 | /* Just to make sure we do not have more than one server instance */ 310 | unlink(ACTIVITY_SERVER_NAME); 311 | 312 | sock = socket(AF_UNIX, SOCK_SEQPACKET, 0); 313 | if (sock == -1) { 314 | ALOGE("Error %s creating socket\n", strerror(errno)); 315 | exit(1); 316 | } 317 | 318 | ret = bind(sock, (struct sockaddr *) &server_addr, sizeof(server_addr)); 319 | if (ret == -1) { 320 | ALOGE("Error %s binding socket\n", strerror(errno)); 321 | exit(1); 322 | } 323 | 324 | ret = listen(sock, 1); 325 | if (ret == -1) 326 | ALOGW("Error %s setting socket to listening state\n", strerror(errno)); 327 | 328 | /* Accept commands and send them to HAL */ 329 | while (1) { 330 | char buffer[526], cmsg_buffer[526]; 331 | struct sockaddr_un from; 332 | struct iovec recv_buff = { 333 | .iov_base = buffer, 334 | .iov_len = sizeof(buffer), 335 | }; 336 | struct msghdr msg = { 337 | .msg_name = &from, 338 | .msg_namelen = sizeof(from), 339 | .msg_iov = &recv_buff, 340 | .msg_iovlen = 1, 341 | .msg_control = cmsg_buffer, 342 | .msg_controllen = sizeof(cmsg_buffer), 343 | }; 344 | struct cmsghdr *cmsg; 345 | bool close_now = true; 346 | 347 | conn = accept(sock, NULL, NULL); 348 | if (conn == -1) { 349 | ALOGE("Error %s accepting connection\n", strerror(errno)); 350 | continue; 351 | } 352 | 353 | ret = recvmsg(conn, &msg, 0); 354 | if (ret == -1) { 355 | ALOGE("Error %s in receiving message, conn = %d\n", strerror(errno), conn); 356 | close(conn); 357 | 358 | continue; 359 | } 360 | 361 | if (!ret) 362 | continue; 363 | 364 | /* Check for shutdown from the peer */ 365 | if (ret == 0) 366 | break; 367 | 368 | for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; cmsg = CMSG_NXTHDR(&msg, cmsg)) { 369 | if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_RIGHTS) { 370 | int *received_fd = (int *)CMSG_DATA(cmsg); 371 | client = fdopen(*received_fd, "w"); 372 | break; 373 | } 374 | } 375 | 376 | ret = parse_cmd(buffer); 377 | if (ret < 0) { 378 | close(conn); 379 | continue; 380 | } 381 | if (ret == MONITOR_START_FLAG) { 382 | start_monitoring(conn, &flag); 383 | close_now = false; 384 | } else if (ret == MONITOR_STOP_FLAG) 385 | stop_monitoring(&flag); 386 | else 387 | flag = ACK_FLAG; 388 | 389 | /* Confirm succesfull dispatch */ 390 | write(conn, &flag, sizeof(flag)); 391 | 392 | if (close_now) { 393 | fclose(client); 394 | close(conn); 395 | } 396 | } 397 | } 398 | 399 | static const char *hal_paths[] ={ 400 | "./activity.gmin.so", 401 | "/lib/activity.gmin.so", 402 | "/system/lib/hw/activity.gmin.so", 403 | }; 404 | 405 | static int start_hal(void) 406 | { 407 | void *hal; 408 | int i, ret, no_paths; 409 | const char *path = NULL; 410 | pid_t child; 411 | 412 | no_paths = sizeof(hal_paths)/sizeof(const char*); 413 | for (i = 0; i < no_paths; i++) { 414 | if (access(hal_paths[i], R_OK) == 0) { 415 | path = hal_paths[i]; 416 | break; 417 | } 418 | } 419 | 420 | if (!path) { 421 | fprintf(stderr, "Unable to find HAL\n"); 422 | exit(1); 423 | } 424 | 425 | hal = dlopen(path, RTLD_NOW); 426 | if (!hal) { 427 | fprintf(stderr, "Error \"%s\" opening activity HAL\n", dlerror()); 428 | return HAL_OPEN_ERR; 429 | } 430 | 431 | hmi = dlsym(hal, HAL_MODULE_INFO_SYM_AS_STR); 432 | if (!hmi) { 433 | fprintf(stderr, "Error \"%s\" finding entry symbol\n", dlerror()); 434 | return HAL_SYMBOL_ERR; 435 | } 436 | 437 | printf("Activity HAL loaded: name %s vendor %s version %d.%d id %s\n", 438 | hmi->common.name, hmi->common.author, 439 | hmi->common.version_major, hmi->common.version_minor, 440 | hmi->common.id); 441 | 442 | /* Daemonize it */ 443 | child = fork(); 444 | if (child) { 445 | usleep(100); 446 | return 0; 447 | } 448 | 449 | if (setsid() == (pid_t)-1) { 450 | fprintf(stderr, "failed to send process to background\n"); 451 | exit(1); 452 | } 453 | 454 | /* Close stdio */ 455 | close(0); close(1); close(2); 456 | 457 | ALOGI("Proceeding to HAL initialization\n"); 458 | 459 | ret = hmi->common.methods->open((struct hw_module_t *) hmi, 460 | ACTIVITY_RECOGNITION_HARDWARE_INTERFACE, 461 | &device); 462 | if (ret) { 463 | ALOGE("Error %d occurred at HAL module opening\n", ret); 464 | exit(1); 465 | } 466 | 467 | start_server(); 468 | 469 | return 0; 470 | } 471 | 472 | static int send_cmd(int argc, char **argv) 473 | { 474 | char cmd[MAX_CMD_SIZE]; 475 | int i, sock, ret, flag; 476 | struct iovec rcv_buffer = { 477 | .iov_base = cmd, 478 | .iov_len = sizeof(cmd) + 1, 479 | }; 480 | struct cmsg_fd { 481 | struct cmsghdr hdr; 482 | int fd; 483 | } cmsg_buff = { 484 | .hdr = { 485 | .cmsg_level = SOL_SOCKET, 486 | .cmsg_type = SCM_RIGHTS, 487 | .cmsg_len = CMSG_LEN(sizeof(int)), 488 | }, 489 | .fd = 1, 490 | }; 491 | struct msghdr msg = { 492 | .msg_name = NULL, 493 | .msg_namelen = 0, 494 | .msg_iov = &rcv_buffer, 495 | .msg_iovlen = 1, 496 | .msg_control = &cmsg_buff, 497 | .msg_controllen = sizeof(cmsg_buff), 498 | }; 499 | 500 | strcpy(cmd, argv[1]); 501 | for (i = 2; i < argc; i++) { 502 | strncat(cmd, CMD_SEPARATOR, sizeof(CMD_SEPARATOR)); 503 | strcat(cmd, argv[i]); 504 | } 505 | 506 | sock = socket(AF_UNIX, SOCK_SEQPACKET, 0); 507 | if (sock == -1) { 508 | fprintf(stderr, "Error \"%s\" creating socket\n", strerror(errno)); 509 | return errno; 510 | } 511 | 512 | ret = connect(sock, (struct sockaddr *)&server_addr, sizeof(server_addr)); 513 | if (ret == -1) { 514 | fprintf(stderr, "Error \"%s\" connecting to server\n", strerror(errno)); 515 | return errno; 516 | } 517 | 518 | ret = sendmsg(sock, &msg, 0); 519 | if (ret == -1) { 520 | fprintf(stderr, "Error \"%s\" sending message to server\n", strerror(errno)); 521 | return errno; 522 | } 523 | 524 | ret = read(sock, &flag, sizeof(flag)); 525 | if (ret == -1) { 526 | fprintf(stderr, "Error \"%s\" getting answer from server\n", strerror(errno)); 527 | return errno; 528 | } 529 | 530 | /* Check for ACK or monitoring */ 531 | if (flag == MONITOR_START_FLAG) { 532 | do { 533 | ret = read(sock, &flag, sizeof(flag)); 534 | } while (ret > 0 && flag != ACK_FLAG); 535 | } else if (flag != ACK_FLAG) 536 | fprintf(stderr, "Error answer from HAL server: %d. Check logs for more details\n", flag); 537 | 538 | close(sock); 539 | 540 | return 0; 541 | } 542 | 543 | static int run_cmd(int argc, char **argv) 544 | { 545 | if (strncmp(argv[1], START_CMD, sizeof(START_CMD)) == 0) { 546 | if (argc == 2) 547 | return start_hal(); 548 | return print_usage(); 549 | } 550 | 551 | /* Send user command to HAL server socket */ 552 | return send_cmd(argc, argv); 553 | } 554 | 555 | int main(int argc, char **argv) 556 | { 557 | if (argc < 2) 558 | return print_usage(); 559 | 560 | return run_cmd(argc, argv); 561 | } 562 | -------------------------------------------------------------------------------- /sens.c: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | int usage(void) 31 | { 32 | fprintf(stderr, "sens start [sensors.gmin.so]\n"); 33 | fprintf(stderr, "sens [activate | deactivate] sensor_id\n"); 34 | fprintf(stderr, "sens set_delay sensor_id delay\n"); 35 | fprintf(stderr, "sens poll\n"); 36 | fprintf(stderr, "sens poll [duration] [number_of_events] \n"); 37 | fprintf(stderr, "sens poll_stop\n"); 38 | fprintf(stderr, "sens check_sample_rate [rate] \n"); 39 | return 1; 40 | } 41 | 42 | static struct sensors_module_t *hmi; 43 | 44 | static const char* types[] = { 45 | "metadata", 46 | "accelerometer", 47 | "magnetometer", 48 | "orientation", 49 | "gyroscope", 50 | "light", 51 | "pressure", 52 | "temperature", 53 | "proximity", 54 | "gravity", 55 | "linear acceleration", 56 | "rotation vector", 57 | "relative humitidy", 58 | "ambient temperature", 59 | "uncalibrated magnetometer", 60 | "game rotation vector", 61 | "uncalibrated gyrocope", 62 | "significant motion", 63 | "step detector", 64 | "step counter", 65 | "geomagnetic rotation vector", 66 | }; 67 | 68 | static const char *type_str(int type) 69 | { 70 | int type_count = sizeof(types)/sizeof(char *); 71 | 72 | if (type < 0 || type >= type_count) 73 | return "unknown"; 74 | return types[type]; 75 | } 76 | 77 | 78 | static struct sensors_module_t *hmi; 79 | static struct hw_device_t *dev; 80 | static FILE *client; 81 | static pthread_mutex_t client_mutex = PTHREAD_MUTEX_INITIALIZER; 82 | static pthread_cond_t cond = PTHREAD_COND_INITIALIZER; 83 | static int ready_to_close = 0; 84 | static int number_of_events = 0; 85 | static int non_param_poll = 1; 86 | static int event_no = 0; 87 | static int init_events = 0; 88 | static int print_events = 1; 89 | static long long timestamp = 0; 90 | static long long event_init_poll_time = 0; 91 | static long long poll_duration = 0; 92 | 93 | static void print_event(struct sensors_event_t *e) 94 | { 95 | FILE *f; 96 | 97 | pthread_mutex_lock(&client_mutex); 98 | if (!client) { 99 | pthread_mutex_unlock(&client_mutex); 100 | return; 101 | } 102 | f = client; 103 | 104 | fprintf(f, "event %d: version=%d sensor=%d type=%s timestamp=%lld\n",event_no, 105 | e->version, e->sensor, type_str(e->type), (long long)e->timestamp); 106 | if (poll_duration != 0) 107 | fprintf(f,"Time remaining:%lld \n",poll_duration - ((long long)e->timestamp 108 | - event_init_poll_time)); 109 | switch (e->type) { 110 | case SENSOR_TYPE_META_DATA: 111 | break; 112 | case SENSOR_TYPE_ACCELEROMETER: 113 | case SENSOR_TYPE_LINEAR_ACCELERATION: 114 | case SENSOR_TYPE_GRAVITY: 115 | fprintf(f, "event: x=%10.2f y=%10.2f z=%10.2f status=%d\n", 116 | e->acceleration.x, e->acceleration.y, e->acceleration.z, 117 | e->acceleration.status); 118 | break; 119 | case SENSOR_TYPE_MAGNETIC_FIELD: 120 | fprintf(f, "event: x=%10.2f y=%10.2f z=%10.2f status=%d\n", 121 | e->magnetic.x, e->magnetic.y, e->magnetic.z, 122 | e->magnetic.status); 123 | break; 124 | case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED: 125 | fprintf(f, "event: x=%10.2f y=%10.2f z=%10.2f bias_x=%10.2f bias_y=%10.2f bias_z=%10.2f \n", 126 | e->uncalibrated_magnetic.x_uncalib, 127 | e->uncalibrated_magnetic.y_uncalib, 128 | e->uncalibrated_magnetic.z_uncalib, 129 | e->uncalibrated_magnetic.x_bias, 130 | e->uncalibrated_magnetic.y_bias, 131 | e->uncalibrated_magnetic.z_bias); 132 | break; 133 | case SENSOR_TYPE_ORIENTATION: 134 | fprintf(f, "event: azimuth=%10.2f pitch=%10.2f roll=%10.2f status=%d\n", 135 | e->orientation.azimuth, e->orientation.pitch, e->orientation.roll, 136 | e->orientation.status); 137 | break; 138 | case SENSOR_TYPE_GYROSCOPE: 139 | fprintf(f, "event: x=%10.2f y=%10.2f z=%10.2f status=%d\n", 140 | e->gyro.x, e->gyro.y, e->gyro.z, e->gyro.status); 141 | break; 142 | case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED: 143 | fprintf(f, "event: x=%10.2f y=%10.2f z=%10.2f bias_x=%10.2f bias_y=%10.2f bias_z=%10.2f \n", 144 | e->uncalibrated_gyro.x_uncalib, 145 | e->uncalibrated_gyro.y_uncalib, 146 | e->uncalibrated_gyro.z_uncalib, 147 | e->uncalibrated_gyro.x_bias, 148 | e->uncalibrated_gyro.y_bias, 149 | e->uncalibrated_gyro.z_bias); 150 | break; 151 | case SENSOR_TYPE_LIGHT: 152 | fprintf(f, "event: light=%10.2f\n", e->light); 153 | break; 154 | case SENSOR_TYPE_PRESSURE: 155 | fprintf(f, "event: pressure=%10.2f\n", e->pressure); 156 | break; 157 | case SENSOR_TYPE_TEMPERATURE: 158 | case SENSOR_TYPE_AMBIENT_TEMPERATURE: 159 | fprintf(f, "event: temperature=%10.2f\n", e->temperature); 160 | break; 161 | case SENSOR_TYPE_PROXIMITY: 162 | fprintf(f, "event: distance=%10.2f\n", e->distance); 163 | break; 164 | case SENSOR_TYPE_ROTATION_VECTOR: 165 | case SENSOR_TYPE_GAME_ROTATION_VECTOR: 166 | case SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR: 167 | fprintf(f, "event: rot_x=%10.2f rot_y=%10.2f rot_z=%10.2f cos=%10.2f estimated_accuracy=%10.2f\n", 168 | e->data[0], e->data[1], e->data[2], e->data[3], e->data[4]); 169 | break; 170 | case SENSOR_TYPE_RELATIVE_HUMIDITY: 171 | fprintf(f, "event: humidity=%10.2f\n", e->relative_humidity); 172 | break; 173 | case SENSOR_TYPE_SIGNIFICANT_MOTION: 174 | fprintf(f, "event: significant_motion=%10.2f\n", e->data[0]); 175 | break; 176 | case SENSOR_TYPE_STEP_DETECTOR: 177 | fprintf(f, "event: step_detector=%10.2f\n", e->data[0]); 178 | break; 179 | case SENSOR_TYPE_STEP_COUNTER: 180 | fprintf(f, "event: step_counter=%llu\n", 181 | (unsigned long long)e->u64.step_counter); 182 | break; 183 | } 184 | fprintf(f, "\n"); 185 | fflush(f); 186 | 187 | pthread_mutex_unlock(&client_mutex); 188 | } 189 | 190 | static void print_result(int result) 191 | { 192 | FILE *f; 193 | pthread_mutex_lock(&client_mutex); 194 | if (!client) { 195 | pthread_mutex_unlock(&client_mutex); 196 | return; 197 | } 198 | f = client; 199 | fprintf(f, "Number of events: %d \n", event_no - init_events); 200 | fprintf(f, "Duration: %lld \n\n", (long long) timestamp - event_init_poll_time); 201 | if(!print_events){ 202 | if(result) 203 | fprintf(f, "Test passed\n\n"); 204 | else 205 | fprintf(f, "Test failed\n\n"); 206 | } 207 | fflush(f); 208 | pthread_mutex_unlock(&client_mutex); 209 | 210 | } 211 | 212 | static void process_event(struct sensors_event_t *e) 213 | { 214 | int is_poll_duration_over = 0; 215 | int is_event_number_reached = 0; 216 | 217 | if (event_init_poll_time == 0) { 218 | event_init_poll_time = (long long) e->timestamp; 219 | init_events = event_no; 220 | } 221 | is_poll_duration_over = (long long) e->timestamp - event_init_poll_time <= poll_duration ? 0 : 1; 222 | is_event_number_reached = (event_no - init_events) < number_of_events ? 0 : 1; 223 | 224 | if ((!is_poll_duration_over && !is_event_number_reached) || non_param_poll) 225 | { 226 | timestamp = e -> timestamp; 227 | event_no++; 228 | if(print_events) 229 | print_event(e); 230 | } else { 231 | ready_to_close = 1; 232 | print_result(is_event_number_reached); 233 | pthread_cond_signal(&cond); 234 | } 235 | } 236 | 237 | static void run_sensors_poll_v0(void) 238 | { 239 | struct sensors_poll_device_t *poll_dev = (struct sensors_poll_device_t *)dev; 240 | 241 | while (1) { 242 | sensors_event_t events[256]; 243 | int i, count; 244 | 245 | count = poll_dev->poll(poll_dev, events, sizeof(events)/sizeof(sensors_event_t)); 246 | 247 | for(i = 0; i < count; i++) 248 | process_event(&events[i]); 249 | } 250 | } 251 | 252 | static void sig_pipe(int sig) 253 | { 254 | client = NULL; 255 | } 256 | 257 | static void *run_sensors_thread(void *arg __attribute((unused))) 258 | { 259 | 260 | signal(SIGPIPE, sig_pipe); 261 | 262 | switch (dev->version) { 263 | case SENSORS_DEVICE_API_VERSION_0_1: 264 | default: 265 | run_sensors_poll_v0(); 266 | break; 267 | } 268 | 269 | return NULL; 270 | } 271 | 272 | void print_sensor(const struct sensor_t *s, FILE *f) 273 | { 274 | if (!f) 275 | return; 276 | 277 | fprintf(f, "sensor%d: name=%s vendor=%s version=%d type=%s\n", 278 | s->handle, s->name, s->vendor, s->version, type_str(s->type)); 279 | fprintf(f, "sensor%d: maxRange=%10.2f resolution=%10.2f power=%10.2f\n", 280 | s->handle, s->maxRange, s->resolution, s->power); 281 | fprintf(f, "sensor%d: minDelay=%d fifoReservedEventCount=%d fifoMaxEventCount=%d\n", 282 | s->handle, s->minDelay, s->fifoReservedEventCount, 283 | s->fifoMaxEventCount); 284 | 285 | } 286 | 287 | static int sensor_set_delay(int handle, int64_t delay) 288 | { 289 | switch (dev->version) { 290 | default: 291 | case SENSORS_DEVICE_API_VERSION_0_1: 292 | { 293 | struct sensors_poll_device_t *poll_dev = (struct sensors_poll_device_t *)dev; 294 | 295 | return poll_dev->setDelay(poll_dev, handle, delay); 296 | } 297 | } 298 | } 299 | 300 | 301 | static int sensor_activate(int handle, int enable) 302 | { 303 | switch (dev->version) { 304 | default: 305 | case SENSORS_DEVICE_API_VERSION_0_1: 306 | { 307 | struct sensors_poll_device_t *poll_dev = (struct sensors_poll_device_t *)dev; 308 | 309 | return poll_dev->activate(poll_dev, handle, enable); 310 | } 311 | } 312 | } 313 | 314 | #define CLIENT_ERR(f, fmt...) \ 315 | { if (f) { fprintf(f, fmt); fprintf(f, "\n"); } ALOGE(fmt); } 316 | 317 | static int dispatch_cmd(char *cmd, FILE *f) 318 | { 319 | char *argv[16], *tmp; 320 | int argc = 0, handle; 321 | 322 | tmp = strtok(cmd, " "); 323 | while (tmp) { 324 | argv[argc++] = tmp; 325 | tmp = strtok(NULL, " "); 326 | } 327 | if (!argc) 328 | argv[argc++] = tmp; 329 | 330 | if (argc < 1) { 331 | CLIENT_ERR(f, "invalid cmd: %s", cmd); 332 | return -1; 333 | } 334 | 335 | if (!strcmp(argv[0], "ls")) { 336 | struct sensor_t const* list; 337 | int i, count = hmi->get_sensors_list(hmi, &list); 338 | 339 | for(i = 0; i < count; i++) 340 | print_sensor(&list[i], f);; 341 | 342 | return 0; 343 | } else if (!strcmp(argv[0], "activate")) { 344 | 345 | if (argc < 2) { 346 | CLIENT_ERR(f, "activate: no sensor handle"); 347 | return -1; 348 | } 349 | 350 | handle = atoi(argv[1]); 351 | 352 | return sensor_activate(handle, 1); 353 | 354 | } else if (!strcmp(argv[0], "deactivate")) { 355 | 356 | if (argc < 2) { 357 | CLIENT_ERR(f, "activate: no sensor handle"); 358 | return -1; 359 | } 360 | 361 | handle = atoi(argv[1]); 362 | 363 | return sensor_activate(handle, 0); 364 | 365 | } else if (!strcmp(argv[0], "set_delay")) { 366 | int64_t delay; 367 | 368 | if (argc < 3) { 369 | CLIENT_ERR(f, "setDelay: no sensor handle and/or delay"); 370 | return -1; 371 | } 372 | 373 | handle=atoi(argv[1]); 374 | delay=atoll(argv[2]); 375 | 376 | return sensor_set_delay(handle, delay); 377 | 378 | } else if (!strcmp(argv[0], "poll")) { 379 | if (argc == 1) { 380 | non_param_poll = 1; 381 | } else if (argc == 3) { 382 | non_param_poll = 0; 383 | poll_duration = atoll(argv[1]); 384 | number_of_events = atoi(argv[2]); 385 | event_init_poll_time = 0; 386 | ready_to_close = 0; 387 | } else { 388 | CLIENT_ERR(f, "poll: no poll duration or number of events set"); 389 | return -1; 390 | } 391 | print_events = 1; 392 | pthread_mutex_lock(&client_mutex); 393 | if (client) 394 | fclose(client); 395 | client = f; 396 | 397 | if (!non_param_poll) { 398 | pthread_cond_wait(&cond, &client_mutex); 399 | fclose(client); 400 | client = NULL; 401 | } 402 | 403 | pthread_mutex_unlock(&client_mutex); 404 | 405 | return 1; 406 | } else if (!strcmp(argv[0], "check_sample_rate")) { 407 | 408 | if (argc < 2) { 409 | CLIENT_ERR(f, "check_sample_rate: no events rate"); 410 | return -1; 411 | } 412 | 413 | non_param_poll = 0; 414 | poll_duration = 1000000000; 415 | number_of_events = atoi(argv[1]); 416 | event_init_poll_time = 0; 417 | ready_to_close = 0; 418 | print_events = 0; 419 | 420 | pthread_mutex_lock(&client_mutex); 421 | if (client) 422 | fclose(client); 423 | client = f; 424 | pthread_cond_wait(&cond, &client_mutex); 425 | fclose(client); 426 | client = NULL; 427 | pthread_mutex_unlock(&client_mutex); 428 | return 1; 429 | } else if (!strcmp(argv[0], "poll_stop")) { 430 | pthread_mutex_lock(&client_mutex); 431 | if (client){ 432 | fclose(client); 433 | client = NULL; 434 | } 435 | pthread_mutex_unlock(&client_mutex); 436 | 437 | return 1; 438 | } else if (!strcmp(argv[0], "stop")) { 439 | exit(1); 440 | } else { 441 | CLIENT_ERR(f, "invalid command: %s", cmd); 442 | return -1; 443 | } 444 | 445 | } 446 | 447 | #ifdef ANDROID 448 | #define NAME_PREFIX "/dev/socket/" 449 | #else 450 | #define NAME_PREFIX "/tmp/" 451 | #endif 452 | 453 | #define SENS_SERVER_NAME NAME_PREFIX "sens-server" 454 | 455 | struct sockaddr_un server_addr = { 456 | .sun_family = AF_UNIX, 457 | .sun_path = SENS_SERVER_NAME, 458 | }; 459 | 460 | static int start_server(void) 461 | { 462 | int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0), conn; 463 | int err; 464 | 465 | unlink(SENS_SERVER_NAME); 466 | 467 | if (sock < 0) { 468 | ALOGE("failed to create socket: %s", strerror(errno)); 469 | exit(1); 470 | } 471 | 472 | err = bind(sock, (struct sockaddr *)&server_addr, sizeof(server_addr)); 473 | if (err) { 474 | ALOGE("failed to bind socket: %s", strerror(errno)); 475 | exit(1); 476 | } 477 | 478 | listen(sock, 1); 479 | 480 | while (1) { 481 | char data_buff[1024], cmsg_buffer[1024]; 482 | struct iovec recv_buff = { 483 | .iov_base = data_buff, 484 | .iov_len = sizeof(data_buff), 485 | }; 486 | struct sockaddr_un from; 487 | struct msghdr msg = { 488 | .msg_name = &from, 489 | .msg_namelen = sizeof(from), 490 | .msg_iov = &recv_buff, 491 | .msg_iovlen = 1, 492 | .msg_control = cmsg_buffer, 493 | .msg_controllen = sizeof(cmsg_buffer), 494 | }; 495 | FILE *f =NULL; 496 | struct cmsghdr *cmsg; 497 | 498 | conn = accept(sock, NULL, NULL); 499 | if (conn < 0) { 500 | ALOGE("failed to accept connection: %s", strerror(errno)); 501 | continue; 502 | } 503 | 504 | err = recvmsg(conn, &msg, 0); 505 | if (err < 0) { 506 | ALOGE("error in recvmsg: %s", strerror(errno)); 507 | close(conn); 508 | continue; 509 | } 510 | 511 | if (err == 0) 512 | continue; 513 | 514 | for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; 515 | cmsg = CMSG_NXTHDR(&msg,cmsg)) { 516 | if (cmsg->cmsg_level == SOL_SOCKET 517 | && cmsg->cmsg_type == SCM_RIGHTS) { 518 | int *fd = (int *)CMSG_DATA(cmsg); 519 | f = fdopen(*fd, "w"); 520 | break; 521 | } 522 | } 523 | 524 | if (data_buff[err - 1] != 0) { 525 | ALOGE("command is not NULL terminated\n"); 526 | close(conn); 527 | continue; 528 | } 529 | 530 | err = dispatch_cmd(data_buff, f); 531 | if (err < 0) { 532 | ALOGE("error dispatching command: %d", err); 533 | close(conn); 534 | continue; 535 | } 536 | 537 | /* send ack */ 538 | if (!err) { 539 | write(conn, data_buff, 1); 540 | fclose(f); 541 | } 542 | 543 | close(conn); 544 | } 545 | } 546 | 547 | static const char *hal_paths[] = { 548 | "/system/lib/hw/sensors.gmin.so", 549 | "sensors.gmin.so", 550 | "/lib/sensors.gmin.so", 551 | }; 552 | 553 | static int start_hal(int argc, char **argv) 554 | { 555 | void *hal; 556 | pid_t child; 557 | int err; 558 | pthread_t sensors_thread; 559 | const char *hal_path = NULL; 560 | 561 | if (argc == 2) { 562 | unsigned i; 563 | 564 | for(i = 0; i < sizeof(hal_paths)/sizeof(const char*); i++) { 565 | if (!access(hal_paths[i], R_OK)) { 566 | hal_path = hal_paths[i]; 567 | break; 568 | } 569 | } 570 | 571 | if (!hal_path) { 572 | fprintf(stderr, "unable to find HAL\n"); 573 | exit(1); 574 | } 575 | } else 576 | hal_path = argv[2]; 577 | 578 | hal = dlopen(hal_path, RTLD_NOW); 579 | if (!hal) { 580 | fprintf(stderr, "unable to load HAL %s: %s\n", hal_path, 581 | dlerror()); 582 | return 2; 583 | } 584 | 585 | hmi = dlsym(hal, HAL_MODULE_INFO_SYM_AS_STR); 586 | if (!hmi) { 587 | fprintf(stderr, "unable to find %s entry point in HAL\n", 588 | HAL_MODULE_INFO_SYM_AS_STR); 589 | return 3; 590 | } 591 | 592 | printf("HAL loaded: name %s vendor %s version %d.%d id %s\n", 593 | hmi->common.name, hmi->common.author, 594 | hmi->common.version_major, hmi->common.version_minor, 595 | hmi->common.id); 596 | 597 | child = fork(); 598 | if (child) { 599 | usleep(100); 600 | return 0; 601 | } 602 | 603 | if (setsid() == (pid_t)-1) { 604 | fprintf(stderr, "failed to send process to background\n"); 605 | exit(1); 606 | } 607 | 608 | close(0); close(1); close(2); 609 | 610 | ALOGI("Initializing HAL"); 611 | 612 | err = hmi->common.methods->open((struct hw_module_t *)hmi, 613 | SENSORS_HARDWARE_POLL, &dev); 614 | 615 | if (err) { 616 | ALOGE("failed to initialize HAL: %d\n", err); 617 | exit(1); 618 | } 619 | 620 | if (pthread_create(&sensors_thread, NULL, run_sensors_thread, NULL)) { 621 | ALOGE("failed to create sensor thread"); 622 | exit(1); 623 | } 624 | 625 | return start_server(); 626 | } 627 | 628 | int main(int argc, char **argv) 629 | { 630 | char cmd[1024]; 631 | int sock, i; 632 | struct iovec buff = { 633 | .iov_base = cmd, 634 | }; 635 | struct cmsg_fd { 636 | struct cmsghdr hdr; 637 | int fd; 638 | } cmsg_buff = { 639 | .hdr = { 640 | .cmsg_level = SOL_SOCKET, 641 | .cmsg_type = SCM_RIGHTS, 642 | .cmsg_len = CMSG_LEN(sizeof(int)), 643 | }, 644 | .fd = 1, 645 | }; 646 | struct msghdr msg = { 647 | .msg_name = NULL, 648 | .msg_namelen = 0, 649 | .msg_iov = &buff, 650 | .msg_iovlen = 1, 651 | .msg_control = &cmsg_buff, 652 | .msg_controllen = sizeof(cmsg_buff), 653 | }; 654 | 655 | 656 | if (argc < 2) { 657 | usage(); 658 | return 1; 659 | } 660 | 661 | if (!strcmp(argv[1], "start")) { 662 | if (argc < 2) 663 | return usage(); 664 | 665 | return start_hal(argc, argv); 666 | } 667 | 668 | if (strlen(argv[1]) >= sizeof(cmd)) 669 | return usage(); 670 | strncpy(cmd, argv[1], sizeof(cmd) - 1); 671 | strncat(cmd, " ", sizeof(cmd) - strlen(cmd) - 1); 672 | for(i = 2; i < argc; i++) { 673 | strncat(cmd, argv[i], sizeof(cmd) - strlen(cmd) - 1); 674 | strncat(cmd, " ", sizeof(cmd) - strlen(cmd) - 1); 675 | } 676 | 677 | sock = socket(AF_UNIX, SOCK_SEQPACKET, 0); 678 | if (!sock) { 679 | fprintf(stderr, "failed to create socket: %s\n", strerror(errno)); 680 | return 3; 681 | } 682 | 683 | if (connect(sock, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) { 684 | fprintf(stderr, "failed to connect to server: %s\n", strerror(errno)); 685 | return 5; 686 | } 687 | 688 | buff.iov_len = strlen(cmd) + 1; 689 | if (sendmsg(sock, &msg, 0) < 0) { 690 | fprintf(stderr, "failed sending command to server: %s\n", strerror(errno)); 691 | return 6; 692 | } 693 | 694 | buff.iov_len = sizeof(cmd); 695 | if (read(sock, cmd, 1) < 0) { 696 | fprintf(stderr, "failed getting ack from server: %s\n", strerror(errno)); 697 | return 7; 698 | } 699 | 700 | close(sock); 701 | 702 | return 0; 703 | } 704 | -------------------------------------------------------------------------------- /compass-calibration.c: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include "calibration.h" 22 | #include "matrix-ops.h" 23 | #include "description.h" 24 | 25 | 26 | /* Compass defines */ 27 | #define COMPASS_CALIBRATION_PATH "/data/compass.conf" 28 | #define EPSILON 0.000000001 29 | 30 | #define MAGNETIC_LOW 960 /* 31 micro tesla squared */ 31 | #define CAL_STEPS 5 32 | #define CAL_VERSION 1.0 33 | 34 | /* We'll have multiple calibration levels so that we can provide an estimation as fast as possible */ 35 | static const float min_diffs [CAL_STEPS] = {0.2, 0.25, 0.4, 0.6, 1.0}; 36 | static const float max_sqr_errs [CAL_STEPS] = {10.0, 10.0, 8.0, 5.0, 3.5}; 37 | static const unsigned int lookback_counts [CAL_STEPS] = {2, 3, 4, 5, 6 }; 38 | 39 | 40 | /* Reset calibration algorithm */ 41 | static void reset_sample (compass_cal_t* data) 42 | { 43 | int i,j; 44 | data->sample_count = 0; 45 | for (i = 0; i < MAGN_DS_SIZE; i++) 46 | for (j=0; j < 3; j++) 47 | data->sample[i][j] = 0; 48 | 49 | data->average[0] = data->average[1] = data->average[2] = 0; 50 | } 51 | 52 | 53 | static double calc_square_err (compass_cal_t* data) 54 | { 55 | double err = 0; 56 | double raw[3][1], result[3][1], mat_diff[3][1]; 57 | int i; 58 | float stdev[3] = {0,0,0}; 59 | double diff; 60 | 61 | for (i = 0; i < MAGN_DS_SIZE; i++) { 62 | raw[0][0] = data->sample[i][0]; 63 | raw[1][0] = data->sample[i][1]; 64 | raw[2][0] = data->sample[i][2]; 65 | 66 | stdev[0] += (raw[0][0] - data->average[0]) * (raw[0][0] - data->average[0]); 67 | stdev[1] += (raw[1][0] - data->average[1]) * (raw[1][0] - data->average[1]); 68 | stdev[2] += (raw[2][0] - data->average[2]) * (raw[2][0] - data->average[2]); 69 | 70 | substract (3, 1, raw, data->offset, mat_diff); 71 | multiply(3, 3, 1, data->w_invert, mat_diff, result); 72 | 73 | diff = sqrt(result[0][0] * result[0][0] + result[1][0] * result[1][0] + result[2][0] * result[2][0]) - data->bfield; 74 | 75 | err += diff * diff; 76 | } 77 | 78 | stdev[0] = sqrt(stdev[0] / MAGN_DS_SIZE); 79 | stdev[1] = sqrt(stdev[1] / MAGN_DS_SIZE); 80 | stdev[2] = sqrt(stdev[2] / MAGN_DS_SIZE); 81 | 82 | /* A sanity check - if we have too little variation for an axis it's best to reject the calibration than risking a wrong calibration */ 83 | if (stdev[0] <= 1 || stdev[1] <= 1 || stdev[2] <= 1) 84 | return max_sqr_errs[0]; 85 | 86 | err /= MAGN_DS_SIZE; 87 | return err; 88 | } 89 | 90 | 91 | /* Given an real symmetric 3x3 matrix A, compute the eigenvalues */ 92 | static void compute_eigenvalues (double mat[3][3], double* eig1, double* eig2, double* eig3) 93 | { 94 | double p = mat[0][1] * mat[0][1] + mat[0][2] * mat[0][2] + mat[1][2] * mat[1][2]; 95 | 96 | if (p < EPSILON) { 97 | *eig1 = mat[0][0]; 98 | *eig2 = mat[1][1]; 99 | *eig3 = mat[2][2]; 100 | return; 101 | } 102 | 103 | double q = (mat[0][0] + mat[1][1] + mat[2][2]) / 3; 104 | double temp1 = mat[0][0] - q; 105 | double temp2 = mat[1][1] - q; 106 | double temp3 = mat[2][2] - q; 107 | 108 | p = temp1 * temp1 + temp2 * temp2 + temp3 * temp3 + 2 * p; 109 | p = sqrt(p / 6); 110 | 111 | double mat2[3][3]; 112 | assign(3, 3, mat, mat2); 113 | mat2[0][0] -= q; 114 | mat2[1][1] -= q; 115 | mat2[2][2] -= q; 116 | multiply_scalar_inplace(3, 3, mat2, 1/p); 117 | 118 | double r = (mat2[0][0] * mat2[1][1] * mat2[2][2] + mat2[0][1] * mat2[1][2] * mat2[2][0] 119 | + mat2[0][2] * mat2[1][0] * mat2[2][1] - mat2[0][2] * mat2[1][1] * mat2[2][0] 120 | - mat2[0][0] * mat2[1][2] * mat2[2][1] - mat2[0][1] * mat2[1][0] * mat2[2][2]) / 2; 121 | 122 | double phi; 123 | if (r <= -1.0) 124 | phi = M_PI/3; 125 | else if (r >= 1.0) 126 | phi = 0; 127 | else 128 | phi = acos(r) / 3; 129 | 130 | *eig3 = q + 2 * p * cos(phi); 131 | *eig1 = q + 2 * p * cos(phi + 2 * M_PI / 3); 132 | *eig2 = 3 * q - *eig1 - *eig3; 133 | } 134 | 135 | 136 | static void calc_evector (double mat[3][3], double eig, double vec[3][1]) 137 | { 138 | double h[3][3]; 139 | double x_tmp[2][2]; 140 | assign(3, 3, mat, h); 141 | h[0][0] -= eig; 142 | h[1][1] -= eig; 143 | h[2][2] -= eig; 144 | 145 | double x[2][2]; 146 | x[0][0] = h[1][1]; 147 | x[0][1] = h[1][2]; 148 | x[1][0] = h[2][1]; 149 | x[1][1] = h[2][2]; 150 | invert(2, x, x_tmp); 151 | assign(2, 2, x_tmp, x); 152 | 153 | double temp1 = x[0][0] * (-h[1][0]) + x[0][1] * (-h[2][0]); 154 | double temp2 = x[1][0] * (-h[1][0]) + x[1][1] * (-h[2][0]); 155 | double norm = sqrt(1 + temp1 * temp1 + temp2 * temp2); 156 | 157 | vec[0][0] = 1.0 / norm; 158 | vec[1][0] = temp1 / norm; 159 | vec[2][0] = temp2 / norm; 160 | } 161 | 162 | 163 | static int ellipsoid_fit (mat_input_t m, double offset[3][1], double w_invert[3][3], double* bfield) 164 | { 165 | int i; 166 | double h[MAGN_DS_SIZE][9]; 167 | double w[MAGN_DS_SIZE][1]; 168 | double h_trans[9][MAGN_DS_SIZE]; 169 | double p_temp1[9][9]; 170 | double p_temp2[9][MAGN_DS_SIZE]; 171 | double temp1[3][3], temp[3][3]; 172 | double temp1_inv[3][3]; 173 | double temp2[3][1]; 174 | double result[9][9]; 175 | double p[9][1]; 176 | double a[3][3], sqrt_evals[3][3], evecs[3][3], evecs_trans[3][3]; 177 | double evec1[3][1], evec2[3][1], evec3[3][1]; 178 | 179 | for (i = 0; i < MAGN_DS_SIZE; i++) { 180 | w[i][0] = m[i][0] * m[i][0]; 181 | h[i][0] = m[i][0]; 182 | h[i][1] = m[i][1]; 183 | h[i][2] = m[i][2]; 184 | h[i][3] = -1 * m[i][0] * m[i][1]; 185 | h[i][4] = -1 * m[i][0] * m[i][2]; 186 | h[i][5] = -1 * m[i][1] * m[i][2]; 187 | h[i][6] = -1 * m[i][1] * m[i][1]; 188 | h[i][7] = -1 * m[i][2] * m[i][2]; 189 | h[i][8] = 1; 190 | } 191 | 192 | transpose (MAGN_DS_SIZE, 9, h, h_trans); 193 | multiply (9, MAGN_DS_SIZE, 9, h_trans, h, result); 194 | invert (9, result, p_temp1); 195 | multiply (9, 9, MAGN_DS_SIZE, p_temp1, h_trans, p_temp2); 196 | multiply (9, MAGN_DS_SIZE, 1, p_temp2, w, p); 197 | 198 | temp1[0][0] = 2; 199 | temp1[0][1] = p[3][0]; 200 | temp1[0][2] = p[4][0]; 201 | temp1[1][0] = p[3][0]; 202 | temp1[1][1] = 2 * p[6][0]; 203 | temp1[1][2] = p[5][0]; 204 | temp1[2][0] = p[4][0]; 205 | temp1[2][1] = p[5][0]; 206 | temp1[2][2] = 2 * p[7][0]; 207 | 208 | temp2[0][0] = p[0][0]; 209 | temp2[1][0] = p[1][0]; 210 | temp2[2][0] = p[2][0]; 211 | 212 | invert(3, temp1, temp1_inv); 213 | multiply(3, 3, 1, temp1_inv, temp2, offset); 214 | double off_x = offset[0][0]; 215 | double off_y = offset[1][0]; 216 | double off_z = offset[2][0]; 217 | 218 | a[0][0] = 1.0 / (p[8][0] + off_x * off_x + p[6][0] * off_y * off_y 219 | + p[7][0] * off_z * off_z + p[3][0] * off_x * off_y 220 | + p[4][0] * off_x * off_z + p[5][0] * off_y * off_z); 221 | 222 | a[0][1] = p[3][0] * a[0][0] / 2; 223 | a[0][2] = p[4][0] * a[0][0] / 2; 224 | a[1][2] = p[5][0] * a[0][0] / 2; 225 | a[1][1] = p[6][0] * a[0][0]; 226 | a[2][2] = p[7][0] * a[0][0]; 227 | a[2][1] = a[1][2]; 228 | a[1][0] = a[0][1]; 229 | a[2][0] = a[0][2]; 230 | 231 | double eig1 = 0, eig2 = 0, eig3 = 0; 232 | compute_eigenvalues(a, &eig1, &eig2, &eig3); 233 | 234 | if (eig1 <=0 || eig2 <= 0 || eig3 <= 0) 235 | return 0; 236 | 237 | sqrt_evals[0][0] = sqrt(eig1); 238 | sqrt_evals[1][0] = 0; 239 | sqrt_evals[2][0] = 0; 240 | sqrt_evals[0][1] = 0; 241 | sqrt_evals[1][1] = sqrt(eig2); 242 | sqrt_evals[2][1] = 0; 243 | sqrt_evals[0][2] = 0; 244 | sqrt_evals[1][2] = 0; 245 | sqrt_evals[2][2] = sqrt(eig3); 246 | 247 | calc_evector(a, eig1, evec1); 248 | calc_evector(a, eig2, evec2); 249 | calc_evector(a, eig3, evec3); 250 | 251 | evecs[0][0] = evec1[0][0]; 252 | evecs[1][0] = evec1[1][0]; 253 | evecs[2][0] = evec1[2][0]; 254 | evecs[0][1] = evec2[0][0]; 255 | evecs[1][1] = evec2[1][0]; 256 | evecs[2][1] = evec2[2][0]; 257 | evecs[0][2] = evec3[0][0]; 258 | evecs[1][2] = evec3[1][0]; 259 | evecs[2][2] = evec3[2][0]; 260 | 261 | multiply (3, 3, 3, evecs, sqrt_evals, temp1); 262 | transpose(3, 3, evecs, evecs_trans); 263 | multiply (3, 3, 3, temp1, evecs_trans, temp); 264 | transpose (3, 3, temp, w_invert); 265 | 266 | *bfield = pow(sqrt(1/eig1) * sqrt(1/eig2) * sqrt(1/eig3), 1.0/3.0); 267 | 268 | if (*bfield < 0) 269 | return 0; 270 | 271 | multiply_scalar_inplace(3, 3, w_invert, *bfield); 272 | 273 | return 1; 274 | } 275 | 276 | 277 | static void compass_cal_init (FILE* data_file, sensor_info_t* info) 278 | { 279 | compass_cal_t* cal_data = (compass_cal_t*) info->cal_data; 280 | int cal_steps = (info->max_cal_level && info->max_cal_level <= CAL_STEPS) ? info->max_cal_level : CAL_STEPS; 281 | float version; 282 | 283 | if (cal_data == NULL) 284 | return; 285 | 286 | int data_count = 15; 287 | reset_sample(cal_data); 288 | 289 | if (!info->cal_level && data_file != NULL) { 290 | int ret = fscanf(data_file, "%f %d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", 291 | &version, &info->cal_level, 292 | &cal_data->offset[0][0], &cal_data->offset[1][0], &cal_data->offset[2][0], 293 | &cal_data->w_invert[0][0], &cal_data->w_invert[0][1], &cal_data->w_invert[0][2], 294 | &cal_data->w_invert[1][0], &cal_data->w_invert[1][1], &cal_data->w_invert[1][2], 295 | &cal_data->w_invert[2][0], &cal_data->w_invert[2][1], &cal_data->w_invert[2][2], 296 | &cal_data->bfield); 297 | 298 | if (ret != data_count || info->cal_level >= cal_steps || version != CAL_VERSION) 299 | info->cal_level = 0; 300 | } 301 | 302 | if (info->cal_level) { 303 | ALOGV("CompassCalibration: load old data, caldata: %f %f %f %f %f %f %f %f %f %f %f %f %f", 304 | cal_data->offset[0][0], cal_data->offset[1][0], cal_data->offset[2][0], 305 | cal_data->w_invert[0][0], cal_data->w_invert[0][1], cal_data->w_invert[0][2], cal_data->w_invert[1][0], 306 | cal_data->w_invert[1][1], cal_data->w_invert[1][2], cal_data->w_invert[2][0], cal_data->w_invert[2][1], 307 | cal_data->w_invert[2][2], cal_data->bfield); 308 | } else { 309 | cal_data->offset[0][0] = 0; 310 | cal_data->offset[1][0] = 0; 311 | cal_data->offset[2][0] = 0; 312 | 313 | cal_data->w_invert[0][0] = 1; 314 | cal_data->w_invert[1][0] = 0; 315 | cal_data->w_invert[2][0] = 0; 316 | cal_data->w_invert[0][1] = 0; 317 | cal_data->w_invert[1][1] = 1; 318 | cal_data->w_invert[2][1] = 0; 319 | cal_data->w_invert[0][2] = 0; 320 | cal_data->w_invert[1][2] = 0; 321 | cal_data->w_invert[2][2] = 1; 322 | 323 | cal_data->bfield = 0; 324 | } 325 | } 326 | 327 | 328 | static void compass_store_result (FILE* data_file, sensor_info_t* info) 329 | { 330 | compass_cal_t* cal_data = (compass_cal_t*) info->cal_data; 331 | 332 | if (data_file == NULL || cal_data == NULL) 333 | return; 334 | 335 | int ret = fprintf(data_file, "%f %d %f %f %f %f %f %f %f %f %f %f %f %f %f\n", 336 | CAL_VERSION, info->cal_level, 337 | cal_data->offset[0][0], cal_data->offset[1][0], cal_data->offset[2][0], 338 | cal_data->w_invert[0][0], cal_data->w_invert[0][1], cal_data->w_invert[0][2], 339 | cal_data->w_invert[1][0], cal_data->w_invert[1][1], cal_data->w_invert[1][2], 340 | cal_data->w_invert[2][0], cal_data->w_invert[2][1], cal_data->w_invert[2][2], 341 | cal_data->bfield); 342 | 343 | if (ret < 0) 344 | ALOGE ("Compass calibration - store data failed!"); 345 | } 346 | 347 | 348 | static int compass_collect (sensors_event_t* event, sensor_info_t* info) 349 | { 350 | float data[3] = {event->magnetic.x, event->magnetic.y, event->magnetic.z}; 351 | unsigned int index,j; 352 | unsigned int lookback_count; 353 | float min_diff; 354 | 355 | compass_cal_t* cal_data = (compass_cal_t*) info->cal_data; 356 | 357 | if (cal_data == NULL) 358 | return -1; 359 | 360 | /* Discard the point if not valid */ 361 | if (data[0] == 0 || data[1] == 0 || data[2] == 0) 362 | return -1; 363 | 364 | lookback_count = lookback_counts[info->cal_level]; 365 | min_diff = min_diffs[info->cal_level]; 366 | 367 | /* For the current point to be accepted, each x/y/z value must be different enough to the last several collected points */ 368 | if (cal_data->sample_count > 0 && cal_data->sample_count < MAGN_DS_SIZE) { 369 | unsigned int lookback = lookback_count < cal_data->sample_count ? lookback_count : cal_data->sample_count; 370 | for (index = 0; index < lookback; index++) 371 | for (j = 0; j < 3; j++) 372 | if (fabsf(data[j] - cal_data->sample[cal_data->sample_count-1-index][j]) < min_diff) { 373 | ALOGV("CompassCalibration:point reject: [%f,%f,%f], selected_count=%d", data[0], data[1], data[2], cal_data->sample_count); 374 | return 0; 375 | } 376 | } 377 | 378 | if (cal_data->sample_count < MAGN_DS_SIZE) { 379 | memcpy(cal_data->sample[cal_data->sample_count], data, sizeof(float) * 3); 380 | cal_data->sample_count++; 381 | cal_data->average[0] += data[0]; 382 | cal_data->average[1] += data[1]; 383 | cal_data->average[2] += data[2]; 384 | ALOGV("CompassCalibration:point collected [%f,%f,%f], selected_count=%d", (double)data[0], (double)data[1], (double)data[2], cal_data->sample_count); 385 | } 386 | return 1; 387 | } 388 | 389 | 390 | static void scale_event (sensors_event_t* event) 391 | { 392 | float sqr_norm = 0; 393 | float sanity_norm = 0; 394 | float scale = 1; 395 | 396 | sqr_norm = (event->magnetic.x * event->magnetic.x + 397 | event->magnetic.y * event->magnetic.y + 398 | event->magnetic.z * event->magnetic.z); 399 | 400 | if (sqr_norm < MAGNETIC_LOW) 401 | sanity_norm = MAGNETIC_LOW; 402 | 403 | if (sanity_norm && sqr_norm) { 404 | scale = sanity_norm / sqr_norm; 405 | scale = sqrt(scale); 406 | event->magnetic.x = event->magnetic.x * scale; 407 | event->magnetic.y = event->magnetic.y * scale; 408 | event->magnetic.z = event->magnetic.z * scale; 409 | } 410 | } 411 | 412 | 413 | static void compass_compute_cal (sensors_event_t* event, sensor_info_t* info) 414 | { 415 | compass_cal_t* cal_data = (compass_cal_t*) info->cal_data; 416 | double result[3][1], raw[3][1], diff[3][1]; 417 | 418 | if (!info->cal_level || cal_data == NULL) 419 | return; 420 | 421 | raw[0][0] = event->magnetic.x; 422 | raw[1][0] = event->magnetic.y; 423 | raw[2][0] = event->magnetic.z; 424 | 425 | substract(3, 1, raw, cal_data->offset, diff); 426 | multiply (3, 3, 1, cal_data->w_invert, diff, result); 427 | 428 | event->magnetic.x = event->data[0] = result[0][0]; 429 | event->magnetic.y = event->data[1] = result[1][0]; 430 | event->magnetic.z = event->data[2] = result[2][0]; 431 | 432 | scale_event(event); 433 | } 434 | 435 | 436 | static int compass_ready (sensor_info_t* info) 437 | { 438 | mat_input_t mat; 439 | int i; 440 | float max_sqr_err; 441 | 442 | compass_cal_t* cal_data = (compass_cal_t*) info->cal_data; 443 | compass_cal_t new_cal_data; 444 | 445 | /* 446 | * Some sensors take unrealistically long to calibrate at higher levels. We'll use a max_cal_level if we have such a property setup, 447 | * or go with the default settings if not. 448 | */ 449 | int cal_steps = (info->max_cal_level && info->max_cal_level <= CAL_STEPS) ? info->max_cal_level : CAL_STEPS; 450 | 451 | if (cal_data->sample_count < MAGN_DS_SIZE) 452 | return info->cal_level; 453 | 454 | max_sqr_err = max_sqr_errs[info->cal_level]; 455 | 456 | /* Enough points have been collected, do the ellipsoid calibration */ 457 | 458 | /* Compute average per axis */ 459 | cal_data->average[0] /= MAGN_DS_SIZE; 460 | cal_data->average[1] /= MAGN_DS_SIZE; 461 | cal_data->average[2] /= MAGN_DS_SIZE; 462 | 463 | for (i = 0; i < MAGN_DS_SIZE; i++) { 464 | mat[i][0] = cal_data->sample[i][0]; 465 | mat[i][1] = cal_data->sample[i][1]; 466 | mat[i][2] = cal_data->sample[i][2]; 467 | } 468 | 469 | /* Check if result is good. The sample data must remain the same */ 470 | new_cal_data = *cal_data; 471 | 472 | if (ellipsoid_fit(mat, new_cal_data.offset, new_cal_data.w_invert, &new_cal_data.bfield)) { 473 | double new_err = calc_square_err (&new_cal_data); 474 | ALOGI("new err is %f, max sqr err id %f", new_err,max_sqr_err); 475 | if (new_err < max_sqr_err) { 476 | double err = calc_square_err(cal_data); 477 | if (new_err < err) { 478 | /* New cal data is better, so we switch to the new */ 479 | memcpy(cal_data->offset, new_cal_data.offset, sizeof(cal_data->offset)); 480 | memcpy(cal_data->w_invert, new_cal_data.w_invert, sizeof(cal_data->w_invert)); 481 | cal_data->bfield = new_cal_data.bfield; 482 | if (info->cal_level < (cal_steps - 1)) 483 | info->cal_level++; 484 | ALOGV("CompassCalibration: ready check success, caldata: %f %f %f %f %f %f %f %f %f %f %f %f %f, err %f", 485 | cal_data->offset[0][0], cal_data->offset[1][0], cal_data->offset[2][0], cal_data->w_invert[0][0], 486 | cal_data->w_invert[0][1], cal_data->w_invert[0][2], cal_data->w_invert[1][0], cal_data->w_invert[1][1], 487 | cal_data->w_invert[1][2], cal_data->w_invert[2][0], cal_data->w_invert[2][1], cal_data->w_invert[2][2], 488 | cal_data->bfield, new_err); 489 | } 490 | } 491 | } 492 | reset_sample(cal_data); 493 | return info->cal_level; 494 | } 495 | 496 | 497 | void calibrate_compass (int s, sensors_event_t* event) 498 | { 499 | int cal_level; 500 | 501 | /* Calibration is continuous */ 502 | compass_collect (event, &sensor[s]); 503 | 504 | cal_level = compass_ready(&sensor[s]); 505 | 506 | switch (cal_level) { 507 | case 0: 508 | scale_event(event); 509 | event->magnetic.status = SENSOR_STATUS_UNRELIABLE; 510 | break; 511 | 512 | case 1: 513 | compass_compute_cal (event, &sensor[s]); 514 | event->magnetic.status = SENSOR_STATUS_ACCURACY_LOW; 515 | break; 516 | 517 | case 2: 518 | compass_compute_cal (event, &sensor[s]); 519 | event->magnetic.status = SENSOR_STATUS_ACCURACY_MEDIUM; 520 | break; 521 | 522 | default: 523 | compass_compute_cal (event, &sensor[s]); 524 | event->magnetic.status = SENSOR_STATUS_ACCURACY_HIGH; 525 | break; 526 | } 527 | } 528 | 529 | void compass_read_data (int s) 530 | { 531 | FILE* data_file = fopen (COMPASS_CALIBRATION_PATH, "r"); 532 | 533 | compass_cal_init(data_file, &sensor[s]); 534 | 535 | if (data_file) 536 | fclose(data_file); 537 | } 538 | 539 | 540 | void compass_store_data (int s) 541 | { 542 | FILE* data_file = fopen (COMPASS_CALIBRATION_PATH, "w"); 543 | 544 | compass_store_result(data_file, &sensor[s]); 545 | 546 | if (data_file) 547 | fclose(data_file); 548 | } 549 | -------------------------------------------------------------------------------- /transform.c: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "calibration.h" 23 | #include "common.h" 24 | #include "description.h" 25 | #include "transform.h" 26 | #include "utils.h" 27 | #include "filtering.h" 28 | #include "enumeration.h" 29 | 30 | #define GYRO_MIN_SAMPLES 5 /* Drop first few gyro samples after enable */ 31 | 32 | 33 | /*----------------------------------------------------------------------------*/ 34 | 35 | 36 | /* Macros related to Intel Sensor Hub */ 37 | 38 | #define GRAVITY 9.80665 39 | 40 | /* 720 LSG = 1G */ 41 | #define LSG 1024.0 42 | #define NUMOFACCDATA 8.0 43 | 44 | /* Conversion of acceleration data to SI units (m/s^2) */ 45 | #define CONVERT_A (GRAVITY_EARTH / LSG / NUMOFACCDATA) 46 | #define CONVERT_A_X(x) ((float(x) / 1000) * (GRAVITY * -1.0)) 47 | #define CONVERT_A_Y(x) ((float(x) / 1000) * (GRAVITY * 1.0)) 48 | #define CONVERT_A_Z(x) ((float(x) / 1000) * (GRAVITY * 1.0)) 49 | 50 | /* Conversion of magnetic data to uT units */ 51 | #define CONVERT_M (1.0 / 6.6) 52 | #define CONVERT_M_X (-CONVERT_M) 53 | #define CONVERT_M_Y (-CONVERT_M) 54 | #define CONVERT_M_Z (CONVERT_M) 55 | 56 | /* Conversion of orientation data to degree units */ 57 | #define CONVERT_O (1.0 / 64) 58 | #define CONVERT_O_A (CONVERT_O) 59 | #define CONVERT_O_P (CONVERT_O) 60 | #define CONVERT_O_R (-CONVERT_O) 61 | 62 | /* Conversion of gyro data to SI units (radian/sec) */ 63 | #define CONVERT_GYRO (2000.0 / 32767 * M_PI / 180) 64 | #define CONVERT_GYRO_X (-CONVERT_GYRO) 65 | #define CONVERT_GYRO_Y (-CONVERT_GYRO) 66 | #define CONVERT_GYRO_Z (CONVERT_GYRO) 67 | 68 | #define BIT(x) (1 << (x)) 69 | 70 | #define PROXIMITY_THRESHOLD 1 71 | 72 | inline unsigned int set_bit_range (int start, int end) 73 | { 74 | int i; 75 | unsigned int value = 0; 76 | 77 | for (i = start; i < end; ++i) 78 | value |= BIT(i); 79 | 80 | return value; 81 | } 82 | 83 | inline float convert_from_vtf_format (int size, int exponent, unsigned int value) 84 | { 85 | int divider = 1; 86 | int i; 87 | float sample; 88 | float mul = 1.0; 89 | 90 | value = value & set_bit_range(0, size * 8); 91 | 92 | if (value & BIT(size*8-1)) { 93 | value = ((1LL << (size * 8)) - value); 94 | mul = -1.0; 95 | } 96 | 97 | sample = value * 1.0; 98 | 99 | if (exponent < 0) { 100 | exponent = abs(exponent); 101 | for (i = 0; i < exponent; ++i) 102 | divider = divider * 10; 103 | 104 | return mul * sample/divider; 105 | } 106 | 107 | return mul * sample * pow(10.0, exponent); 108 | } 109 | 110 | /* Platform sensor orientation */ 111 | #define DEF_ORIENT_ACCEL_X -1 112 | #define DEF_ORIENT_ACCEL_Y -1 113 | #define DEF_ORIENT_ACCEL_Z -1 114 | 115 | #define DEF_ORIENT_GYRO_X 1 116 | #define DEF_ORIENT_GYRO_Y 1 117 | #define DEF_ORIENT_GYRO_Z 1 118 | 119 | /* G to m/s^2 */ 120 | #define CONVERT_FROM_VTF16(s,d,x) convert_from_vtf_format(s,d,x) 121 | #define CONVERT_A_G_VTF16E14_X(s,d,x) (DEF_ORIENT_ACCEL_X * convert_from_vtf_format(s,d,x) * GRAVITY) 122 | #define CONVERT_A_G_VTF16E14_Y(s,d,x) (DEF_ORIENT_ACCEL_Y * convert_from_vtf_format(s,d,x) * GRAVITY) 123 | #define CONVERT_A_G_VTF16E14_Z(s,d,x) (DEF_ORIENT_ACCEL_Z * convert_from_vtf_format(s,d,x) * GRAVITY) 124 | 125 | /* Degree/sec to radian/sec */ 126 | #define CONVERT_G_D_VTF16E14_X(s,d,x) (DEF_ORIENT_GYRO_X * convert_from_vtf_format(s,d,x) * M_PI / 180) 127 | #define CONVERT_G_D_VTF16E14_Y(s,d,x) (DEF_ORIENT_GYRO_Y * convert_from_vtf_format(s,d,x) * M_PI / 180) 128 | #define CONVERT_G_D_VTF16E14_Z(s,d,x) (DEF_ORIENT_GYRO_Z * convert_from_vtf_format(s,d,x) * M_PI / 180) 129 | 130 | /* Milli gauss to micro tesla */ 131 | #define CONVERT_M_MG_VTF16E14_X(s,d,x) (convert_from_vtf_format(s,d,x) / 10) 132 | #define CONVERT_M_MG_VTF16E14_Y(s,d,x) (convert_from_vtf_format(s,d,x) / 10) 133 | #define CONVERT_M_MG_VTF16E14_Z(s,d,x) (convert_from_vtf_format(s,d,x) / 10) 134 | 135 | 136 | static int64_t sample_as_int64 (unsigned char* sample, datum_info_t* type) 137 | { 138 | uint64_t u64; 139 | int i; 140 | int zeroed_bits = type->storagebits - type->realbits; 141 | uint64_t sign_mask; 142 | uint64_t value_mask; 143 | 144 | u64 = 0; 145 | 146 | if (type->endianness == 'b') 147 | for (i=0; istoragebits/8; i++) 148 | u64 = (u64 << 8) | sample[i]; 149 | else 150 | for (i=type->storagebits/8 - 1; i>=0; i--) 151 | u64 = (u64 << 8) | sample[i]; 152 | 153 | u64 = (u64 >> type->shift) & (~0ULL >> zeroed_bits); 154 | 155 | if (type->sign == 'u') 156 | return (int64_t) u64; /* We don't handle unsigned 64 bits int */ 157 | 158 | /* Signed integer */ 159 | 160 | switch (type->realbits) { 161 | case 0 ... 1: 162 | return 0; 163 | 164 | case 8: 165 | return (int64_t) (int8_t) u64; 166 | 167 | case 16: 168 | return (int64_t) (int16_t) u64; 169 | 170 | case 32: 171 | return (int64_t) (int32_t) u64; 172 | 173 | case 64: 174 | return (int64_t) u64; 175 | 176 | default: 177 | sign_mask = 1 << (type->realbits-1); 178 | value_mask = sign_mask - 1; 179 | 180 | if (u64 & sign_mask) 181 | return - ((~u64 & value_mask) + 1); /* Negative value: return 2-complement */ 182 | else 183 | return (int64_t) u64; /* Positive value */ 184 | } 185 | } 186 | 187 | 188 | static void reorder_fields (float* data, unsigned char map[MAX_CHANNELS]) 189 | { 190 | int i; 191 | float temp[MAX_CHANNELS]; 192 | 193 | for (i=0; idata[0]; 218 | y = data->data[1]; 219 | z = data->data[2]; 220 | 221 | /* If we're calibrated, don't filter out as much */ 222 | if (sensor[s].cal_level > 0) 223 | near_zero = 0.02; /* rad/s */ 224 | else 225 | near_zero = 0.1; 226 | 227 | /* If motion on all axes is small enough */ 228 | if (fabs(x) < near_zero && fabs(y) < near_zero && fabs(z) < near_zero) { 229 | 230 | /* 231 | * Report that we're not moving at all... but not exactly zero as composite sensors (orientation, rotation vector) don't 232 | * seem to react very well to it. 233 | */ 234 | 235 | data->data[0] *= 0.000001; 236 | data->data[1] *= 0.000001; 237 | data->data[2] *= 0.000001; 238 | } 239 | } 240 | 241 | 242 | static void process_event_gyro_uncal (int s, int i, sensors_event_t* data) 243 | { 244 | gyro_cal_t* gyro_data; 245 | 246 | if (sensor[s].type == SENSOR_TYPE_GYROSCOPE) { 247 | gyro_data = (gyro_cal_t*) sensor[s].cal_data; 248 | 249 | memcpy(&sensor[i].sample, data, sizeof(sensors_event_t)); 250 | 251 | sensor[i].sample.type = SENSOR_TYPE_GYROSCOPE_UNCALIBRATED; 252 | sensor[i].sample.sensor = s; 253 | 254 | sensor[i].sample.data[0] = data->data[0] + gyro_data->bias_x; 255 | sensor[i].sample.data[1] = data->data[1] + gyro_data->bias_y; 256 | sensor[i].sample.data[2] = data->data[2] + gyro_data->bias_z; 257 | 258 | sensor[i].sample.uncalibrated_gyro.bias[0] = gyro_data->bias_x; 259 | sensor[i].sample.uncalibrated_gyro.bias[1] = gyro_data->bias_y; 260 | sensor[i].sample.uncalibrated_gyro.bias[2] = gyro_data->bias_z; 261 | 262 | sensor[i].report_pending = 1; 263 | } 264 | } 265 | 266 | static void process_event_magn_uncal (int s, int i, sensors_event_t* data) 267 | { 268 | compass_cal_t* magn_data; 269 | 270 | if (sensor[s].type == SENSOR_TYPE_MAGNETIC_FIELD) { 271 | magn_data = (compass_cal_t*) sensor[s].cal_data; 272 | 273 | memcpy(&sensor[i].sample, data, sizeof(sensors_event_t)); 274 | 275 | sensor[i].sample.type = SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED; 276 | sensor[i].sample.sensor = s; 277 | 278 | sensor[i].sample.data[0] = data->data[0] + magn_data->offset[0][0]; 279 | sensor[i].sample.data[1] = data->data[1] + magn_data->offset[1][0]; 280 | sensor[i].sample.data[2] = data->data[2] + magn_data->offset[2][0]; 281 | 282 | sensor[i].sample.uncalibrated_magnetic.bias[0] = magn_data->offset[0][0]; 283 | sensor[i].sample.uncalibrated_magnetic.bias[1] = magn_data->offset[1][0]; 284 | sensor[i].sample.uncalibrated_magnetic.bias[2] = magn_data->offset[2][0]; 285 | 286 | sensor[i].report_pending = 1; 287 | } 288 | } 289 | 290 | static void process_event (int s, sensors_event_t* data) 291 | { 292 | /* 293 | * This gets the real event (post process - calibration, filtering & co.) and makes it into a virtual one. 294 | * The specific processing function for each sensor will populate the necessary fields and set up the report pending flag. 295 | */ 296 | 297 | int i; 298 | 299 | /* Go through out virtual sensors and check if we can use this event */ 300 | for (i = 0; i < sensor_count; i++) 301 | switch (sensor[i].type) { 302 | case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED: 303 | process_event_gyro_uncal(s, i, data); 304 | break; 305 | case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED: 306 | process_event_magn_uncal(s, i, data); 307 | break; 308 | default: 309 | break; 310 | } 311 | } 312 | 313 | 314 | static int finalize_sample_default (int s, sensors_event_t* data) 315 | { 316 | /* Swap fields if we have a custom channel ordering on this sensor */ 317 | if (sensor[s].quirks & QUIRK_FIELD_ORDERING) 318 | reorder_fields(data->data, sensor[s].order); 319 | if (sensor[s].quirks & QUIRK_MOUNTING_MATRIX) 320 | mount_correction(data->data, sensor[s].mounting_matrix); 321 | 322 | sensor[s].event_count++; 323 | 324 | switch (sensor[s].type) { 325 | case SENSOR_TYPE_ACCELEROMETER: 326 | /* Always consider the accelerometer accurate */ 327 | data->acceleration.status = SENSOR_STATUS_ACCURACY_HIGH; 328 | if (sensor[s].quirks & QUIRK_BIASED) 329 | calibrate_accel(s, data); 330 | denoise(s, data); 331 | break; 332 | 333 | case SENSOR_TYPE_MAGNETIC_FIELD: 334 | calibrate_compass (s, data); 335 | denoise(s, data); 336 | break; 337 | 338 | case SENSOR_TYPE_GYROSCOPE: 339 | 340 | /* Report medium accuracy by default ; higher accuracy levels will be reported once, and if, we achieve calibration. */ 341 | data->gyro.status = SENSOR_STATUS_ACCURACY_MEDIUM; 342 | 343 | /* 344 | * We're only trying to calibrate data from continuously firing gyroscope drivers, as motion based ones use 345 | * movement thresholds that may lead us to incorrectly estimate bias. 346 | */ 347 | if (sensor[s].selected_trigger != 348 | sensor[s].motion_trigger_name) 349 | calibrate_gyro(s, data); 350 | 351 | /* 352 | * For noisy sensors drop a few samples to make sure we have at least GYRO_MIN_SAMPLES events in the 353 | * filtering queue. This improves mean and std dev. 354 | */ 355 | if (sensor[s].filter_type) { 356 | if (sensor[s].selected_trigger != 357 | sensor[s].motion_trigger_name && 358 | sensor[s].event_count < GYRO_MIN_SAMPLES) 359 | return 0; 360 | 361 | denoise(s, data); 362 | } 363 | 364 | /* Clamp near zero moves to (0,0,0) if appropriate */ 365 | clamp_gyro_readings_to_zero(s, data); 366 | break; 367 | 368 | case SENSOR_TYPE_PROXIMITY: 369 | /* 370 | * See iio spec for in_proximity* - depending on the device 371 | * this value is either in meters either unit-less and cannot 372 | * be translated to SI units. Where the translation is not possible 373 | * lower values indicate something is close and higher ones indicate distance. 374 | */ 375 | if (data->data[0] > PROXIMITY_THRESHOLD) 376 | data->data[0] = PROXIMITY_THRESHOLD; 377 | 378 | /* ... fall through ... */ 379 | case SENSOR_TYPE_LIGHT: 380 | case SENSOR_TYPE_AMBIENT_TEMPERATURE: 381 | case SENSOR_TYPE_TEMPERATURE: 382 | case SENSOR_TYPE_INTERNAL_ILLUMINANCE: 383 | case SENSOR_TYPE_INTERNAL_INTENSITY: 384 | /* Only keep two decimals for these readings */ 385 | data->data[0] = 0.01 * ((int) (data->data[0] * 100)); 386 | 387 | /* These are on change sensors ; drop the sample if it has the same value as the previously reported one. */ 388 | if (data->data[0] == sensor[s].prev_val.data) 389 | return 0; 390 | 391 | sensor[s].prev_val.data = data->data[0]; 392 | break; 393 | case SENSOR_TYPE_STEP_COUNTER: 394 | if (data->u64.step_counter == sensor[s].prev_val.data64) 395 | return 0; 396 | sensor[s].prev_val.data64 = data->u64.data[0]; 397 | break; 398 | 399 | } 400 | 401 | /* If there are active virtual sensors depending on this one - process the event */ 402 | if (sensor[s].ref_count) 403 | process_event(s, data); 404 | 405 | 406 | return 1; /* Return sample to Android */ 407 | } 408 | 409 | 410 | static float transform_sample_default (int s, int c, unsigned char* sample_data) 411 | { 412 | datum_info_t* sample_type = &sensor[s].channel[c].type_info; 413 | int64_t s64 = sample_as_int64(sample_data, sample_type); 414 | float scale = sensor[s].scale ? sensor[s].scale : sensor[s].channel[c].scale; 415 | 416 | /* In case correction has been requested using properties, apply it */ 417 | float correction = sensor[s].channel[c].opt_scale; 418 | 419 | /* Correlated with "acquire_immediate_value" method */ 420 | if (sensor[s].type == SENSOR_TYPE_MAGNETIC_FIELD) 421 | return CONVERT_GAUSS_TO_MICROTESLA((sensor[s].offset + s64) * scale) * correction; 422 | 423 | /* Apply default scaling rules */ 424 | return (sensor[s].offset + s64) * scale * correction; 425 | } 426 | 427 | 428 | static int finalize_sample_ISH (int s, sensors_event_t* data) 429 | { 430 | float pitch, roll, yaw; 431 | 432 | /* Swap fields if we have a custom channel ordering on this sensor */ 433 | if (sensor[s].quirks & QUIRK_FIELD_ORDERING) 434 | reorder_fields(data->data, sensor[s].order); 435 | 436 | if (sensor[s].type == SENSOR_TYPE_ORIENTATION) { 437 | 438 | pitch = data->data[0]; 439 | roll = data->data[1]; 440 | yaw = data->data[2]; 441 | 442 | data->data[0] = 360.0 - yaw; 443 | data->data[1] = -pitch; 444 | data->data[2] = -roll; 445 | } 446 | 447 | /* Add this event to our global records, for filtering purposes */ 448 | record_sample(s, data); 449 | 450 | return 1; /* Return sample to Android */ 451 | } 452 | 453 | 454 | static float transform_sample_ISH (int s, int c, unsigned char* sample_data) 455 | { 456 | datum_info_t* sample_type = &sensor[s].channel[c].type_info; 457 | int val = (int) sample_as_int64(sample_data, sample_type); 458 | float correction; 459 | int data_bytes = (sample_type->realbits)/8; 460 | int exponent = sensor[s].offset; 461 | 462 | /* In case correction has been requested using properties, apply it */ 463 | correction = sensor[s].channel[c].opt_scale; 464 | 465 | switch (sensor_desc[s].type) { 466 | case SENSOR_TYPE_ACCELEROMETER: 467 | switch (c) { 468 | case 0: 469 | return correction * CONVERT_A_G_VTF16E14_X(data_bytes, exponent, val); 470 | 471 | case 1: 472 | return correction * CONVERT_A_G_VTF16E14_Y(data_bytes, exponent, val); 473 | 474 | case 2: 475 | return correction * CONVERT_A_G_VTF16E14_Z(data_bytes, exponent, val); 476 | } 477 | break; 478 | 479 | case SENSOR_TYPE_GYROSCOPE: 480 | switch (c) { 481 | case 0: 482 | return correction * CONVERT_G_D_VTF16E14_X(data_bytes, exponent, val); 483 | 484 | case 1: 485 | return correction * CONVERT_G_D_VTF16E14_Y(data_bytes, exponent, val); 486 | 487 | case 2: 488 | return correction * CONVERT_G_D_VTF16E14_Z(data_bytes, exponent, val); 489 | } 490 | break; 491 | 492 | case SENSOR_TYPE_MAGNETIC_FIELD: 493 | switch (c) { 494 | case 0: 495 | return correction * CONVERT_M_MG_VTF16E14_X(data_bytes, exponent, val); 496 | 497 | case 1: 498 | return correction * CONVERT_M_MG_VTF16E14_Y(data_bytes, exponent, val); 499 | 500 | case 2: 501 | return correction * CONVERT_M_MG_VTF16E14_Z(data_bytes, exponent, val); 502 | } 503 | break; 504 | 505 | case SENSOR_TYPE_LIGHT: 506 | return (float) val; 507 | 508 | case SENSOR_TYPE_ORIENTATION: 509 | return correction * convert_from_vtf_format(data_bytes, exponent, val); 510 | 511 | case SENSOR_TYPE_ROTATION_VECTOR: 512 | return correction * convert_from_vtf_format(data_bytes, exponent, val); 513 | } 514 | 515 | return 0; 516 | } 517 | 518 | 519 | void select_transform (int s) 520 | { 521 | char prop_name[PROP_NAME_MAX]; 522 | char prop_val[PROP_VALUE_MAX]; 523 | int i = sensor[s].catalog_index; 524 | const char *prefix = sensor_catalog[i].tag; 525 | 526 | sprintf(prop_name, PROP_BASE, prefix, "transform"); 527 | 528 | if (property_get(prop_name, prop_val, "")) 529 | if (!strcmp(prop_val, "ISH")) { 530 | ALOGI( "Using Intel Sensor Hub semantics on %s\n", sensor[s].friendly_name); 531 | 532 | sensor[s].ops.transform = transform_sample_ISH; 533 | sensor[s].ops.finalize = finalize_sample_ISH; 534 | return; 535 | } 536 | 537 | sensor[s].ops.transform = transform_sample_default; 538 | sensor[s].ops.finalize = finalize_sample_default; 539 | } 540 | 541 | 542 | float acquire_immediate_float_value (int s, int c) 543 | { 544 | char sysfs_path[PATH_MAX]; 545 | float val; 546 | int ret; 547 | int dev_num = sensor[s].dev_num; 548 | int i = sensor[s].catalog_index; 549 | const char* raw_path = sensor_catalog[i].channel[c].raw_path; 550 | const char* input_path = sensor_catalog[i].channel[c].input_path; 551 | float scale = sensor[s].scale ? sensor[s].scale : sensor[s].channel[c].scale; 552 | float offset = sensor[s].offset; 553 | float correction; 554 | 555 | /* In case correction has been requested using properties, apply it */ 556 | correction = sensor[s].channel[c].opt_scale; 557 | 558 | /* Acquire a sample value for sensor s / channel c through sysfs */ 559 | 560 | if (sensor[s].channel[c].input_path_present) { 561 | sprintf(sysfs_path, BASE_PATH "%s", dev_num, input_path); 562 | ret = sysfs_read_float(sysfs_path, &val); 563 | 564 | if (!ret) { 565 | if (sensor[s].type == SENSOR_TYPE_MAGNETIC_FIELD) 566 | return CONVERT_GAUSS_TO_MICROTESLA (val * correction); 567 | return val * correction; 568 | } 569 | } 570 | 571 | if (!sensor[s].channel[c].raw_path_present) 572 | return 0; 573 | 574 | sprintf(sysfs_path, BASE_PATH "%s", dev_num, raw_path); 575 | ret = sysfs_read_float(sysfs_path, &val); 576 | 577 | if (ret == -1) 578 | return 0; 579 | 580 | /* 581 | * There is no transform ops defined yet for raw sysfs values. 582 | * Use this function to perform transformation as well. 583 | */ 584 | if (sensor[s].type == SENSOR_TYPE_MAGNETIC_FIELD) 585 | return CONVERT_GAUSS_TO_MICROTESLA ((val + offset) * scale) * correction; 586 | 587 | return (val + offset) * scale * correction; 588 | } 589 | 590 | uint64_t acquire_immediate_uint64_value (int s, int c) 591 | { 592 | char sysfs_path[PATH_MAX]; 593 | uint64_t val; 594 | int ret; 595 | int dev_num = sensor[s].dev_num; 596 | int i = sensor[s].catalog_index; 597 | const char* raw_path = sensor_catalog[i].channel[c].raw_path; 598 | const char* input_path = sensor_catalog[i].channel[c].input_path; 599 | float scale = sensor[s].scale ? sensor[s].scale : sensor[s].channel[c].scale; 600 | float offset = sensor[s].offset; 601 | int sensor_type = sensor_catalog[i].type; 602 | float correction; 603 | 604 | /* In case correction has been requested using properties, apply it */ 605 | correction = sensor[s].channel[c].opt_scale; 606 | 607 | /* Acquire a sample value for sensor s / channel c through sysfs */ 608 | 609 | if (sensor[s].channel[c].input_path_present) { 610 | sprintf(sysfs_path, BASE_PATH "%s", dev_num, input_path); 611 | ret = sysfs_read_uint64(sysfs_path, &val); 612 | 613 | if (!ret) 614 | return val * correction; 615 | }; 616 | 617 | if (!sensor[s].channel[c].raw_path_present) 618 | return 0; 619 | 620 | sprintf(sysfs_path, BASE_PATH "%s", dev_num, raw_path); 621 | ret = sysfs_read_uint64(sysfs_path, &val); 622 | 623 | if (ret == -1) 624 | return 0; 625 | 626 | return (val + offset) * scale * correction; 627 | } 628 | -------------------------------------------------------------------------------- /description.c: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2015 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "common.h" 23 | #include "enumeration.h" 24 | #include "description.h" 25 | #include "utils.h" 26 | #include "transform.h" 27 | 28 | #define IIO_SENSOR_HAL_VERSION 1 29 | 30 | #define MIN_ON_CHANGE_SAMPLING_PERIOD_US 200000 /* For on change sensors (temperature, proximity, ALS, etc.) report we support 5 Hz max (0.2 s min period) */ 31 | #define MAX_ON_CHANGE_SAMPLING_PERIOD_US 10000000 /* 0.1 Hz min (10 s max period)*/ 32 | #define ANDROID_MAX_FREQ 1000 /* 1000 Hz - This is how much Android requests for the fastest frequency */ 33 | 34 | /* 35 | * About properties 36 | * 37 | * We acquire a number of parameters about sensors by reading properties. 38 | * The idea here is that someone (either a script, or daemon, sets them 39 | * depending on the set of sensors present on the machine. 40 | * 41 | * There are fallback paths in case the properties are not defined, but it is 42 | * highly desirable to at least have the following for each sensor: 43 | * 44 | * ro.iio.anglvel.name = Gyroscope 45 | * ro.iio.anglvel.vendor = Intel 46 | * ro.iio.anglvel.max_range = 35 47 | * ro.iio.anglvel.resolution = 0.002 48 | * ro.iio.anglvel.power = 6.1 49 | * 50 | * Besides these, we have a couple of knobs initially used to cope with Intel 51 | * Sensor Hub oddities, such as HID inspired units or firmware bugs: 52 | * 53 | * ro.iio.anglvel.transform = ISH 54 | * ro.iio.anglvel.quirks = init-rate 55 | * 56 | * The "terse" quirk indicates that the underlying driver only sends events 57 | * when the sensor reports a change. The HAL then periodically generates 58 | * duplicate events so the sensor behaves as a continously firing one. 59 | * 60 | * The "noisy" quirk indicates that the underlying driver has a unusually high 61 | * level of noise in its readings, and that the HAL has to accomodate it 62 | * somehow, e.g. in the magnetometer calibration code path. 63 | * 64 | * This one is used specifically to pass a calibration scale to ALS drivers: 65 | * 66 | * ro.iio.illuminance.name = CPLM3218x Ambient Light Sensor 67 | * ro.iio.illuminance.vendor = Capella Microsystems 68 | * ro.iio.illuminance.max_range = 167000 69 | * ro.iio.illuminance.resolution = 1 70 | * ro.iio.illuminance.power = .001 71 | * ro.iio.illuminance.illumincalib = 7400 72 | * 73 | * There's a 'opt_scale' specifier, documented as follows: 74 | * 75 | * This adds support for a scaling factor that can be expressed 76 | * using properties, for all sensors, on a channel basis. That 77 | * scaling factor is applied after all other transforms have been 78 | * applied, and is intended as a way to compensate for problems 79 | * such as an incorrect axis polarity for a given sensor. 80 | * 81 | * The syntax is ..opt_scale, e.g. 82 | * ro.iio.accel.y.opt_scale = -1 to negate the sign of the y readings 83 | * for the accelerometer. 84 | * 85 | * For sensors using a single channel - and only those - the channel 86 | * name is implicitly void and a syntax such as ro.iio.illuminance. 87 | * opt_scale = 3 has to be used. 88 | * 89 | * 'panel' and 'rotation' specifiers can be used to express ACPI PLD placement 90 | * information ; if found they will be used in priority over the actual ACPI 91 | * data. That is intended as a way to verify values during development. 92 | * 93 | * It's possible to use the contents of the iio device name as a way to 94 | * discriminate between sensors. Several sensors of the same type can coexist: 95 | * e.g. ro.iio.temp.bmg160.name = BMG160 Thermometer will be used in priority 96 | * over ro.iio.temp.name = BMC150 Thermometer if the sensor for which we query 97 | * properties values happen to have its iio device name set to bmg160. 98 | */ 99 | 100 | int sensor_get_st_prop (int s, const char* sel, char val[MAX_NAME_SIZE]) 101 | { 102 | char prop_name[PROP_NAME_MAX]; 103 | char prop_val[PROP_VALUE_MAX]; 104 | char extended_sel[PROP_VALUE_MAX]; 105 | 106 | int i = sensor[s].catalog_index; 107 | const char *prefix = sensor_catalog[i].tag; 108 | const char *shorthand = sensor_catalog[i].shorthand; 109 | 110 | /* First try most specialized form, like ro.iio.anglvel.bmg160.name */ 111 | 112 | snprintf(extended_sel, PROP_NAME_MAX, "%s.%s", 113 | sensor[s].internal_name, sel); 114 | 115 | snprintf(prop_name, PROP_NAME_MAX, PROP_BASE, prefix, extended_sel); 116 | 117 | if (property_get(prop_name, prop_val, "")) { 118 | strncpy(val, prop_val, MAX_NAME_SIZE-1); 119 | val[MAX_NAME_SIZE-1] = '\0'; 120 | return 0; 121 | } 122 | 123 | if (shorthand[0] != '\0') { 124 | /* Try with shorthand instead of prefix */ 125 | snprintf(prop_name, PROP_NAME_MAX, PROP_BASE, shorthand, extended_sel); 126 | 127 | if (property_get(prop_name, prop_val, "")) { 128 | strncpy(val, prop_val, MAX_NAME_SIZE-1); 129 | val[MAX_NAME_SIZE-1] = '\0'; 130 | return 0; 131 | } 132 | } 133 | /* Fall back to simple form, like ro.iio.anglvel.name */ 134 | 135 | snprintf(prop_name, PROP_NAME_MAX, PROP_BASE, prefix, sel); 136 | 137 | if (property_get(prop_name, prop_val, "")) { 138 | strncpy(val, prop_val, MAX_NAME_SIZE-1); 139 | val[MAX_NAME_SIZE-1] = '\0'; 140 | return 0; 141 | } 142 | 143 | return -1; 144 | } 145 | 146 | 147 | int sensor_get_prop (int s, const char* sel, int* val) 148 | { 149 | char buf[MAX_NAME_SIZE]; 150 | 151 | if (sensor_get_st_prop(s, sel, buf)) 152 | return -1; 153 | 154 | *val = atoi(buf); 155 | return 0; 156 | } 157 | 158 | 159 | int sensor_get_fl_prop (int s, const char* sel, float* val) 160 | { 161 | char buf[MAX_NAME_SIZE]; 162 | 163 | if (sensor_get_st_prop(s, sel, buf)) 164 | return -1; 165 | 166 | *val = (float) strtod(buf, NULL); 167 | return 0; 168 | } 169 | 170 | 171 | char* sensor_get_name (int s) 172 | { 173 | char buf[MAX_NAME_SIZE]; 174 | 175 | if (sensor[s].is_virtual) { 176 | switch (sensor[s].type) { 177 | case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED: 178 | case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED: 179 | strcpy(buf, sensor[sensor[s].base[0]].friendly_name); 180 | snprintf(sensor[s].friendly_name, 181 | MAX_NAME_SIZE, 182 | "%s %s", "Uncalibrated", buf); 183 | return sensor[s].friendly_name; 184 | 185 | default: 186 | return ""; 187 | } 188 | } 189 | 190 | if (sensor[s].friendly_name[0] != '\0' || 191 | !sensor_get_st_prop(s, "name", sensor[s].friendly_name)) 192 | return sensor[s].friendly_name; 193 | 194 | /* If we got a iio device name from sysfs, use it */ 195 | if (sensor[s].internal_name[0]) { 196 | snprintf(sensor[s].friendly_name, MAX_NAME_SIZE, "S%d-%s", 197 | s, sensor[s].internal_name); 198 | } else { 199 | sprintf(sensor[s].friendly_name, "S%d", s); 200 | } 201 | 202 | return sensor[s].friendly_name; 203 | } 204 | 205 | 206 | char* sensor_get_vendor (int s) 207 | { 208 | if (sensor[s].is_virtual) { 209 | switch (sensor[s].type) { 210 | case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED: 211 | case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED: 212 | return sensor[sensor[s].base[0]].vendor_name; 213 | break; 214 | 215 | default: 216 | return ""; 217 | 218 | } 219 | } 220 | 221 | if (sensor[s].vendor_name[0] || 222 | !sensor_get_st_prop(s, "vendor", sensor[s].vendor_name)) 223 | return sensor[s].vendor_name; 224 | 225 | return ""; 226 | } 227 | 228 | 229 | int sensor_get_version (__attribute__((unused)) int s) 230 | { 231 | return IIO_SENSOR_HAL_VERSION; 232 | } 233 | 234 | void sensor_update_max_range(int s) 235 | { 236 | if (sensor[s].max_range) 237 | return; 238 | 239 | if (sensor[s].num_channels && sensor[s].channel[0].type_info.realbits) { 240 | switch (sensor[s].type) { 241 | case SENSOR_TYPE_MAGNETIC_FIELD: 242 | sensor[s].max_range = (1ULL << sensor[s].channel[0].type_info.realbits) * 243 | CONVERT_MICROTESLA_TO_GAUSS(sensor[s].resolution) + 244 | (sensor[s].offset || sensor[s].channel[0].offset); 245 | sensor[s].max_range = CONVERT_GAUSS_TO_MICROTESLA(sensor[s].max_range); 246 | break; 247 | case SENSOR_TYPE_PROXIMITY: 248 | break; 249 | default: 250 | sensor[s].max_range = (1ULL << sensor[s].channel[0].type_info.realbits) * 251 | sensor[s].resolution + (sensor[s].offset || sensor[s].channel[0].offset); 252 | break; 253 | } 254 | } 255 | 256 | if (!sensor[s].max_range) { 257 | /* Try returning a sensible value given the sensor type */ 258 | /* We should cap returned samples accordingly... */ 259 | switch (sensor[s].type) { 260 | case SENSOR_TYPE_ACCELEROMETER: /* m/s^2 */ 261 | sensor[s].max_range = 50; 262 | break; 263 | case SENSOR_TYPE_MAGNETIC_FIELD: /* micro-tesla */ 264 | sensor[s].max_range = 500; 265 | break; 266 | case SENSOR_TYPE_ORIENTATION: /* degrees */ 267 | sensor[s].max_range = 360; 268 | break; 269 | case SENSOR_TYPE_GYROSCOPE: /* radians/s */ 270 | sensor[s].max_range = 10; 271 | break; 272 | case SENSOR_TYPE_LIGHT: /* SI lux units */ 273 | sensor[s].max_range = 50000; 274 | break; 275 | case SENSOR_TYPE_AMBIENT_TEMPERATURE: /* °C */ 276 | case SENSOR_TYPE_TEMPERATURE: /* °C */ 277 | case SENSOR_TYPE_PROXIMITY: /* centimeters */ 278 | case SENSOR_TYPE_PRESSURE: /* hecto-pascal */ 279 | case SENSOR_TYPE_RELATIVE_HUMIDITY: /* percent */ 280 | sensor[s].max_range = 100; 281 | break; 282 | } 283 | } 284 | 285 | if (sensor[s].max_range) 286 | sensor_desc[s].maxRange = sensor[s].max_range; 287 | } 288 | 289 | float sensor_get_max_range (int s) 290 | { 291 | if (sensor[s].is_virtual) { 292 | switch (sensor[s].type) { 293 | case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED: 294 | case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED: 295 | return sensor[sensor[s].base[0]].max_range; 296 | 297 | default: 298 | return 0.0; 299 | } 300 | } 301 | 302 | if (sensor[s].max_range != 0.0 || 303 | !sensor_get_fl_prop(s, "max_range", &sensor[s].max_range)) 304 | return sensor[s].max_range; 305 | 306 | return 0; 307 | } 308 | 309 | float sensor_get_min_freq (int s) 310 | { 311 | /* 312 | * Check if a low cap has been specified for this sensor sampling rate. 313 | * In some case, even when the driver supports lower rate, we still 314 | * wish to receive a certain number of samples per seconds for various 315 | * reasons (calibration, filtering, no change in power consumption...). 316 | */ 317 | 318 | float min_freq; 319 | 320 | if (!sensor_get_fl_prop(s, "min_freq", &min_freq)) 321 | return min_freq; 322 | 323 | return 0; 324 | } 325 | 326 | 327 | float sensor_get_max_freq (int s) 328 | { 329 | float max_freq; 330 | 331 | if (!sensor_get_fl_prop(s, "max_freq", &max_freq)) 332 | return max_freq; 333 | 334 | return ANDROID_MAX_FREQ; 335 | } 336 | 337 | int sensor_get_cal_steps (int s) 338 | { 339 | int cal_steps; 340 | if (!sensor_get_prop(s, "cal_steps", &cal_steps)) 341 | return cal_steps; 342 | 343 | return 0; 344 | } 345 | 346 | float sensor_get_resolution (int s) 347 | { 348 | if (sensor[s].is_virtual) { 349 | switch (sensor[s].type) { 350 | case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED: 351 | case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED: 352 | return sensor[sensor[s].base[0]].resolution; 353 | 354 | default: 355 | return 0; 356 | } 357 | } 358 | 359 | if (sensor[s].resolution != 0.0 || 360 | !sensor_get_fl_prop(s, "resolution", &sensor[s].resolution)) { 361 | return sensor[s].resolution; 362 | } 363 | 364 | sensor[s].resolution = sensor[s].scale; 365 | if (!sensor[s].resolution && sensor[s].num_channels) 366 | sensor[s].resolution = sensor[s].channel[0].scale; 367 | 368 | if (sensor[s].type == SENSOR_TYPE_MAGNETIC_FIELD) 369 | sensor[s].resolution = CONVERT_GAUSS_TO_MICROTESLA(sensor[s].resolution); 370 | 371 | return sensor[s].resolution ? : 1; 372 | } 373 | 374 | 375 | float sensor_get_power (int s) 376 | { 377 | 378 | if (sensor[s].is_virtual) { 379 | switch (sensor[s].type) { 380 | case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED: 381 | case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED: 382 | return sensor[sensor[s].base[0]].power; 383 | 384 | default: 385 | return 0; 386 | } 387 | } 388 | 389 | /* mA used while sensor is in use ; not sure about volts :) */ 390 | if (sensor[s].power != 0.0 || 391 | !sensor_get_fl_prop(s, "power", &sensor[s].power)) 392 | return sensor[s].power; 393 | 394 | return 0; 395 | } 396 | 397 | 398 | float sensor_get_illumincalib (int s) 399 | { 400 | /* calibrating the ALS Sensor*/ 401 | if (sensor[s].illumincalib != 0.0 || 402 | !sensor_get_fl_prop(s, "illumincalib", &sensor[s].illumincalib)) { 403 | return sensor[s].illumincalib; 404 | } 405 | 406 | return 0; 407 | } 408 | 409 | 410 | uint32_t sensor_get_quirks (int s) 411 | { 412 | char quirks_buf[MAX_NAME_SIZE]; 413 | 414 | /* Read and decode quirks property on first reference */ 415 | if (!(sensor[s].quirks & QUIRK_ALREADY_DECODED)) { 416 | quirks_buf[0] = '\0'; 417 | sensor_get_st_prop(s, "quirks", quirks_buf); 418 | 419 | if (strstr(quirks_buf, "init-rate")) 420 | sensor[s].quirks |= QUIRK_INITIAL_RATE; 421 | 422 | if (strstr(quirks_buf, "continuous")) 423 | sensor[s].quirks |= QUIRK_FORCE_CONTINUOUS; 424 | 425 | if (strstr(quirks_buf, "terse")) 426 | sensor[s].quirks |= QUIRK_TERSE_DRIVER; 427 | 428 | if (strstr(quirks_buf, "noisy")) 429 | sensor[s].quirks |= QUIRK_NOISY; 430 | 431 | if (strstr(quirks_buf, "biased")) 432 | sensor[s].quirks |= QUIRK_BIASED; 433 | 434 | if (strstr(quirks_buf, "spotty")) 435 | sensor[s].quirks |= QUIRK_SPOTTY; 436 | 437 | if (strstr(quirks_buf, "no-event")) 438 | sensor[s].quirks |= QUIRK_NO_EVENT_MODE; 439 | 440 | if (strstr(quirks_buf, "no-trig")) 441 | sensor[s].quirks |= QUIRK_NO_TRIG_MODE; 442 | 443 | if (strstr(quirks_buf, "no-poll")) 444 | sensor[s].quirks |= QUIRK_NO_POLL_MODE; 445 | 446 | if (strstr(quirks_buf, "hrtimer")) 447 | sensor[s].quirks |= QUIRK_HRTIMER; 448 | 449 | if (strstr(quirks_buf, "secondary")) 450 | sensor[s].quirks |= QUIRK_SECONDARY; 451 | 452 | sensor[s].quirks |= QUIRK_ALREADY_DECODED; 453 | } 454 | 455 | return sensor[s].quirks; 456 | } 457 | 458 | 459 | int sensor_get_order (int s, unsigned char map[MAX_CHANNELS]) 460 | { 461 | char buf[MAX_NAME_SIZE]; 462 | int i; 463 | int count = sensor_catalog[sensor[s].catalog_index].num_channels; 464 | 465 | if (sensor_get_st_prop(s, "order", buf)) 466 | return 0; /* No order property */ 467 | 468 | /* Assume ASCII characters, in the '0'..'9' range */ 469 | 470 | for (i=0; i= count) { 472 | ALOGE("Order index out of range for sensor %d\n", s); 473 | return 0; 474 | } 475 | 476 | for (i=0; i max_supported_rate && 785 | sensor[s].avail_freqs[i] <= max_from_prop) { 786 | max_supported_rate = sensor[s].avail_freqs[i]; 787 | } 788 | } 789 | } 790 | 791 | /* return 0 for wrong values */ 792 | if (max_supported_rate < 0.1) 793 | return 0; 794 | 795 | /* Return microseconds */ 796 | return (int32_t) (1000000.0 / max_supported_rate); 797 | } 798 | --------------------------------------------------------------------------------