[Feature]add MT2731_MP2_MR2_SVN388 baseline version
Change-Id: Ief04314834b31e27effab435d3ca8ba33b499059
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;
+}