[Feature]add MT2731_MP2_MR2_SVN388 baseline version
Change-Id: Ief04314834b31e27effab435d3ca8ba33b499059
diff --git a/src/navigation/sensor/sensorhaltest/LICENSE b/src/navigation/sensor/sensorhaltest/LICENSE
new file mode 100644
index 0000000..77f59ed
--- /dev/null
+++ b/src/navigation/sensor/sensorhaltest/LICENSE
@@ -0,0 +1,31 @@
+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) 2015. 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.
diff --git a/src/navigation/sensor/sensorhaltest/Makefile.am b/src/navigation/sensor/sensorhaltest/Makefile.am
new file mode 100644
index 0000000..f2460dc
--- /dev/null
+++ b/src/navigation/sensor/sensorhaltest/Makefile.am
@@ -0,0 +1,9 @@
+AUTOMAKE_OPTIONS = foreign subdir-objects
+bin_PROGRAMS = sensorhal_test
+AM_CFLAGS = -P
+sensorhal_test_SOURCES = main.c \
+ module_sensor_helper.c \
+ log.c
+
+sensorhal_test_LDADD = -lpthread -ldl -lm -ludev
+#AM_LDCLAGS = -lpthread -ldl -lm -lgpshal -llibadr
diff --git a/src/navigation/sensor/sensorhaltest/configure.ac b/src/navigation/sensor/sensorhaltest/configure.ac
new file mode 100644
index 0000000..8112574
--- /dev/null
+++ b/src/navigation/sensor/sensorhaltest/configure.ac
@@ -0,0 +1,5 @@
+AC_INIT(main, 1.0)
+AM_INIT_AUTOMAKE(foreign)
+AM_MAINTAINER_MODE
+AC_PROG_CC
+AC_OUTPUT([Makefile])
diff --git a/src/navigation/sensor/sensorhaltest/log.c b/src/navigation/sensor/sensorhaltest/log.c
new file mode 100644
index 0000000..e24f745
--- /dev/null
+++ b/src/navigation/sensor/sensorhaltest/log.c
@@ -0,0 +1,95 @@
+/*
+* 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.
+*/
+#include <sys/time.h>
+#include <stdarg.h>
+
+#include "log.h"
+
+typedef enum
+{
+ QUIET_L = 0,
+ ERROR_L,
+ WARNING_L,
+ INFO_L,
+ DEBUG_L,
+ LOGALL,
+ LogLevel_Cnt,
+} LogLevelDef;
+
+
+int log_dbg_level = (DEBUG_L);
+
+unsigned int get_time(void)
+{
+ struct timeval tv;
+
+ gettimeofday(&tv, NULL);
+
+ return tv.tv_sec * 1000 + tv.tv_usec / 1000;
+}
+
+extern int get_log_level()
+{
+ printf("Current debug level=%d", (int)log_dbg_level);
+
+ return log_dbg_level;
+}
+
+
+extern int set_log_level(int fg_enable, int level)
+{
+ printf(" Enter %s \n", __FUNCTION__);
+ printf("Current debug level=%d", (int)log_dbg_level);
+
+ if (level < QUIET_L || level > LOGALL)
+ {
+ printf("Invalid debug level, level=%d @ %s\n", level, __FUNCTION__);
+ printf(" [level] -\n");
+ printf(" QUIET = 0 \n");
+ printf(" ERROR = 1 \n");
+ printf(" WARNING = 2 \n");
+ printf(" INFO = 3 \n");
+ printf(" DEBUG = 4 \n");
+ printf(" LogAll = 5 \n");
+
+ return -1;
+ }
+
+ log_dbg_level = level;
+
+ printf(" Set log debug level successfully\n");
+ printf("New debug level=%d ", (int)log_dbg_level);
+
+ return 0;
+}
\ No newline at end of file
diff --git a/src/navigation/sensor/sensorhaltest/log.h b/src/navigation/sensor/sensorhaltest/log.h
new file mode 100644
index 0000000..1cdfc1d
--- /dev/null
+++ b/src/navigation/sensor/sensorhaltest/log.h
@@ -0,0 +1,69 @@
+/*
+* 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.
+*/
+#ifndef LOG_H
+#define LOG_H
+
+#include <stdio.h>
+
+extern int log_dbg_level;
+
+#define LOG_IS_ENABLED(level) \
+ (log_dbg_level >= level)
+
+unsigned int get_time(void);
+int set_log_level(int fg_enable, int level);
+int get_log_level();
+
+#ifndef gettid
+#include <unistd.h>
+#include <sys/syscall.h>
+#define gettid() syscall(__NR_gettid)
+#endif
+
+#define PRINT_LOG(loglevel, tag, fmt, args...) \
+ do {\
+ if (LOG_IS_ENABLED(loglevel)) {\
+ printf(tag " %d " fmt "\n",\
+ __LINE__, ##args);\
+ fflush(stdout);\
+ }\
+ } while (0)
+
+#define LOG_ERROR(fmt, args...) PRINT_LOG(1, "[ERROR]: ", fmt, ##args)
+#define LOG_TEST_INFO(fmt, args...) PRINT_LOG(1, "[TEST_INFO]: ", fmt, ##args)
+#define LOG_WARN(fmt, args...) PRINT_LOG(2, "[WARNING]: ", fmt, ##args)
+#define LOG_INFO(fmt, args...) PRINT_LOG(3, "[INFO]: ", fmt, ##args)
+#define LOG_DEBUG(fmt, args...) PRINT_LOG(4, "[DEBUG]: ", fmt, ##args)
+
+#endif
diff --git a/src/navigation/sensor/sensorhaltest/main.c b/src/navigation/sensor/sensorhaltest/main.c
new file mode 100644
index 0000000..a581504
--- /dev/null
+++ b/src/navigation/sensor/sensorhaltest/main.c
@@ -0,0 +1,432 @@
+/*
+* 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;
+}
diff --git a/src/navigation/sensor/sensorhaltest/module_sensor_helper.c b/src/navigation/sensor/sensorhaltest/module_sensor_helper.c
new file mode 100644
index 0000000..993d96a
--- /dev/null
+++ b/src/navigation/sensor/sensorhaltest/module_sensor_helper.c
@@ -0,0 +1,91 @@
+/*
+* 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.
+*/
+
+#include "module_sensor_helper.h"
+
+ssize_t service_poll(sensors_poll_device_1_t* inf, sensors_event_t* buffer, size_t count)
+{
+ if (!inf) return 0;
+ ssize_t c;
+ //do
+ {
+ c = inf->poll((struct sensors_poll_device_t *) (inf),
+ buffer, count);
+ }
+ //while (c == -EINTR);
+ return c;
+}
+
+
+int32_t activate(sensors_poll_device_1_t* inf, int handle, int enabled)
+{
+ if (!inf) return -1;
+
+ //add mutex lock -- todo
+ int32_t err;
+
+ LOG_DEBUG("\t>>> actuating h/w activate handle=%d enabled=%d\n", handle, enabled);
+ err = inf->activate(
+ (struct sensors_poll_device_t *) (inf), handle, enabled);
+
+ if (err)
+ {
+ LOG_DEBUG("Error %s sensor %d (%s)", enabled ? "activating" : "disabling", handle,
+ strerror(-err));
+ }
+
+ return err;
+}
+
+int32_t setDelay(sensors_poll_device_1_t* inf, int handle, int64_t samplingPeriodNs)
+{
+ if (!inf) return -1;
+
+ return inf->setDelay((struct sensors_poll_device_t *)(inf),
+ handle, samplingPeriodNs);
+}
+
+int32_t getODR(sensors_poll_device_1_t* inf, int handle, int *odr)
+{
+ if (!inf) return -1;
+ return inf->getODR((struct sensors_poll_device_t *)(inf),
+ handle, odr);
+}
+
+int32_t flush(sensors_poll_device_1_t* inf, int handle)
+{
+ LOG_DEBUG("\t>>> actuating h/w flush %d", handle);
+ return inf->flush(inf, handle);
+}
+
diff --git a/src/navigation/sensor/sensorhaltest/module_sensor_helper.h b/src/navigation/sensor/sensorhaltest/module_sensor_helper.h
new file mode 100644
index 0000000..a67236c
--- /dev/null
+++ b/src/navigation/sensor/sensorhaltest/module_sensor_helper.h
@@ -0,0 +1,76 @@
+/*
+* 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.
+*/
+
+#ifndef MODULE_SENSOR_HELPER_H
+#define MODULE_SENSOR_HELPER_H
+
+#include <errno.h>
+#include <string.h>
+
+#include "log.h"
+#include "sensors.h"
+
+typedef struct sensor_module_data {
+ double ts;
+ int ts_ms;
+ double acc_x;
+ double acc_y;
+ double acc_z;
+ double acc_T;
+ /* time is in nanosecond */
+ int64_t gyro_ns;
+ double gyro_x;
+ double gyro_y;
+ double gyro_z;
+ double gyro_T;
+ double mag_x;
+ double mag_y;
+ double mag_z;
+ float baro;
+ float baroT;
+
+ /* from uart inteface*/
+ float odom;
+ int gear;
+ float rudder;
+
+ struct timespec sensor_boottime;
+} sensor_module_data;
+
+ssize_t service_poll(sensors_poll_device_1_t* inf, sensors_event_t* buffer, size_t count);
+int32_t activate(sensors_poll_device_1_t* inf, int handle, int enabled);
+int32_t setDelay(sensors_poll_device_1_t* inf, int handle, int64_t samplingPeriodNs);
+int32_t getODR(sensors_poll_device_1_t* inf, int handle, int *odr);
+
+#endif
diff --git a/src/navigation/sensor/sensorhaltest/zalloc.h b/src/navigation/sensor/sensorhaltest/zalloc.h
new file mode 100644
index 0000000..8b7655f
--- /dev/null
+++ b/src/navigation/sensor/sensorhaltest/zalloc.h
@@ -0,0 +1,54 @@
+/*
+* 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.
+*/
+
+#ifndef ADR_ZALLOC_H
+#define ADR_ZALLOC_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdlib.h>
+
+static inline void *
+zalloc(size_t size)
+{
+ return calloc(1, size);
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* ADR_ZALLOC_H */