| /* |
| * Copyright Statement: |
| * |
| * This software/firmware and related documentation ("MediaTek Software") are |
| * protected under relevant copyright laws. The information contained herein is |
| * confidential and proprietary to MediaTek Inc. and/or its licensors. Without |
| * the prior written permission of MediaTek inc. and/or its licensors, any |
| * reproduction, modification, use or disclosure of MediaTek Software, and |
| * information contained herein, in whole or in part, shall be strictly |
| * prohibited. |
| * |
| * MediaTek Inc. (C) 2017. All rights reserved. |
| * |
| * BY OPENING THIS FILE, RECEIVER HEREBY UNEQUIVOCALLY ACKNOWLEDGES AND AGREES |
| * THAT THE SOFTWARE/FIRMWARE AND ITS DOCUMENTATIONS ("MEDIATEK SOFTWARE") |
| * RECEIVED FROM MEDIATEK AND/OR ITS REPRESENTATIVES ARE PROVIDED TO RECEIVER |
| * ON AN "AS-IS" BASIS ONLY. MEDIATEK EXPRESSLY DISCLAIMS ANY AND ALL |
| * WARRANTIES, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED |
| * WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE OR |
| * NONINFRINGEMENT. NEITHER DOES MEDIATEK PROVIDE ANY WARRANTY WHATSOEVER WITH |
| * RESPECT TO THE SOFTWARE OF ANY THIRD PARTY WHICH MAY BE USED BY, |
| * INCORPORATED IN, OR SUPPLIED WITH THE MEDIATEK SOFTWARE, AND RECEIVER AGREES |
| * TO LOOK ONLY TO SUCH THIRD PARTY FOR ANY WARRANTY CLAIM RELATING THERETO. |
| * RECEIVER EXPRESSLY ACKNOWLEDGES THAT IT IS RECEIVER'S SOLE RESPONSIBILITY TO |
| * OBTAIN FROM ANY THIRD PARTY ALL PROPER LICENSES CONTAINED IN MEDIATEK |
| * SOFTWARE. MEDIATEK SHALL ALSO NOT BE RESPONSIBLE FOR ANY MEDIATEK SOFTWARE |
| * RELEASES MADE TO RECEIVER'S SPECIFICATION OR TO CONFORM TO A PARTICULAR |
| * STANDARD OR OPEN FORUM. RECEIVER'S SOLE AND EXCLUSIVE REMEDY AND MEDIATEK'S |
| * ENTIRE AND CUMULATIVE LIABILITY WITH RESPECT TO THE MEDIATEK SOFTWARE |
| * RELEASED HEREUNDER WILL BE, AT MEDIATEK'S OPTION, TO REVISE OR REPLACE THE |
| * MEDIATEK SOFTWARE AT ISSUE, OR REFUND ANY SOFTWARE LICENSE FEES OR SERVICE |
| * CHARGE PAID BY RECEIVER TO MEDIATEK FOR SUCH MEDIATEK SOFTWARE AT ISSUE. |
| */ |
| |
| //sensor driver |
| #include <stdio.h> |
| #include <stdint.h> |
| |
| #include <stdio.h> |
| #include <stdint.h> |
| #include <pthread.h> |
| #include <string.h> |
| #include <dlfcn.h> |
| #include <unistd.h> |
| #include <errno.h> |
| |
| #include "zalloc.h" |
| #include "log.h" |
| |
| #include "sensors.h" |
| #include "module_sensor_helper.h" |
| |
| |
| #define SENSOR_LIB "/usr/lib/libsensorhal.so.0" |
| #define SENSOR_DISABLE 0 |
| #define SENSOR_ENABLE 1 |
| #define SENSOR_POLL_NS 20000000LL |
| #define SENSOR_EVENT_BUFFER_SIZE 16 |
| #define BARO_EVENT_BUFFER_SIZE 16 |
| #define SENSOR_EVENT_LEN 15 |
| #define SENSOR_MODULE_STAT_MSG_LEN 8 |
| |
| #define nsec_2_sec 1000000000 |
| #define nsec_2_msec 1000000 |
| |
| typedef struct sensor_device { |
| char *name; |
| void *sensor_hal; |
| |
| pthread_t poll_thread; |
| int poll_thread_exit; |
| |
| pthread_t dataready_thread; |
| int dataready_thread_exit; |
| |
| FILE *sensor_fp; |
| ssize_t sensor_count; |
| struct sensor_t* sensor_list; |
| |
| sensors_event_t accEvent[SENSOR_EVENT_BUFFER_SIZE]; |
| sensors_event_t gyroEvent[SENSOR_EVENT_BUFFER_SIZE]; |
| sensors_event_t baroEvent[BARO_EVENT_BUFFER_SIZE]; |
| int (*get_sensors_list)(struct sensor_t const** list); |
| |
| sensors_poll_device_1_t* halInf; |
| |
| //rectify time for sensor |
| struct timespec spec; |
| |
| struct timespec odom_boottime; |
| } sensor_device; |
| |
| static sensor_device *g_sensor_ctx = NULL; |
| static int sensor_data_number = 0; |
| |
| int32_t get_sensor_handle(sensor_device *dev, int type) |
| { |
| int i = 0; |
| for (i = 0; i < (int)(dev->sensor_count); i++) { |
| if (dev->sensor_list[i].type == type) |
| return dev->sensor_list[i].handle; |
| } |
| return -1; |
| } |
| |
| int32_t sensor_set_event_rate(int handle, int64_t samplingPeriodNs, int odr) |
| { |
| int64_t sampling_ns = samplingPeriodNs; |
| if (odr == 1) |
| { |
| int odr_us[8] = {0}; // 6.25,12.5,25,50,100,400,800 |
| int i = 0,ret = -1; |
| ret = getODR(g_sensor_ctx->halInf, handle, odr_us); |
| if (ret) |
| { |
| LOG_ERROR("sensor get ODR return fail , ret = %d",ret); |
| return -1; |
| } |
| int sampling_us = (int)(sampling_ns / 1000); |
| /* get the nearest value */ |
| /*int d_value = abs(odr_us[0] - sampling_us), index = 0, tmp; |
| for (i = 1; i < 8; i++) |
| { |
| LOG_DEBUG("odr[%d] = %d\n", i, odr_us[i]); |
| tmp = abs(odr_us[i] - sampling_us); |
| if (tmp < d_value) { |
| index = i; |
| d_value = tmp; |
| } |
| } |
| sampling_ns = odr_us[index] * 1000; |
| } |
| LOG_DEBUG("sampling rate %lld, the end sampling rate to be set to driver is %lld",samplingPeriodNs, sampling_ns);*/ |
| for (i = 0; i < 8; i++) |
| { |
| LOG_DEBUG("odr[%d] = %d\n", i, odr_us[i]); |
| if (odr_us[i] == sampling_us) |
| break; |
| } |
| if (i == 8) { |
| LOG_DEBUG("sampling rate %lld is not matched with sensor odr setting",samplingPeriodNs); |
| } |
| } |
| return setDelay(g_sensor_ctx->halInf, handle, sampling_ns); |
| } |
| |
| static int sensor_stop_thread(sensor_device *dev) |
| { |
| dev->poll_thread_exit = 1; |
| dev->dataready_thread_exit = 1; |
| LOG_DEBUG("sensor thread exit~"); |
| pthread_join(dev->poll_thread, NULL); |
| pthread_join(dev->dataready_thread, NULL); |
| return 0; |
| } |
| |
| /* |
| * @ get ring arrary index, |
| * member: |
| * @len: the length of arrary |
| * @cur: cur positoon |
| * @offset: the offset of @in |
| * @out: the final index of the ring |
| */ |
| #define get_ring_idx(len, cur, offset, out) \ |
| do \ |
| { \ |
| if ((cur + offset) >= len) { \ |
| out = cur + offset - len; \ |
| } else if((cur + offset) < 0) { \ |
| out = len + (cur + offset); \ |
| } else { \ |
| out = cur + offset; \ |
| } \ |
| }while (0) |
| |
| #define get_ring_avilable_len(len, start, end, out) \ |
| do \ |
| { \ |
| out = end + len - start; \ |
| if (out >= len) \ |
| out -= len; \ |
| }while (0) |
| |
| //just cache event & calcu len of acc/gyro |
| void sensor_data_print(sensor_device *dev, const sensors_event_t* buffer, size_t count) |
| { |
| size_t i = 0; |
| int32_t next = 0; |
| |
| LOG_INFO("Event number %d\n", count); |
| sensor_data_number += count; |
| for (i = 0; i < count; i++) { |
| const sensors_event_t* event = &buffer[i]; |
| if (event->type != SENSOR_TYPE_META_DATA) { |
| if (!event->timestamp) |
| continue; |
| switch (event->type) { |
| case SENSOR_TYPE_ACCELEROMETER: |
| dev->accEvent[0] = *event; |
| LOG_TEST_INFO("acc: x %f, y %f, z %f, temp %f, time %lld\n", |
| dev->accEvent[0].acceleration.x, dev->accEvent[0].acceleration.y, |
| dev->accEvent[0].acceleration.z, dev->accEvent[0].acceleration.t, |
| dev->accEvent[0].timestamp); |
| break; |
| |
| case SENSOR_TYPE_GYROSCOPE: |
| dev->gyroEvent[0] = *event; |
| LOG_TEST_INFO("gyro: x %f, y %f, z %f, temp %f, time %lld\n", |
| dev->gyroEvent[0].gyro.azimuth, dev->gyroEvent[0].gyro.pitch, |
| dev->gyroEvent[0].gyro.roll, dev->gyroEvent[0].gyro.temperature, |
| dev->gyroEvent[0].timestamp); |
| break; |
| |
| case SENSOR_TYPE_PRESSURE: |
| dev->baroEvent[0] = *event; |
| LOG_TEST_INFO("Pressure: press %f, temp %f, time %lld\n", |
| dev->baroEvent[0].pressure.data, |
| dev->baroEvent[0].pressure.temperature, |
| dev->baroEvent[0].timestamp); |
| break; |
| |
| default: |
| break; |
| } |
| } |
| } |
| } |
| |
| static void * |
| sensor_poll_thread(void *data) |
| { |
| sensor_device *dev = (sensor_device *)data; |
| sensors_event_t mSensorEvent[SENSOR_EVENT_BUFFER_SIZE]; |
| ssize_t count = 0; |
| |
| sleep(3); |
| |
| while (!dev->poll_thread_exit) { |
| count = service_poll(dev->halInf, mSensorEvent, SENSOR_EVENT_BUFFER_SIZE); |
| sensor_data_print(dev, mSensorEvent, count); |
| if (sensor_data_number > 100) { |
| LOG_INFO("sensor_data_number %d\n", sensor_data_number); |
| dev->poll_thread_exit = 1; |
| } |
| } |
| LOG_INFO("<-- sensor_poll_thread exit"); |
| pthread_exit(0); |
| } |
| |
| static int32_t sensor_hal_deinit(sensor_device* dev) |
| { |
| free(dev->sensor_list); |
| dlclose(dev->sensor_hal); |
| |
| return 0; |
| } |
| |
| static int32_t sensor_hal_init(sensor_device* dev) |
| { |
| void *module, *get_sensors_list; |
| int (*sensors_open)(sensors_poll_device_1_t** mSensorDevice); |
| |
| module = dlopen(SENSOR_LIB, RTLD_NOW | RTLD_NOLOAD); |
| if (module) { |
| LOG_ERROR("Module '%s' already loaded", SENSOR_LIB); |
| goto ERROR; |
| } |
| |
| LOG_DEBUG("Loading module '%s'", SENSOR_LIB); |
| module = dlopen(SENSOR_LIB, RTLD_NOW); |
| if (!module) { |
| LOG_ERROR("Failed to load module: %s", dlerror()); |
| return -1; |
| } |
| sensors_open = dlsym(module, "sensor_open"); |
| if (!sensors_open) { |
| LOG_ERROR("Failed to lookup sensors_open function: %s", dlerror()); |
| goto ERROR; |
| } |
| |
| get_sensors_list = dlsym(module, "sensors__get_sensors_list"); |
| if (!get_sensors_list) { |
| LOG_ERROR("Failed to lookup get_sensors_list function: %s", dlerror()); |
| goto ERROR; |
| } |
| dev->get_sensors_list = get_sensors_list; |
| sensors_open(&dev->halInf); |
| |
| if (dev->halInf) { |
| struct sensor_t const* list; |
| ssize_t count = dev->get_sensors_list(&list); |
| |
| LOG_DEBUG("get_sensors_list count %d",(int)count); |
| dev->sensor_count = count; |
| dev->sensor_list = (struct sensor_t *)zalloc(count * sizeof(struct sensor_t)); |
| if(!dev->sensor_list) |
| goto ERROR; |
| |
| memcpy(dev->sensor_list, list, count * sizeof(struct sensor_t)); |
| if (count > 0) { |
| size_t i = 0 ; |
| LOG_DEBUG("------------------------sensor list BEGIN---------------------------"); |
| for (i=0 ; i< count ; i++) { |
| dev->halInf->activate( |
| (struct sensors_poll_device_t *)(dev->halInf), |
| list[i].handle, SENSOR_DISABLE); |
| LOG_DEBUG("name:%s version:%d maxRange:%f\n",list[i].name, list[i].version, list[i].maxRange); |
| } |
| LOG_DEBUG("------------------------sensor list END---------------------------"); |
| } |
| } |
| dev->sensor_hal = module; |
| return 0; |
| ERROR: |
| dlclose(module); |
| return -1; |
| } |
| |
| static int32_t sensor_driver_init(struct sensor_device *dev) |
| { |
| int32_t ret; |
| |
| ret = sensor_hal_init(dev); |
| |
| dev->poll_thread_exit = 0; |
| ret = pthread_create(&dev->poll_thread, NULL, sensor_poll_thread, (void *)dev); |
| if (ret != 0) { |
| LOG_ERROR("can't create thread for sensor poll:%s",strerror(errno)); |
| return -1; |
| } |
| |
| return 0; |
| } |
| |
| static void sensor_driver_start(struct sensor_device *dev) |
| { |
| int32_t handle, samplingPeriodNs; |
| |
| samplingPeriodNs = SENSOR_POLL_NS; |
| |
| handle = get_sensor_handle(dev, SENSOR_TYPE_ACCELEROMETER); |
| activate(dev->halInf, handle, SENSOR_ENABLE); |
| sensor_set_event_rate(handle, samplingPeriodNs, 1); |
| |
| handle = get_sensor_handle(dev, SENSOR_TYPE_GYROSCOPE); |
| activate(dev->halInf, handle, SENSOR_ENABLE); |
| sensor_set_event_rate(handle, samplingPeriodNs, 1); |
| |
| handle = get_sensor_handle(dev, SENSOR_TYPE_PRESSURE); |
| activate(dev->halInf, handle, SENSOR_ENABLE); |
| sensor_set_event_rate(handle , samplingPeriodNs, 0); |
| } |
| |
| static void sensor_driver_stop(struct sensor_device *dev) |
| { |
| int32_t handle; |
| |
| sensor_stop_thread(dev); |
| |
| handle = get_sensor_handle(dev, SENSOR_TYPE_ACCELEROMETER); |
| activate(dev->halInf, handle, SENSOR_DISABLE); |
| |
| handle = get_sensor_handle(dev, SENSOR_TYPE_GYROSCOPE); |
| activate(dev->halInf, handle, SENSOR_DISABLE); |
| |
| handle = get_sensor_handle(dev, SENSOR_TYPE_PRESSURE); |
| activate(dev->halInf, handle, SENSOR_DISABLE); |
| } |
| |
| static void sensor_driver_deinit(struct sensor_device *dev) |
| { |
| sensor_hal_deinit(dev); |
| } |
| |
| struct sensorhal { |
| char *name; |
| int32_t (*init)(struct sensor_device *dev); |
| void (*start)(struct sensor_device *dev); |
| void (*stop)(struct sensor_device *dev); |
| void (*deinit)(struct sensor_device *dev); |
| }; |
| |
| static struct sensorhal sensor_driver = { |
| .name = "mtk_sensor", |
| .init = sensor_driver_init, |
| .start = sensor_driver_start, |
| .stop = sensor_driver_stop, |
| .deinit = sensor_driver_deinit, |
| }; |
| |
| int module_sensor_init(void) |
| { |
| struct sensor_device *dev; |
| |
| dev = (sensor_device*)zalloc(sizeof(sensor_device)); |
| if (!dev) { |
| LOG_ERROR("Fail to create sensor_device"); |
| return -1; |
| } |
| g_sensor_ctx = dev; |
| sensor_driver_init(dev); |
| return 0; |
| } |
| |
| int main() |
| { |
| int err = 0; |
| struct sensor_device *dev; |
| |
| err = module_sensor_init(); |
| if (err < 0) { |
| LOG_INFO("Create sensor_device err\n"); |
| return -1; |
| } |
| dev = g_sensor_ctx; |
| |
| LOG_INFO("Enable and set data rate to sensor driver\n"); |
| sensor_driver_start(dev); |
| |
| do { |
| sleep(1); |
| } while (dev->poll_thread_exit == 0); |
| |
| LOG_INFO("Disable sensor\n"); |
| sensor_driver_stop(dev); |
| |
| LOG_INFO("Unload sensorhal.so\n"); |
| sensor_driver_deinit(dev); |
| |
| return 0; |
| } |