1 #include <pthread.h>
2 #include <signal.h>
3 #include <stdbool.h>
4 #include <stdint.h>
5 #include <stdio.h>
6 #include <stdlib.h>
7 #include <string.h>
8 #include <unistd.h>
9 #include <inttypes.h>
10
11 #include "rk_aiq_mems_sensor.h"
12 #include "rkiio.h"
13
14 //#define IIO_TEST
15
16 typedef struct rkiio_sensor_ctx_s {
17 uint64_t ctx_id;
18 } rkiio_sensor_ctx_t;
19
20 typedef struct rkiio_sensor_handle_buf_s {
21 pthread_mutex_t buf_mutex;
22 bool is_valid;
23 void* buf;
24 } rkiio_sensor_handle_buf_t;
25
26 typedef struct rkiio_sensor_handle_ctx_s {
27 mems_sensor_type_t sensor_type;
28 RKIIO_DATA_HANDLE handle;
29 uint32_t max_data_num;
30 uint32_t buf_cnt;
31 rkiio_sensor_handle_buf_t* buf_pool;
32 pthread_mutex_t fifo_buf_mutex;
33 RKIIO_FIFO_DATA fifo_data_buf;
34 } rkiio_sensor_handle_t;
35
36 static pthread_mutex_t g_mutex = PTHREAD_MUTEX_INITIALIZER;
37 static bool g_init_flag = false;
38 static uint32_t g_sensor_cnt = 0;
39 static uint32_t g_handle_cnt = 0;
40
41
rkiio_aiq_sensor_ctx_create()42 static mems_sensor_ctx_t rkiio_aiq_sensor_ctx_create() {
43 pthread_mutex_lock(&g_mutex);
44 g_sensor_cnt++;
45 if (!g_init_flag) {
46 rkiio_sys_init();
47 rkiio_timestamp_type_set_all(RKIIO_TIMESTAMP_MONOTONIC);
48 }
49 g_init_flag = true;
50 pthread_mutex_unlock(&g_mutex);
51 rkiio_sensor_ctx_t* ctx = (rkiio_sensor_ctx_t*)malloc(sizeof(rkiio_sensor_ctx_t));
52 memset(ctx, 0, sizeof(rkiio_sensor_ctx_t));
53 return (mems_sensor_ctx_t)ctx;
54 }
55
rkiio_aiq_sensor_ctx_destroy(mems_sensor_ctx_t ctx)56 static mems_sensor_return_t rkiio_aiq_sensor_ctx_destroy(mems_sensor_ctx_t ctx) {
57 if (!ctx) {
58 return SENSOR_ERR_NULL_PTR;
59 }
60 rkiio_sensor_ctx_t* rkiio_ctx = (rkiio_sensor_ctx_t*)ctx;
61 free(rkiio_ctx);
62 pthread_mutex_lock(&g_mutex);
63 g_sensor_cnt--;
64 if (!g_sensor_cnt) {
65 rkiio_sys_uninit();
66 g_init_flag = false;
67 }
68 pthread_mutex_unlock(&g_mutex);
69 return SENSOR_NO_ERR;
70 }
71
rkiio_aiq_sensors_list_get(mems_sensor_ctx_t ctx,mems_sensor_type_t sensor_type,mems_sensor_list_t * sensors_list)72 static mems_sensor_return_t rkiio_aiq_sensors_list_get(mems_sensor_ctx_t ctx,
73 mems_sensor_type_t sensor_type,
74 mems_sensor_list_t* sensors_list) {
75 if (!g_init_flag) {
76 return SENSOR_ERR_FAIL;
77 }
78 if (!sensors_list || !ctx) {
79 return SENSOR_ERR_NULL_PTR;
80 }
81 switch (sensor_type) {
82 case SENSOR_GRYO_TYPE:
83 sensors_list->key_list = rkiio_anglvel_dev_list_get(&sensors_list->count);
84 break;
85 case SENSOR_ACCEL_TYPE:
86 sensors_list->key_list = rkiio_accel_dev_list_get(&sensors_list->count);
87 break;
88 case SENSOR_TEMP_TYPE:
89 sensors_list->key_list = rkiio_temp_dev_list_get(&sensors_list->count);
90 break;
91 case SENSOR_ALL_TYPE:
92 sensors_list->key_list = rkiio_all_dev_list_get(&sensors_list->count);
93 break;
94 default:
95 return SENSOR_ERR_MISMATCH_SENSOR;
96 }
97 if (sensors_list->count) {
98 return SENSOR_NO_ERR;
99 }
100 return SENSOR_ERR_MISMATCH_SENSOR;
101 }
102
rkiio_aiq_sensors_list_release(mems_sensor_list_t * sensors_list)103 static mems_sensor_return_t rkiio_aiq_sensors_list_release(mems_sensor_list_t* sensors_list) {
104 if (!sensors_list) {
105 return SENSOR_ERR_NULL_PTR;
106 }
107 int32_t ret = rkiio_dev_list_destroy(sensors_list->key_list, sensors_list->count);
108 if (ret) {
109 return SENSOR_ERR_FAIL;
110 }
111 return SENSOR_NO_ERR;
112 }
113
rkiio_aiq_sensor_cap_get(mems_sensor_ctx_t ctx,mems_sensor_type_t sensor_type,char * sensor_key,mems_sensor_capabilities_t * caps)114 static mems_sensor_return_t rkiio_aiq_sensor_cap_get(mems_sensor_ctx_t ctx,
115 mems_sensor_type_t sensor_type,
116 char* sensor_key,
117 mems_sensor_capabilities_t* caps) {
118 if (!g_init_flag) {
119 return SENSOR_ERR_FAIL;
120 }
121 if (!ctx) {
122 return SENSOR_ERR_NULL_PTR;
123 }
124 memset(caps, 0, sizeof(mems_sensor_capabilities_t));
125 caps->type = sensor_type;
126 switch (sensor_type) {
127 case SENSOR_GRYO_TYPE:
128 rkiio_sampling_rate_cap_get(sensor_key, RKIIO_SENSOR_ANGLVEL_TYPE,
129 &(caps->sample_rates), &(caps->num_sample_rates));
130 break;
131 case SENSOR_ACCEL_TYPE:
132 rkiio_sampling_rate_cap_get(sensor_key, RKIIO_SENSOR_ACCEL_TYPE, &(caps->sample_rates),
133 &(caps->num_sample_rates));
134 break;
135 case SENSOR_TEMP_TYPE:
136 rkiio_sampling_rate_cap_get(sensor_key, RKIIO_SENSOR_TEMP_TYPE, &(caps->sample_rates),
137 &(caps->num_sample_rates));
138 break;
139 case SENSOR_ALL_TYPE:
140 rkiio_sampling_rate_cap_get(sensor_key, RKIIO_SENSOR_ALL_TYPE, &(caps->sample_rates),
141 &(caps->num_sample_rates));
142 break;
143 default:
144 return SENSOR_ERR_MISMATCH_SENSOR;
145 }
146 caps->is_data_valid = true;
147 return SENSOR_NO_ERR;
148 }
149
rkiio_aiq_sensor_cap_release(mems_sensor_capabilities_t * caps)150 static mems_sensor_return_t rkiio_aiq_sensor_cap_release(mems_sensor_capabilities_t* caps) {
151 if (caps->sample_rates) rkiio_sampling_rate_cap_release(caps->sample_rates);
152 return SENSOR_NO_ERR;
153 }
154
155 // only support sampling rate
rkiio_aiq_sensor_config_get(mems_sensor_ctx_t ctx,mems_sensor_type_t sensor_type,char * sensor_key,mems_sensor_config_t * cfg)156 static mems_sensor_return_t rkiio_aiq_sensor_config_get(mems_sensor_ctx_t ctx,
157 mems_sensor_type_t sensor_type,
158 char* sensor_key,
159 mems_sensor_config_t* cfg) {
160 if (!g_init_flag) {
161 return SENSOR_ERR_FAIL;
162 }
163 if (!cfg || !ctx) {
164 return SENSOR_ERR_NULL_PTR;
165 }
166 if (g_handle_cnt) {
167 return SENSOR_ERR_DEV_BUSY;
168 }
169 memset(cfg, 0, sizeof(mems_sensor_config_t));
170 int32_t ret = 0;
171 switch (sensor_type) {
172 case SENSOR_GRYO_TYPE:
173 ret =
174 rkiio_sampling_rate_get(sensor_key, RKIIO_SENSOR_ANGLVEL_TYPE, &(cfg->sample_rate));
175 break;
176 case SENSOR_ACCEL_TYPE:
177 ret = rkiio_sampling_rate_get(sensor_key, RKIIO_SENSOR_ACCEL_TYPE, &(cfg->sample_rate));
178 break;
179 case SENSOR_TEMP_TYPE:
180 ret = rkiio_sampling_rate_get(sensor_key, RKIIO_SENSOR_TEMP_TYPE, &(cfg->sample_rate));
181 break;
182 case SENSOR_ALL_TYPE:
183 ret = rkiio_sampling_rate_get(sensor_key, RKIIO_SENSOR_ALL_TYPE, &(cfg->sample_rate));
184 break;
185 default:
186 return SENSOR_ERR_MISMATCH_SENSOR;
187 }
188 if (ret) {
189 cfg->sample_rate = 0.0f;
190 return SENSOR_ERR_FAIL;
191 }
192 return SENSOR_NO_ERR;
193 }
194
rkiio_aiq_sensor_config_set(mems_sensor_ctx_t ctx,mems_sensor_type_t sensor_type,char * sensor_key,mems_sensor_config_t cfg)195 static mems_sensor_return_t rkiio_aiq_sensor_config_set(mems_sensor_ctx_t ctx,
196 mems_sensor_type_t sensor_type,
197 char* sensor_key,
198 mems_sensor_config_t cfg) {
199 if (!g_init_flag) {
200 return SENSOR_ERR_FAIL;
201 }
202 if (!ctx) {
203 return SENSOR_ERR_NULL_PTR;
204 }
205 if (g_handle_cnt) {
206 return SENSOR_ERR_DEV_BUSY;
207 }
208 int32_t ret = 0;
209 switch (sensor_type) {
210 case SENSOR_GRYO_TYPE:
211 ret = rkiio_sampling_rate_set(sensor_key, RKIIO_SENSOR_ANGLVEL_TYPE, cfg.sample_rate);
212 break;
213 case SENSOR_ACCEL_TYPE:
214 ret = rkiio_sampling_rate_set(sensor_key, RKIIO_SENSOR_ACCEL_TYPE, cfg.sample_rate);
215 break;
216 case SENSOR_TEMP_TYPE:
217 ret = rkiio_sampling_rate_set(sensor_key, RKIIO_SENSOR_TEMP_TYPE, cfg.sample_rate);
218 break;
219 case SENSOR_ALL_TYPE:
220 ret = rkiio_sampling_rate_set(sensor_key, RKIIO_SENSOR_ALL_TYPE, cfg.sample_rate);
221 break;
222 default:
223 return SENSOR_ERR_MISMATCH_SENSOR;
224 }
225 if (ret) {
226 return SENSOR_ERR_FAIL;
227 }
228 return SENSOR_NO_ERR;
229 }
230
_rkiio_aiq_buf_id_get(void * buf,uint32_t max_data_num)231 static uint32_t _rkiio_aiq_buf_id_get(void* buf, uint32_t max_data_num) {
232 uint8_t* data = (uint8_t*)buf;
233 uint64_t idx = sizeof(mems_sensor_event_t) * max_data_num;
234 int32_t* reserverd_data = (int32_t*)&(data[idx]);
235 return *reserverd_data;
236 }
237
_rkiio_aiq_buf_id_set(void * buf,uint32_t max_data_num,uint32_t id)238 static void _rkiio_aiq_buf_id_set(void* buf, uint32_t max_data_num, uint32_t id) {
239 uint8_t* data = (uint8_t*)buf;
240 uint64_t idx = sizeof(mems_sensor_event_t) * max_data_num;
241 int32_t* reserverd_data = (int32_t*)&(data[idx]);
242 *reserverd_data = id;
243 }
244
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)245 static mems_sensor_handle_t rkiio_aiq_sensor_handle_create(mems_sensor_ctx_t ctx,
246 mems_sensor_type_t sensor_type,
247 char* sensor_key, uint32_t max_data_num,
248 uint32_t buf_cnt) {
249 if (!g_init_flag || !ctx || !max_data_num || !buf_cnt) {
250 return NULL;
251 }
252 rkiio_sensor_handle_t* sensor_handle =
253 (rkiio_sensor_handle_t*)malloc(sizeof(rkiio_sensor_handle_t));
254 memset(sensor_handle, 0, sizeof(rkiio_sensor_handle_t));
255 sensor_handle->sensor_type = sensor_type;
256 sensor_handle->buf_cnt = buf_cnt;
257
258 sensor_handle->handle = rkiio_data_handle_create(sensor_key, 0, max_data_num);
259 uint32_t real_max_data_num = 0;
260 rkiio_data_handle_max_fifo_num_get(sensor_handle->handle, &real_max_data_num);
261 sensor_handle->max_data_num =
262 (max_data_num > real_max_data_num) ? max_data_num : real_max_data_num;
263
264 pthread_mutex_init(&(sensor_handle->fifo_buf_mutex), NULL);
265 sensor_handle->fifo_data_buf =
266 (RKIIO_FIFO_DATA)malloc(sizeof(rkiio_data_0_t) * sensor_handle->max_data_num);
267
268 sensor_handle->buf_pool =
269 (rkiio_sensor_handle_buf_t*)malloc(sizeof(rkiio_sensor_handle_buf_t) * buf_cnt);
270 memset(sensor_handle->buf_pool, 0, sizeof(rkiio_sensor_handle_buf_t) * buf_cnt);
271 for (uint32_t i = 0; i < buf_cnt; i++) {
272 pthread_mutex_init(&(sensor_handle->buf_pool[i].buf_mutex), NULL);
273 sensor_handle->buf_pool[i].is_valid = true;
274 sensor_handle->buf_pool[i].buf = (void*)malloc(
275 sizeof(mems_sensor_event_t) * sensor_handle->max_data_num + sizeof(uint32_t));
276 memset(sensor_handle->buf_pool[i].buf, 0,
277 sizeof(mems_sensor_event_t) * sensor_handle->max_data_num + sizeof(uint32_t));
278 _rkiio_aiq_buf_id_set(sensor_handle->buf_pool[i].buf, sensor_handle->max_data_num, i);
279 }
280 pthread_mutex_lock(&g_mutex);
281 g_handle_cnt++;
282 pthread_mutex_unlock(&g_mutex);
283 return (mems_sensor_handle_t)sensor_handle;
284 }
285
rkiio_aiq_sensor_handle_destroy(mems_sensor_handle_t handle)286 static mems_sensor_return_t rkiio_aiq_sensor_handle_destroy(mems_sensor_handle_t handle) {
287 if (!handle) {
288 return SENSOR_ERR_NULL_PTR;
289 }
290 rkiio_sensor_handle_t* sensor_handle = (rkiio_sensor_handle_t*)handle;
291 pthread_mutex_lock(&(sensor_handle->fifo_buf_mutex));
292 rkiio_data_handle_destroy(sensor_handle->handle);
293 if (sensor_handle->fifo_data_buf) free(sensor_handle->fifo_data_buf);
294 if (sensor_handle->buf_pool) {
295 for (uint32_t i = 0; i < sensor_handle->buf_cnt; i++) {
296 if (sensor_handle->buf_pool[i].buf) free(sensor_handle->buf_pool[i].buf);
297 }
298 free(sensor_handle->buf_pool);
299 }
300 pthread_mutex_unlock(&(sensor_handle->fifo_buf_mutex));
301 free(sensor_handle);
302 pthread_mutex_lock(&g_mutex);
303 g_handle_cnt--;
304 pthread_mutex_unlock(&g_mutex);
305 return SENSOR_NO_ERR;
306 }
307
_rkiio_aiq_get_vaild_buf(rkiio_sensor_handle_t * handle)308 static uint32_t _rkiio_aiq_get_vaild_buf(rkiio_sensor_handle_t* handle) {
309 while (1) {
310 for (uint32_t i = 0; i < handle->buf_cnt; i++) {
311 pthread_mutex_lock(&(handle->buf_pool[i].buf_mutex));
312 // printf("%s: id: %u, valid: %d\n", __FUNCTION__, i, handle->buf_pool[i].is_valid);
313 if (handle->buf_pool[i].is_valid) {
314 handle->buf_pool[i].is_valid = false;
315 pthread_mutex_unlock(&(handle->buf_pool[i].buf_mutex));
316 return i;
317 }
318 pthread_mutex_unlock(&(handle->buf_pool[i].buf_mutex));
319 }
320 usleep(100000);
321 }
322 }
323
_rkiio_aiq_getData_gyro(rkiio_sensor_handle_t * handle,uint64_t data_num)324 static mems_sensor_data_t _rkiio_aiq_getData_gyro(rkiio_sensor_handle_t* handle, uint64_t data_num) {
325 rkiio_data_0_t* fifo = (rkiio_data_0_t*)handle->fifo_data_buf;
326 uint32_t valid_id = _rkiio_aiq_get_vaild_buf(handle);
327 mems_sensor_event_t* event = (mems_sensor_event_t*)(handle->buf_pool[valid_id].buf);
328 memset(event, 0, sizeof(mems_sensor_event_t) * data_num);
329 for (uint64_t i = 0; i < data_num; i++) {
330 memcpy(&(event[i].gyro), &(fifo[i].anglvel), sizeof(xyz_data_t));
331 event[i].timestamp_us = fifo[i].timestamp;
332 event[i].id = fifo[i].id;
333 // printf("%llu:, x: %f, y: %f, z: %f, t: %llu, %llu:, x: %f, y: %f, z: %f, t: %llu\n",
334 // fifo[i].id, fifo[i].anglvel.x, fifo[i].anglvel.y, fifo[i].anglvel.z,
335 // fifo[i].timestamp, event[i].id, event[i].gyro.x, event[i].gyro.y, event[i].gyro.z,
336 // event[i].timestamp_us);
337 }
338 return (mems_sensor_data_t)event;
339 }
340
_rkiio_aiq_getData_accel(rkiio_sensor_handle_t * handle,uint64_t data_num)341 static mems_sensor_data_t _rkiio_aiq_getData_accel(rkiio_sensor_handle_t* handle, uint64_t data_num) {
342 rkiio_data_0_t* fifo = (rkiio_data_0_t*)handle->fifo_data_buf;
343 uint32_t valid_id = _rkiio_aiq_get_vaild_buf(handle);
344 mems_sensor_event_t* event = (mems_sensor_event_t*)(handle->buf_pool[valid_id].buf);
345 memset(event, 0, sizeof(mems_sensor_event_t) * data_num);
346 for (uint64_t i = 0; i < data_num; i++) {
347 memcpy(&(event[i].accel), &(fifo[i].accel), sizeof(xyz_data_t));
348 event[i].timestamp_us = fifo[i].timestamp;
349 event[i].id = fifo[i].id;
350 }
351 return (mems_sensor_data_t)event;
352 }
353
_rkiio_aiq_getData_temp(rkiio_sensor_handle_t * handle,uint64_t data_num)354 static mems_sensor_data_t _rkiio_aiq_getData_temp(rkiio_sensor_handle_t* handle, uint64_t data_num) {
355 rkiio_data_0_t* fifo = (rkiio_data_0_t*)handle->fifo_data_buf;
356 uint32_t valid_id = _rkiio_aiq_get_vaild_buf(handle);
357 mems_sensor_event_t* event = (mems_sensor_event_t*)(handle->buf_pool[valid_id].buf);
358 memset(event, 0, sizeof(mems_sensor_event_t) * data_num);
359 for (uint64_t i = 0; i < data_num; i++) {
360 // temp alter to s32?
361 event[i].temperature = fifo[i].temp;
362 event[i].timestamp_us = fifo[i].timestamp;
363 event[i].id = fifo[i].id;
364 }
365 return (mems_sensor_data_t)event;
366 }
367
_rkiio_aiq_getData_all(rkiio_sensor_handle_t * handle,uint64_t data_num)368 static mems_sensor_data_t _rkiio_aiq_getData_all(rkiio_sensor_handle_t* handle, uint64_t data_num) {
369 rkiio_data_0_t* fifo = (rkiio_data_0_t*)handle->fifo_data_buf;
370 uint32_t valid_id = _rkiio_aiq_get_vaild_buf(handle);
371 mems_sensor_event_t* event = (mems_sensor_event_t*)(handle->buf_pool[valid_id].buf);
372 memset(event, 0, sizeof(mems_sensor_event_t) * data_num);
373 for (uint64_t i = 0; i < data_num; i++) {
374 memcpy(&(event[i].all.gyro), &(fifo[i].anglvel), sizeof(xyz_data_t));
375 memcpy(&(event[i].all.accel), &(fifo[i].accel), sizeof(xyz_data_t));
376 // temp alter to s32?
377 event[i].all.temperature = fifo[i].temp;
378 event[i].timestamp_us = fifo[i].timestamp;
379 event[i].id = fifo[i].id;
380 }
381 return (mems_sensor_data_t)event;
382 }
383
rkiio_aiq_getData(mems_sensor_handle_t handle,size_t * num_samples)384 static mems_sensor_data_t rkiio_aiq_getData(mems_sensor_handle_t handle, size_t* num_samples) {
385 if (!handle) {
386 return NULL;
387 }
388 int32_t ret = 0;
389 rkiio_sensor_handle_t* sensor_handle = (rkiio_sensor_handle_t*)handle;
390 pthread_mutex_lock(&(sensor_handle->fifo_buf_mutex));
391 uint64_t data_num = 0;
392 ret = rkiio_data_get_all(sensor_handle->handle, sensor_handle->fifo_data_buf, &data_num);
393 if (ret || !data_num) {
394 *num_samples = 0;
395 pthread_mutex_unlock(&(sensor_handle->fifo_buf_mutex));
396 return NULL;
397 }
398 mems_sensor_data_t data = NULL;
399 switch (sensor_handle->sensor_type) {
400 case SENSOR_GRYO_TYPE:
401 data = _rkiio_aiq_getData_gyro(sensor_handle, data_num);
402 break;
403 case SENSOR_ACCEL_TYPE:
404 data = _rkiio_aiq_getData_accel(sensor_handle, data_num);
405 break;
406 case SENSOR_TEMP_TYPE:
407 data = _rkiio_aiq_getData_temp(sensor_handle, data_num);
408 break;
409 case SENSOR_ALL_TYPE:
410 data = _rkiio_aiq_getData_all(sensor_handle, data_num);
411 break;
412 default:
413 data = NULL;
414 }
415 *num_samples = data ? (size_t)data_num : 0;
416 pthread_mutex_unlock(&(sensor_handle->fifo_buf_mutex));
417 return data;
418 }
419
rkiio_aiq_getLastNSamples(mems_sensor_handle_t handle,size_t num_samples)420 static mems_sensor_data_t rkiio_aiq_getLastNSamples(mems_sensor_handle_t handle,
421 size_t num_samples) {
422 if (!handle || !num_samples) {
423 return NULL;
424 }
425 rkiio_sensor_handle_t* sensor_handle = (rkiio_sensor_handle_t*)handle;
426 if ((uint32_t)num_samples > sensor_handle->max_data_num) {
427 printf("%s: num_samples > max_data_num\n", __FUNCTION__);
428 return NULL;
429 }
430 int32_t ret = 0;
431 pthread_mutex_lock(&(sensor_handle->fifo_buf_mutex));
432 uint64_t data_num = (uint64_t)num_samples;
433 ret = rkiio_data_read(sensor_handle->handle, sensor_handle->fifo_data_buf, data_num);
434 if (ret) {
435 pthread_mutex_unlock(&(sensor_handle->fifo_buf_mutex));
436 return NULL;
437 }
438 mems_sensor_data_t data = NULL;
439 switch (sensor_handle->sensor_type) {
440 case SENSOR_GRYO_TYPE:
441 data = _rkiio_aiq_getData_gyro(sensor_handle, data_num);
442 break;
443 case SENSOR_ACCEL_TYPE:
444 data = _rkiio_aiq_getData_accel(sensor_handle, data_num);
445 break;
446 case SENSOR_TEMP_TYPE:
447 data = _rkiio_aiq_getData_temp(sensor_handle, data_num);
448 break;
449 case SENSOR_ALL_TYPE:
450 data = _rkiio_aiq_getData_all(sensor_handle, data_num);
451 break;
452 default:
453 data = NULL;
454 }
455 pthread_mutex_unlock(&(sensor_handle->fifo_buf_mutex));
456 return data;
457 }
458
rkiio_aiq_sensor_data_release(mems_sensor_handle_t sensor_handle,mems_sensor_data_t data)459 static mems_sensor_return_t rkiio_aiq_sensor_data_release(mems_sensor_handle_t sensor_handle,
460 mems_sensor_data_t data) {
461 if (!data || !sensor_handle) {
462 return SENSOR_ERR_NULL_PTR;
463 }
464 rkiio_sensor_handle_t* handle = (rkiio_sensor_handle_t*)sensor_handle;
465 uint32_t idx = _rkiio_aiq_buf_id_get(data, handle->max_data_num);
466 pthread_mutex_lock(&(handle->buf_pool[idx].buf_mutex));
467 handle->buf_pool[idx].is_valid = true;
468 pthread_mutex_unlock(&(handle->buf_pool[idx].buf_mutex));
469 return SENSOR_NO_ERR;
470 }
471
472 #ifndef IIO_TEST
473 rk_aiq_mems_sensor_intf_t g_rkiio_aiq_api = {
474 .createContext = rkiio_aiq_sensor_ctx_create,
475 .destroyContext = rkiio_aiq_sensor_ctx_destroy,
476 .getSensorList = rkiio_aiq_sensors_list_get,
477 .releaseSensorList = rkiio_aiq_sensors_list_release,
478 .getSensorCapabilities = rkiio_aiq_sensor_cap_get,
479 .releaseSensorCapabilities = rkiio_aiq_sensor_cap_release,
480 .getConfig = rkiio_aiq_sensor_config_get,
481 .setConfig = rkiio_aiq_sensor_config_set,
482 .createHandle = rkiio_aiq_sensor_handle_create,
483 .destroyHandle = rkiio_aiq_sensor_handle_destroy,
484 .getData = rkiio_aiq_getData,
485 .getLastNSamples = rkiio_aiq_getLastNSamples,
486 .releaseSamplesData = rkiio_aiq_sensor_data_release,
487 };
488 #endif
489
490 #ifdef IIO_TEST
491
492 static int g_quit = 0; // only for test
493 static FILE* save_file;
494
sigterm_handler(int sig)495 static void sigterm_handler(int sig) {
496 fprintf(stderr, "signal %d\n", sig);
497 g_quit = 1;
498 }
499
virtual_aiq_register(rk_aiq_mems_sensor_intf_t api)500 static void virtual_aiq_register(rk_aiq_mems_sensor_intf_t api) {
501 int32_t ret;
502 mems_sensor_ctx_t ctx = api.createContext();
503 mems_sensor_list_t sensors_list;
504 mems_sensor_type_t type = SENSOR_ALL_TYPE;
505 memset(&sensors_list, 0, sizeof(mems_sensor_list_t));
506 api.getSensorList(ctx, type, &sensors_list);
507 if (sensors_list.count) {
508 for (int32_t i = 0; i < sensors_list.count; i++) {
509 printf("%d: %s\n", i, sensors_list.key_list[i]);
510 }
511 } else {
512 printf("get no dev\n");
513 }
514 if (sensors_list.count) {
515 mems_sensor_capabilities_t caps;
516 memset(&caps, 0, sizeof(mems_sensor_capabilities_t));
517 ret = api.getSensorCapabilities(ctx, type, sensors_list.key_list[0], &caps);
518 if (ret) {
519 printf("rkiio_aiq_sensor_cap_get fail\n");
520 goto api_exit;
521 } else {
522 for (int32_t i = 0; i < caps.num_sample_rates; i++) {
523 printf("rkaiq_sensor_capabilities: %d:sample_cap: %f\n", i, caps.sample_rates[i]);
524 }
525 }
526 // cap set demo
527 if (caps.num_sample_rates) {
528 mems_sensor_config_t sensor_cfg;
529 memset(&sensor_cfg, 0, sizeof(mems_sensor_config_t));
530 ret = api.getConfig(ctx, type, sensors_list.key_list[0], &sensor_cfg);
531 if (ret) {
532 printf("rkiio_aiq_sensor_config_get fail\n");
533 } else {
534 printf("rkiio_aiq_sensor_config_get: %f\n", sensor_cfg.sample_rate);
535 }
536 sensor_cfg.sample_rate = caps.sample_rates[0];
537 ret = api.setConfig(ctx, SENSOR_GRYO_TYPE, sensors_list.key_list[0], sensor_cfg);
538 if (ret) {
539 printf("rkiio_aiq_sensor_config_set fail\n");
540 } else {
541 printf("rkiio_aiq_sensor_config_set success\n");
542 }
543 }
544 // private method set all sampling
545 rkiio_sampling_rate_set_all(sensors_list.key_list[0], "1000");
546 api.releaseSensorCapabilities(&caps);
547
548 // start sampling
549 uint64_t sampling_cnt = 0;
550 mems_sensor_handle_t handle = api.createHandle(ctx, type, sensors_list.key_list[0], 100, 3);
551 if (!handle) {
552 printf("rkiio_aiq_sensor_handle_create fail\n");
553 goto api_exit;
554 }
555 while (!g_quit) {
556 mems_sensor_data_t data;
557 size_t num_samples = 0;
558 if (1) {
559 data = api.getData(handle, &num_samples);
560 printf("getdata: %zu\n", num_samples);
561 } else {
562 num_samples = 50;
563 data = api.getLastNSamples(handle, num_samples);
564 printf("getLastNSamples: %zu\n", num_samples);
565 }
566 // only for test
567 if (type == SENSOR_GRYO_TYPE) {
568 mems_sensor_event_t* real_data = (mems_sensor_event_t*)data;
569 for (uint32_t j = 0; j < num_samples; j++) {
570 printf("gyro data(%" PRIu64 "): x: %f, y: %f, z: %f: t: %" PRIu64 "\n", real_data[j].id,
571 real_data[j].gyro.x, real_data[j].gyro.y, real_data[j].gyro.z,
572 real_data[j].timestamp_us);
573 }
574 } else if (type == SENSOR_ALL_TYPE) {
575 mems_sensor_event_t* real_data = (mems_sensor_event_t*)data;
576 for (uint32_t j = 0; j < num_samples; j++) {
577 char w_data[1024] = {0};
578 sprintf(w_data,
579 "gyro data(%" PRIu64 "): gx: %f, gy: %f, gz: %f: , ax: %f, ay: %f, az: %f, "
580 "temp: %d, time: %" PRIu64 "\n",
581 real_data[j].id, real_data[j].all.gyro.x, real_data[j].all.gyro.y,
582 real_data[j].all.gyro.z, real_data[j].all.accel.x,
583 real_data[j].all.accel.y, real_data[j].all.accel.z,
584 real_data[j].all.temperature, real_data[j].timestamp_us);
585 printf("%s\n", w_data);
586 /* if (save_file) {*/
587 /*fwrite(w_data, sizeof(char), strlen(w_data), save_file);*/
588 /* }*/
589 }
590 } else {
591 printf("only support printf gyro\n");
592 }
593 api.releaseSamplesData(handle, data);
594 sampling_cnt++;
595 data = NULL;
596 // g_quit = 1;
597 printf("===============================split=======================================");
598 usleep(50000);
599 }
600 api.destroyHandle(handle);
601 }
602 api_exit:
603 api.releaseSensorList(&sensors_list);
604 api.destroyContext(ctx);
605 }
606
main()607 int main() {
608 save_file = fopen("/data/test.txt", "w");
609 rk_aiq_mems_sensor_intf_t api;
610 api.createContext = rkiio_aiq_sensor_ctx_create;
611 api.destroyContext = rkiio_aiq_sensor_ctx_destroy;
612 api.getSensorList = rkiio_aiq_sensors_list_get;
613 api.releaseSensorList = rkiio_aiq_sensors_list_release;
614 api.getSensorCapabilities = rkiio_aiq_sensor_cap_get;
615 api.releaseSensorCapabilities = rkiio_aiq_sensor_cap_release;
616 api.getConfig = rkiio_aiq_sensor_config_get;
617 api.setConfig = rkiio_aiq_sensor_config_set;
618 api.createHandle = rkiio_aiq_sensor_handle_create;
619 api.destroyHandle = rkiio_aiq_sensor_handle_destroy;
620 api.getData = rkiio_aiq_getData;
621 api.getLastNSamples = rkiio_aiq_getLastNSamples;
622 api.releaseSamplesData = rkiio_aiq_sensor_data_release;
623 signal(SIGINT, sigterm_handler);
624 signal(SIGTERM, sigterm_handler);
625 virtual_aiq_register(api);
626 if (save_file) fclose(save_file);
627 printf(">>>>quit\n");
628 // mems_sensor_ctx_t ctx = rkiio_aiq_sensor_ctx_create();
629 // mems_sensor_list_t sensors_list;
630 // int32_t ret = 0;
631 // mems_sensor_type_t type = SENSOR_GRYO_TYPE;
632 // signal(SIGINT, sigterm_handler);
633 // memset(&sensors_list, 0, sizeof(mems_sensor_list_t));
634 // rkiio_aiq_sensors_list_get(ctx, type, &sensors_list);
635 // if (sensors_list.count) {
636 // for (int32_t i = 0; i < sensors_list.count; i++) {
637 // printf("%d: %s\n", i, sensors_list.key_list[i]);
638 // }
639 // } else {
640 // printf("get no dev\n");
641 // }
642 // if (sensors_list.count) {
643 // mems_sensor_capabilities_t caps;
644 // memset(&caps, 0, sizeof(mems_sensor_capabilities_t));
645 // ret = rkiio_aiq_sensor_cap_get(ctx, type, sensors_list.key_list[0], &caps);
646 // if (ret) {
647 // printf("rkiio_aiq_sensor_cap_get fail\n");
648 // goto main_exit;
649 // } else {
650 // for (int32_t i = 0; i < caps.num_sample_rates; i++) {
651 // printf("rkaiq_sensor_capabilities: %d:sample_cap: %f\n", i,
652 // caps.sample_rates[i]);
653 // }
654 // }
655 // // cap set demo
656 // if (caps.num_sample_rates) {
657 // mems_sensor_config_t sensor_cfg;
658 // memset(&sensor_cfg, 0, sizeof(mems_sensor_config_t));
659 // rkiio_aiq_sensor_config_get(ctx, type, sensors_list.key_list[0], &sensor_cfg);
660 // if (ret) {
661 // printf("rkiio_aiq_sensor_config_get fail\n");
662 // } else {
663 // printf("rkiio_aiq_sensor_config_get: %f\n", sensor_cfg.sample_rate);
664 // }
665 // sensor_cfg.sample_rate = caps.sample_rates[0];
666 // rkiio_aiq_sensor_config_set(ctx, SENSOR_GRYO_TYPE, sensors_list.key_list[0],
667 // sensor_cfg); if (ret) {
668 // printf("rkiio_aiq_sensor_config_set fail\n");
669 // } else {
670 // printf("rkiio_aiq_sensor_config_set success\n");
671 // }
672 // }
673 // // private method set all sampling
674 // rkiio_sampling_rate_set_all(sensors_list.key_list[0], "1000");
675 // rkiio_aiq_sensor_cap_release(&caps);
676
677 // // start sampling
678 // uint64_t sampling_cnt = 0;
679 // mems_sensor_handle_t handle = rkiio_aiq_sensor_handle_create(ctx, type,
680 // sensors_list.key_list[0], 100, 3); if (!handle) {
681 // printf("rkiio_aiq_sensor_handle_create fail\n");
682 // goto main_exit;
683 // }
684 // while (!g_quit) {
685 // mems_sensor_data_t data;
686 // size_t num_samples = 0;
687 // if (0) {
688 // data = rkiio_aiq_getData(handle, &num_samples);
689 // printf("getdata: %d\n", num_samples);
690 // } else {
691 // num_samples = 50;
692 // data = rkiio_aiq_getLastNSamples(handle, num_samples);
693 // printf("getLastNSamples: %d\n", num_samples);
694 // }
695 // // only for test
696 // if (type == SENSOR_GRYO_TYPE) {
697 // mems_sensor_event_t *real_data = (mems_sensor_event_t *)data;
698 // for (uint32_t j = 0; j < num_samples; j++) {
699 // printf("gyro data(%llu): x: %f, y: %f, z: %f: t: %llu\n",
700 // real_data[j].id, real_data[j].gyro.x, real_data[j].gyro.y,
701 // real_data[j].gyro.z, real_data[j].timestamp_us);
702 // }
703 // } else {
704 // printf("only support printf gyro\n");
705 // }
706 // rkiio_aiq_sensor_data_release(handle, data);
707 // sampling_cnt++;
708 // data = NULL;
709 // // g_quit = 1;
710 // }
711 // rkiio_aiq_sensor_handle_destroy(handle);
712 // }
713 // main_exit:
714 // rkiio_aiq_sensors_list_release(&sensors_list);
715 // rkiio_aiq_sensor_ctx_destroy(ctx);
716 return 0;
717 }
718 #endif
719