#include <pthread.h>
|
#include <signal.h>
|
#include <stdbool.h>
|
#include <stdint.h>
|
#include <stdio.h>
|
#include <stdlib.h>
|
#include <string.h>
|
#include <unistd.h>
|
#include <inttypes.h>
|
|
#include "rk_aiq_mems_sensor.h"
|
#include "rkiio.h"
|
|
//#define IIO_TEST
|
|
typedef struct rkiio_sensor_ctx_s {
|
uint64_t ctx_id;
|
} rkiio_sensor_ctx_t;
|
|
typedef struct rkiio_sensor_handle_buf_s {
|
pthread_mutex_t buf_mutex;
|
bool is_valid;
|
void* buf;
|
} rkiio_sensor_handle_buf_t;
|
|
typedef struct rkiio_sensor_handle_ctx_s {
|
mems_sensor_type_t sensor_type;
|
RKIIO_DATA_HANDLE handle;
|
uint32_t max_data_num;
|
uint32_t buf_cnt;
|
rkiio_sensor_handle_buf_t* buf_pool;
|
pthread_mutex_t fifo_buf_mutex;
|
RKIIO_FIFO_DATA fifo_data_buf;
|
} rkiio_sensor_handle_t;
|
|
static pthread_mutex_t g_mutex = PTHREAD_MUTEX_INITIALIZER;
|
static bool g_init_flag = false;
|
static uint32_t g_sensor_cnt = 0;
|
static uint32_t g_handle_cnt = 0;
|
|
|
static mems_sensor_ctx_t rkiio_aiq_sensor_ctx_create() {
|
pthread_mutex_lock(&g_mutex);
|
g_sensor_cnt++;
|
if (!g_init_flag) {
|
rkiio_sys_init();
|
rkiio_timestamp_type_set_all(RKIIO_TIMESTAMP_MONOTONIC);
|
}
|
g_init_flag = true;
|
pthread_mutex_unlock(&g_mutex);
|
rkiio_sensor_ctx_t* ctx = (rkiio_sensor_ctx_t*)malloc(sizeof(rkiio_sensor_ctx_t));
|
memset(ctx, 0, sizeof(rkiio_sensor_ctx_t));
|
return (mems_sensor_ctx_t)ctx;
|
}
|
|
static mems_sensor_return_t rkiio_aiq_sensor_ctx_destroy(mems_sensor_ctx_t ctx) {
|
if (!ctx) {
|
return SENSOR_ERR_NULL_PTR;
|
}
|
rkiio_sensor_ctx_t* rkiio_ctx = (rkiio_sensor_ctx_t*)ctx;
|
free(rkiio_ctx);
|
pthread_mutex_lock(&g_mutex);
|
g_sensor_cnt--;
|
if (!g_sensor_cnt) {
|
rkiio_sys_uninit();
|
g_init_flag = false;
|
}
|
pthread_mutex_unlock(&g_mutex);
|
return SENSOR_NO_ERR;
|
}
|
|
static mems_sensor_return_t rkiio_aiq_sensors_list_get(mems_sensor_ctx_t ctx,
|
mems_sensor_type_t sensor_type,
|
mems_sensor_list_t* sensors_list) {
|
if (!g_init_flag) {
|
return SENSOR_ERR_FAIL;
|
}
|
if (!sensors_list || !ctx) {
|
return SENSOR_ERR_NULL_PTR;
|
}
|
switch (sensor_type) {
|
case SENSOR_GRYO_TYPE:
|
sensors_list->key_list = rkiio_anglvel_dev_list_get(&sensors_list->count);
|
break;
|
case SENSOR_ACCEL_TYPE:
|
sensors_list->key_list = rkiio_accel_dev_list_get(&sensors_list->count);
|
break;
|
case SENSOR_TEMP_TYPE:
|
sensors_list->key_list = rkiio_temp_dev_list_get(&sensors_list->count);
|
break;
|
case SENSOR_ALL_TYPE:
|
sensors_list->key_list = rkiio_all_dev_list_get(&sensors_list->count);
|
break;
|
default:
|
return SENSOR_ERR_MISMATCH_SENSOR;
|
}
|
if (sensors_list->count) {
|
return SENSOR_NO_ERR;
|
}
|
return SENSOR_ERR_MISMATCH_SENSOR;
|
}
|
|
static mems_sensor_return_t rkiio_aiq_sensors_list_release(mems_sensor_list_t* sensors_list) {
|
if (!sensors_list) {
|
return SENSOR_ERR_NULL_PTR;
|
}
|
int32_t ret = rkiio_dev_list_destroy(sensors_list->key_list, sensors_list->count);
|
if (ret) {
|
return SENSOR_ERR_FAIL;
|
}
|
return SENSOR_NO_ERR;
|
}
|
|
static mems_sensor_return_t rkiio_aiq_sensor_cap_get(mems_sensor_ctx_t ctx,
|
mems_sensor_type_t sensor_type,
|
char* sensor_key,
|
mems_sensor_capabilities_t* caps) {
|
if (!g_init_flag) {
|
return SENSOR_ERR_FAIL;
|
}
|
if (!ctx) {
|
return SENSOR_ERR_NULL_PTR;
|
}
|
memset(caps, 0, sizeof(mems_sensor_capabilities_t));
|
caps->type = sensor_type;
|
switch (sensor_type) {
|
case SENSOR_GRYO_TYPE:
|
rkiio_sampling_rate_cap_get(sensor_key, RKIIO_SENSOR_ANGLVEL_TYPE,
|
&(caps->sample_rates), &(caps->num_sample_rates));
|
break;
|
case SENSOR_ACCEL_TYPE:
|
rkiio_sampling_rate_cap_get(sensor_key, RKIIO_SENSOR_ACCEL_TYPE, &(caps->sample_rates),
|
&(caps->num_sample_rates));
|
break;
|
case SENSOR_TEMP_TYPE:
|
rkiio_sampling_rate_cap_get(sensor_key, RKIIO_SENSOR_TEMP_TYPE, &(caps->sample_rates),
|
&(caps->num_sample_rates));
|
break;
|
case SENSOR_ALL_TYPE:
|
rkiio_sampling_rate_cap_get(sensor_key, RKIIO_SENSOR_ALL_TYPE, &(caps->sample_rates),
|
&(caps->num_sample_rates));
|
break;
|
default:
|
return SENSOR_ERR_MISMATCH_SENSOR;
|
}
|
caps->is_data_valid = true;
|
return SENSOR_NO_ERR;
|
}
|
|
static mems_sensor_return_t rkiio_aiq_sensor_cap_release(mems_sensor_capabilities_t* caps) {
|
if (caps->sample_rates) rkiio_sampling_rate_cap_release(caps->sample_rates);
|
return SENSOR_NO_ERR;
|
}
|
|
// only support sampling rate
|
static mems_sensor_return_t rkiio_aiq_sensor_config_get(mems_sensor_ctx_t ctx,
|
mems_sensor_type_t sensor_type,
|
char* sensor_key,
|
mems_sensor_config_t* cfg) {
|
if (!g_init_flag) {
|
return SENSOR_ERR_FAIL;
|
}
|
if (!cfg || !ctx) {
|
return SENSOR_ERR_NULL_PTR;
|
}
|
if (g_handle_cnt) {
|
return SENSOR_ERR_DEV_BUSY;
|
}
|
memset(cfg, 0, sizeof(mems_sensor_config_t));
|
int32_t ret = 0;
|
switch (sensor_type) {
|
case SENSOR_GRYO_TYPE:
|
ret =
|
rkiio_sampling_rate_get(sensor_key, RKIIO_SENSOR_ANGLVEL_TYPE, &(cfg->sample_rate));
|
break;
|
case SENSOR_ACCEL_TYPE:
|
ret = rkiio_sampling_rate_get(sensor_key, RKIIO_SENSOR_ACCEL_TYPE, &(cfg->sample_rate));
|
break;
|
case SENSOR_TEMP_TYPE:
|
ret = rkiio_sampling_rate_get(sensor_key, RKIIO_SENSOR_TEMP_TYPE, &(cfg->sample_rate));
|
break;
|
case SENSOR_ALL_TYPE:
|
ret = rkiio_sampling_rate_get(sensor_key, RKIIO_SENSOR_ALL_TYPE, &(cfg->sample_rate));
|
break;
|
default:
|
return SENSOR_ERR_MISMATCH_SENSOR;
|
}
|
if (ret) {
|
cfg->sample_rate = 0.0f;
|
return SENSOR_ERR_FAIL;
|
}
|
return SENSOR_NO_ERR;
|
}
|
|
static mems_sensor_return_t rkiio_aiq_sensor_config_set(mems_sensor_ctx_t ctx,
|
mems_sensor_type_t sensor_type,
|
char* sensor_key,
|
mems_sensor_config_t cfg) {
|
if (!g_init_flag) {
|
return SENSOR_ERR_FAIL;
|
}
|
if (!ctx) {
|
return SENSOR_ERR_NULL_PTR;
|
}
|
if (g_handle_cnt) {
|
return SENSOR_ERR_DEV_BUSY;
|
}
|
int32_t ret = 0;
|
switch (sensor_type) {
|
case SENSOR_GRYO_TYPE:
|
ret = rkiio_sampling_rate_set(sensor_key, RKIIO_SENSOR_ANGLVEL_TYPE, cfg.sample_rate);
|
break;
|
case SENSOR_ACCEL_TYPE:
|
ret = rkiio_sampling_rate_set(sensor_key, RKIIO_SENSOR_ACCEL_TYPE, cfg.sample_rate);
|
break;
|
case SENSOR_TEMP_TYPE:
|
ret = rkiio_sampling_rate_set(sensor_key, RKIIO_SENSOR_TEMP_TYPE, cfg.sample_rate);
|
break;
|
case SENSOR_ALL_TYPE:
|
ret = rkiio_sampling_rate_set(sensor_key, RKIIO_SENSOR_ALL_TYPE, cfg.sample_rate);
|
break;
|
default:
|
return SENSOR_ERR_MISMATCH_SENSOR;
|
}
|
if (ret) {
|
return SENSOR_ERR_FAIL;
|
}
|
return SENSOR_NO_ERR;
|
}
|
|
static uint32_t _rkiio_aiq_buf_id_get(void* buf, uint32_t max_data_num) {
|
uint8_t* data = (uint8_t*)buf;
|
uint64_t idx = sizeof(mems_sensor_event_t) * max_data_num;
|
int32_t* reserverd_data = (int32_t*)&(data[idx]);
|
return *reserverd_data;
|
}
|
|
static void _rkiio_aiq_buf_id_set(void* buf, uint32_t max_data_num, uint32_t id) {
|
uint8_t* data = (uint8_t*)buf;
|
uint64_t idx = sizeof(mems_sensor_event_t) * max_data_num;
|
int32_t* reserverd_data = (int32_t*)&(data[idx]);
|
*reserverd_data = id;
|
}
|
|
static mems_sensor_handle_t rkiio_aiq_sensor_handle_create(mems_sensor_ctx_t ctx,
|
mems_sensor_type_t sensor_type,
|
char* sensor_key, uint32_t max_data_num,
|
uint32_t buf_cnt) {
|
if (!g_init_flag || !ctx || !max_data_num || !buf_cnt) {
|
return NULL;
|
}
|
rkiio_sensor_handle_t* sensor_handle =
|
(rkiio_sensor_handle_t*)malloc(sizeof(rkiio_sensor_handle_t));
|
memset(sensor_handle, 0, sizeof(rkiio_sensor_handle_t));
|
sensor_handle->sensor_type = sensor_type;
|
sensor_handle->buf_cnt = buf_cnt;
|
|
sensor_handle->handle = rkiio_data_handle_create(sensor_key, 0, max_data_num);
|
uint32_t real_max_data_num = 0;
|
rkiio_data_handle_max_fifo_num_get(sensor_handle->handle, &real_max_data_num);
|
sensor_handle->max_data_num =
|
(max_data_num > real_max_data_num) ? max_data_num : real_max_data_num;
|
|
pthread_mutex_init(&(sensor_handle->fifo_buf_mutex), NULL);
|
sensor_handle->fifo_data_buf =
|
(RKIIO_FIFO_DATA)malloc(sizeof(rkiio_data_0_t) * sensor_handle->max_data_num);
|
|
sensor_handle->buf_pool =
|
(rkiio_sensor_handle_buf_t*)malloc(sizeof(rkiio_sensor_handle_buf_t) * buf_cnt);
|
memset(sensor_handle->buf_pool, 0, sizeof(rkiio_sensor_handle_buf_t) * buf_cnt);
|
for (uint32_t i = 0; i < buf_cnt; i++) {
|
pthread_mutex_init(&(sensor_handle->buf_pool[i].buf_mutex), NULL);
|
sensor_handle->buf_pool[i].is_valid = true;
|
sensor_handle->buf_pool[i].buf = (void*)malloc(
|
sizeof(mems_sensor_event_t) * sensor_handle->max_data_num + sizeof(uint32_t));
|
memset(sensor_handle->buf_pool[i].buf, 0,
|
sizeof(mems_sensor_event_t) * sensor_handle->max_data_num + sizeof(uint32_t));
|
_rkiio_aiq_buf_id_set(sensor_handle->buf_pool[i].buf, sensor_handle->max_data_num, i);
|
}
|
pthread_mutex_lock(&g_mutex);
|
g_handle_cnt++;
|
pthread_mutex_unlock(&g_mutex);
|
return (mems_sensor_handle_t)sensor_handle;
|
}
|
|
static mems_sensor_return_t rkiio_aiq_sensor_handle_destroy(mems_sensor_handle_t handle) {
|
if (!handle) {
|
return SENSOR_ERR_NULL_PTR;
|
}
|
rkiio_sensor_handle_t* sensor_handle = (rkiio_sensor_handle_t*)handle;
|
pthread_mutex_lock(&(sensor_handle->fifo_buf_mutex));
|
rkiio_data_handle_destroy(sensor_handle->handle);
|
if (sensor_handle->fifo_data_buf) free(sensor_handle->fifo_data_buf);
|
if (sensor_handle->buf_pool) {
|
for (uint32_t i = 0; i < sensor_handle->buf_cnt; i++) {
|
if (sensor_handle->buf_pool[i].buf) free(sensor_handle->buf_pool[i].buf);
|
}
|
free(sensor_handle->buf_pool);
|
}
|
pthread_mutex_unlock(&(sensor_handle->fifo_buf_mutex));
|
free(sensor_handle);
|
pthread_mutex_lock(&g_mutex);
|
g_handle_cnt--;
|
pthread_mutex_unlock(&g_mutex);
|
return SENSOR_NO_ERR;
|
}
|
|
static uint32_t _rkiio_aiq_get_vaild_buf(rkiio_sensor_handle_t* handle) {
|
while (1) {
|
for (uint32_t i = 0; i < handle->buf_cnt; i++) {
|
pthread_mutex_lock(&(handle->buf_pool[i].buf_mutex));
|
// printf("%s: id: %u, valid: %d\n", __FUNCTION__, i, handle->buf_pool[i].is_valid);
|
if (handle->buf_pool[i].is_valid) {
|
handle->buf_pool[i].is_valid = false;
|
pthread_mutex_unlock(&(handle->buf_pool[i].buf_mutex));
|
return i;
|
}
|
pthread_mutex_unlock(&(handle->buf_pool[i].buf_mutex));
|
}
|
usleep(100000);
|
}
|
}
|
|
static mems_sensor_data_t _rkiio_aiq_getData_gyro(rkiio_sensor_handle_t* handle, uint64_t data_num) {
|
rkiio_data_0_t* fifo = (rkiio_data_0_t*)handle->fifo_data_buf;
|
uint32_t valid_id = _rkiio_aiq_get_vaild_buf(handle);
|
mems_sensor_event_t* event = (mems_sensor_event_t*)(handle->buf_pool[valid_id].buf);
|
memset(event, 0, sizeof(mems_sensor_event_t) * data_num);
|
for (uint64_t i = 0; i < data_num; i++) {
|
memcpy(&(event[i].gyro), &(fifo[i].anglvel), sizeof(xyz_data_t));
|
event[i].timestamp_us = fifo[i].timestamp;
|
event[i].id = fifo[i].id;
|
// printf("%llu:, x: %f, y: %f, z: %f, t: %llu, %llu:, x: %f, y: %f, z: %f, t: %llu\n",
|
// fifo[i].id, fifo[i].anglvel.x, fifo[i].anglvel.y, fifo[i].anglvel.z,
|
// fifo[i].timestamp, event[i].id, event[i].gyro.x, event[i].gyro.y, event[i].gyro.z,
|
// event[i].timestamp_us);
|
}
|
return (mems_sensor_data_t)event;
|
}
|
|
static mems_sensor_data_t _rkiio_aiq_getData_accel(rkiio_sensor_handle_t* handle, uint64_t data_num) {
|
rkiio_data_0_t* fifo = (rkiio_data_0_t*)handle->fifo_data_buf;
|
uint32_t valid_id = _rkiio_aiq_get_vaild_buf(handle);
|
mems_sensor_event_t* event = (mems_sensor_event_t*)(handle->buf_pool[valid_id].buf);
|
memset(event, 0, sizeof(mems_sensor_event_t) * data_num);
|
for (uint64_t i = 0; i < data_num; i++) {
|
memcpy(&(event[i].accel), &(fifo[i].accel), sizeof(xyz_data_t));
|
event[i].timestamp_us = fifo[i].timestamp;
|
event[i].id = fifo[i].id;
|
}
|
return (mems_sensor_data_t)event;
|
}
|
|
static mems_sensor_data_t _rkiio_aiq_getData_temp(rkiio_sensor_handle_t* handle, uint64_t data_num) {
|
rkiio_data_0_t* fifo = (rkiio_data_0_t*)handle->fifo_data_buf;
|
uint32_t valid_id = _rkiio_aiq_get_vaild_buf(handle);
|
mems_sensor_event_t* event = (mems_sensor_event_t*)(handle->buf_pool[valid_id].buf);
|
memset(event, 0, sizeof(mems_sensor_event_t) * data_num);
|
for (uint64_t i = 0; i < data_num; i++) {
|
// temp alter to s32?
|
event[i].temperature = fifo[i].temp;
|
event[i].timestamp_us = fifo[i].timestamp;
|
event[i].id = fifo[i].id;
|
}
|
return (mems_sensor_data_t)event;
|
}
|
|
static mems_sensor_data_t _rkiio_aiq_getData_all(rkiio_sensor_handle_t* handle, uint64_t data_num) {
|
rkiio_data_0_t* fifo = (rkiio_data_0_t*)handle->fifo_data_buf;
|
uint32_t valid_id = _rkiio_aiq_get_vaild_buf(handle);
|
mems_sensor_event_t* event = (mems_sensor_event_t*)(handle->buf_pool[valid_id].buf);
|
memset(event, 0, sizeof(mems_sensor_event_t) * data_num);
|
for (uint64_t i = 0; i < data_num; i++) {
|
memcpy(&(event[i].all.gyro), &(fifo[i].anglvel), sizeof(xyz_data_t));
|
memcpy(&(event[i].all.accel), &(fifo[i].accel), sizeof(xyz_data_t));
|
// temp alter to s32?
|
event[i].all.temperature = fifo[i].temp;
|
event[i].timestamp_us = fifo[i].timestamp;
|
event[i].id = fifo[i].id;
|
}
|
return (mems_sensor_data_t)event;
|
}
|
|
static mems_sensor_data_t rkiio_aiq_getData(mems_sensor_handle_t handle, size_t* num_samples) {
|
if (!handle) {
|
return NULL;
|
}
|
int32_t ret = 0;
|
rkiio_sensor_handle_t* sensor_handle = (rkiio_sensor_handle_t*)handle;
|
pthread_mutex_lock(&(sensor_handle->fifo_buf_mutex));
|
uint64_t data_num = 0;
|
ret = rkiio_data_get_all(sensor_handle->handle, sensor_handle->fifo_data_buf, &data_num);
|
if (ret || !data_num) {
|
*num_samples = 0;
|
pthread_mutex_unlock(&(sensor_handle->fifo_buf_mutex));
|
return NULL;
|
}
|
mems_sensor_data_t data = NULL;
|
switch (sensor_handle->sensor_type) {
|
case SENSOR_GRYO_TYPE:
|
data = _rkiio_aiq_getData_gyro(sensor_handle, data_num);
|
break;
|
case SENSOR_ACCEL_TYPE:
|
data = _rkiio_aiq_getData_accel(sensor_handle, data_num);
|
break;
|
case SENSOR_TEMP_TYPE:
|
data = _rkiio_aiq_getData_temp(sensor_handle, data_num);
|
break;
|
case SENSOR_ALL_TYPE:
|
data = _rkiio_aiq_getData_all(sensor_handle, data_num);
|
break;
|
default:
|
data = NULL;
|
}
|
*num_samples = data ? (size_t)data_num : 0;
|
pthread_mutex_unlock(&(sensor_handle->fifo_buf_mutex));
|
return data;
|
}
|
|
static mems_sensor_data_t rkiio_aiq_getLastNSamples(mems_sensor_handle_t handle,
|
size_t num_samples) {
|
if (!handle || !num_samples) {
|
return NULL;
|
}
|
rkiio_sensor_handle_t* sensor_handle = (rkiio_sensor_handle_t*)handle;
|
if ((uint32_t)num_samples > sensor_handle->max_data_num) {
|
printf("%s: num_samples > max_data_num\n", __FUNCTION__);
|
return NULL;
|
}
|
int32_t ret = 0;
|
pthread_mutex_lock(&(sensor_handle->fifo_buf_mutex));
|
uint64_t data_num = (uint64_t)num_samples;
|
ret = rkiio_data_read(sensor_handle->handle, sensor_handle->fifo_data_buf, data_num);
|
if (ret) {
|
pthread_mutex_unlock(&(sensor_handle->fifo_buf_mutex));
|
return NULL;
|
}
|
mems_sensor_data_t data = NULL;
|
switch (sensor_handle->sensor_type) {
|
case SENSOR_GRYO_TYPE:
|
data = _rkiio_aiq_getData_gyro(sensor_handle, data_num);
|
break;
|
case SENSOR_ACCEL_TYPE:
|
data = _rkiio_aiq_getData_accel(sensor_handle, data_num);
|
break;
|
case SENSOR_TEMP_TYPE:
|
data = _rkiio_aiq_getData_temp(sensor_handle, data_num);
|
break;
|
case SENSOR_ALL_TYPE:
|
data = _rkiio_aiq_getData_all(sensor_handle, data_num);
|
break;
|
default:
|
data = NULL;
|
}
|
pthread_mutex_unlock(&(sensor_handle->fifo_buf_mutex));
|
return data;
|
}
|
|
static mems_sensor_return_t rkiio_aiq_sensor_data_release(mems_sensor_handle_t sensor_handle,
|
mems_sensor_data_t data) {
|
if (!data || !sensor_handle) {
|
return SENSOR_ERR_NULL_PTR;
|
}
|
rkiio_sensor_handle_t* handle = (rkiio_sensor_handle_t*)sensor_handle;
|
uint32_t idx = _rkiio_aiq_buf_id_get(data, handle->max_data_num);
|
pthread_mutex_lock(&(handle->buf_pool[idx].buf_mutex));
|
handle->buf_pool[idx].is_valid = true;
|
pthread_mutex_unlock(&(handle->buf_pool[idx].buf_mutex));
|
return SENSOR_NO_ERR;
|
}
|
|
#ifndef IIO_TEST
|
rk_aiq_mems_sensor_intf_t g_rkiio_aiq_api = {
|
.createContext = rkiio_aiq_sensor_ctx_create,
|
.destroyContext = rkiio_aiq_sensor_ctx_destroy,
|
.getSensorList = rkiio_aiq_sensors_list_get,
|
.releaseSensorList = rkiio_aiq_sensors_list_release,
|
.getSensorCapabilities = rkiio_aiq_sensor_cap_get,
|
.releaseSensorCapabilities = rkiio_aiq_sensor_cap_release,
|
.getConfig = rkiio_aiq_sensor_config_get,
|
.setConfig = rkiio_aiq_sensor_config_set,
|
.createHandle = rkiio_aiq_sensor_handle_create,
|
.destroyHandle = rkiio_aiq_sensor_handle_destroy,
|
.getData = rkiio_aiq_getData,
|
.getLastNSamples = rkiio_aiq_getLastNSamples,
|
.releaseSamplesData = rkiio_aiq_sensor_data_release,
|
};
|
#endif
|
|
#ifdef IIO_TEST
|
|
static int g_quit = 0; // only for test
|
static FILE* save_file;
|
|
static void sigterm_handler(int sig) {
|
fprintf(stderr, "signal %d\n", sig);
|
g_quit = 1;
|
}
|
|
static void virtual_aiq_register(rk_aiq_mems_sensor_intf_t api) {
|
int32_t ret;
|
mems_sensor_ctx_t ctx = api.createContext();
|
mems_sensor_list_t sensors_list;
|
mems_sensor_type_t type = SENSOR_ALL_TYPE;
|
memset(&sensors_list, 0, sizeof(mems_sensor_list_t));
|
api.getSensorList(ctx, type, &sensors_list);
|
if (sensors_list.count) {
|
for (int32_t i = 0; i < sensors_list.count; i++) {
|
printf("%d: %s\n", i, sensors_list.key_list[i]);
|
}
|
} else {
|
printf("get no dev\n");
|
}
|
if (sensors_list.count) {
|
mems_sensor_capabilities_t caps;
|
memset(&caps, 0, sizeof(mems_sensor_capabilities_t));
|
ret = api.getSensorCapabilities(ctx, type, sensors_list.key_list[0], &caps);
|
if (ret) {
|
printf("rkiio_aiq_sensor_cap_get fail\n");
|
goto api_exit;
|
} else {
|
for (int32_t i = 0; i < caps.num_sample_rates; i++) {
|
printf("rkaiq_sensor_capabilities: %d:sample_cap: %f\n", i, caps.sample_rates[i]);
|
}
|
}
|
// cap set demo
|
if (caps.num_sample_rates) {
|
mems_sensor_config_t sensor_cfg;
|
memset(&sensor_cfg, 0, sizeof(mems_sensor_config_t));
|
ret = api.getConfig(ctx, type, sensors_list.key_list[0], &sensor_cfg);
|
if (ret) {
|
printf("rkiio_aiq_sensor_config_get fail\n");
|
} else {
|
printf("rkiio_aiq_sensor_config_get: %f\n", sensor_cfg.sample_rate);
|
}
|
sensor_cfg.sample_rate = caps.sample_rates[0];
|
ret = api.setConfig(ctx, SENSOR_GRYO_TYPE, sensors_list.key_list[0], sensor_cfg);
|
if (ret) {
|
printf("rkiio_aiq_sensor_config_set fail\n");
|
} else {
|
printf("rkiio_aiq_sensor_config_set success\n");
|
}
|
}
|
// private method set all sampling
|
rkiio_sampling_rate_set_all(sensors_list.key_list[0], "1000");
|
api.releaseSensorCapabilities(&caps);
|
|
// start sampling
|
uint64_t sampling_cnt = 0;
|
mems_sensor_handle_t handle = api.createHandle(ctx, type, sensors_list.key_list[0], 100, 3);
|
if (!handle) {
|
printf("rkiio_aiq_sensor_handle_create fail\n");
|
goto api_exit;
|
}
|
while (!g_quit) {
|
mems_sensor_data_t data;
|
size_t num_samples = 0;
|
if (1) {
|
data = api.getData(handle, &num_samples);
|
printf("getdata: %zu\n", num_samples);
|
} else {
|
num_samples = 50;
|
data = api.getLastNSamples(handle, num_samples);
|
printf("getLastNSamples: %zu\n", num_samples);
|
}
|
// only for test
|
if (type == SENSOR_GRYO_TYPE) {
|
mems_sensor_event_t* real_data = (mems_sensor_event_t*)data;
|
for (uint32_t j = 0; j < num_samples; j++) {
|
printf("gyro data(%" PRIu64 "): x: %f, y: %f, z: %f: t: %" PRIu64 "\n", real_data[j].id,
|
real_data[j].gyro.x, real_data[j].gyro.y, real_data[j].gyro.z,
|
real_data[j].timestamp_us);
|
}
|
} else if (type == SENSOR_ALL_TYPE) {
|
mems_sensor_event_t* real_data = (mems_sensor_event_t*)data;
|
for (uint32_t j = 0; j < num_samples; j++) {
|
char w_data[1024] = {0};
|
sprintf(w_data,
|
"gyro data(%" PRIu64 "): gx: %f, gy: %f, gz: %f: , ax: %f, ay: %f, az: %f, "
|
"temp: %d, time: %" PRIu64 "\n",
|
real_data[j].id, real_data[j].all.gyro.x, real_data[j].all.gyro.y,
|
real_data[j].all.gyro.z, real_data[j].all.accel.x,
|
real_data[j].all.accel.y, real_data[j].all.accel.z,
|
real_data[j].all.temperature, real_data[j].timestamp_us);
|
printf("%s\n", w_data);
|
/* if (save_file) {*/
|
/*fwrite(w_data, sizeof(char), strlen(w_data), save_file);*/
|
/* }*/
|
}
|
} else {
|
printf("only support printf gyro\n");
|
}
|
api.releaseSamplesData(handle, data);
|
sampling_cnt++;
|
data = NULL;
|
// g_quit = 1;
|
printf("===============================split=======================================");
|
usleep(50000);
|
}
|
api.destroyHandle(handle);
|
}
|
api_exit:
|
api.releaseSensorList(&sensors_list);
|
api.destroyContext(ctx);
|
}
|
|
int main() {
|
save_file = fopen("/data/test.txt", "w");
|
rk_aiq_mems_sensor_intf_t api;
|
api.createContext = rkiio_aiq_sensor_ctx_create;
|
api.destroyContext = rkiio_aiq_sensor_ctx_destroy;
|
api.getSensorList = rkiio_aiq_sensors_list_get;
|
api.releaseSensorList = rkiio_aiq_sensors_list_release;
|
api.getSensorCapabilities = rkiio_aiq_sensor_cap_get;
|
api.releaseSensorCapabilities = rkiio_aiq_sensor_cap_release;
|
api.getConfig = rkiio_aiq_sensor_config_get;
|
api.setConfig = rkiio_aiq_sensor_config_set;
|
api.createHandle = rkiio_aiq_sensor_handle_create;
|
api.destroyHandle = rkiio_aiq_sensor_handle_destroy;
|
api.getData = rkiio_aiq_getData;
|
api.getLastNSamples = rkiio_aiq_getLastNSamples;
|
api.releaseSamplesData = rkiio_aiq_sensor_data_release;
|
signal(SIGINT, sigterm_handler);
|
signal(SIGTERM, sigterm_handler);
|
virtual_aiq_register(api);
|
if (save_file) fclose(save_file);
|
printf(">>>>quit\n");
|
// mems_sensor_ctx_t ctx = rkiio_aiq_sensor_ctx_create();
|
// mems_sensor_list_t sensors_list;
|
// int32_t ret = 0;
|
// mems_sensor_type_t type = SENSOR_GRYO_TYPE;
|
// signal(SIGINT, sigterm_handler);
|
// memset(&sensors_list, 0, sizeof(mems_sensor_list_t));
|
// rkiio_aiq_sensors_list_get(ctx, type, &sensors_list);
|
// if (sensors_list.count) {
|
// for (int32_t i = 0; i < sensors_list.count; i++) {
|
// printf("%d: %s\n", i, sensors_list.key_list[i]);
|
// }
|
// } else {
|
// printf("get no dev\n");
|
// }
|
// if (sensors_list.count) {
|
// mems_sensor_capabilities_t caps;
|
// memset(&caps, 0, sizeof(mems_sensor_capabilities_t));
|
// ret = rkiio_aiq_sensor_cap_get(ctx, type, sensors_list.key_list[0], &caps);
|
// if (ret) {
|
// printf("rkiio_aiq_sensor_cap_get fail\n");
|
// goto main_exit;
|
// } else {
|
// for (int32_t i = 0; i < caps.num_sample_rates; i++) {
|
// printf("rkaiq_sensor_capabilities: %d:sample_cap: %f\n", i,
|
// caps.sample_rates[i]);
|
// }
|
// }
|
// // cap set demo
|
// if (caps.num_sample_rates) {
|
// mems_sensor_config_t sensor_cfg;
|
// memset(&sensor_cfg, 0, sizeof(mems_sensor_config_t));
|
// rkiio_aiq_sensor_config_get(ctx, type, sensors_list.key_list[0], &sensor_cfg);
|
// if (ret) {
|
// printf("rkiio_aiq_sensor_config_get fail\n");
|
// } else {
|
// printf("rkiio_aiq_sensor_config_get: %f\n", sensor_cfg.sample_rate);
|
// }
|
// sensor_cfg.sample_rate = caps.sample_rates[0];
|
// rkiio_aiq_sensor_config_set(ctx, SENSOR_GRYO_TYPE, sensors_list.key_list[0],
|
// sensor_cfg); if (ret) {
|
// printf("rkiio_aiq_sensor_config_set fail\n");
|
// } else {
|
// printf("rkiio_aiq_sensor_config_set success\n");
|
// }
|
// }
|
// // private method set all sampling
|
// rkiio_sampling_rate_set_all(sensors_list.key_list[0], "1000");
|
// rkiio_aiq_sensor_cap_release(&caps);
|
|
// // start sampling
|
// uint64_t sampling_cnt = 0;
|
// mems_sensor_handle_t handle = rkiio_aiq_sensor_handle_create(ctx, type,
|
// sensors_list.key_list[0], 100, 3); if (!handle) {
|
// printf("rkiio_aiq_sensor_handle_create fail\n");
|
// goto main_exit;
|
// }
|
// while (!g_quit) {
|
// mems_sensor_data_t data;
|
// size_t num_samples = 0;
|
// if (0) {
|
// data = rkiio_aiq_getData(handle, &num_samples);
|
// printf("getdata: %d\n", num_samples);
|
// } else {
|
// num_samples = 50;
|
// data = rkiio_aiq_getLastNSamples(handle, num_samples);
|
// printf("getLastNSamples: %d\n", num_samples);
|
// }
|
// // only for test
|
// if (type == SENSOR_GRYO_TYPE) {
|
// mems_sensor_event_t *real_data = (mems_sensor_event_t *)data;
|
// for (uint32_t j = 0; j < num_samples; j++) {
|
// printf("gyro data(%llu): x: %f, y: %f, z: %f: t: %llu\n",
|
// real_data[j].id, real_data[j].gyro.x, real_data[j].gyro.y,
|
// real_data[j].gyro.z, real_data[j].timestamp_us);
|
// }
|
// } else {
|
// printf("only support printf gyro\n");
|
// }
|
// rkiio_aiq_sensor_data_release(handle, data);
|
// sampling_cnt++;
|
// data = NULL;
|
// // g_quit = 1;
|
// }
|
// rkiio_aiq_sensor_handle_destroy(handle);
|
// }
|
// main_exit:
|
// rkiio_aiq_sensors_list_release(&sensors_list);
|
// rkiio_aiq_sensor_ctx_destroy(ctx);
|
return 0;
|
}
|
#endif
|
