src/navigation/sensor/sensorhaltest/main.c

/*
* 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;
}