summaryrefslogtreecommitdiffstats
path: root/libsensors_iio/software/simple_apps/console/linux/console_test.c
diff options
context:
space:
mode:
Diffstat (limited to 'libsensors_iio/software/simple_apps/console/linux/console_test.c')
-rw-r--r--libsensors_iio/software/simple_apps/console/linux/console_test.c742
1 files changed, 742 insertions, 0 deletions
diff --git a/libsensors_iio/software/simple_apps/console/linux/console_test.c b/libsensors_iio/software/simple_apps/console/linux/console_test.c
new file mode 100644
index 0000000..e15b20d
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/console/linux/console_test.c
@@ -0,0 +1,742 @@
+/*******************************************************************************
+ * $Id: $
+ ******************************************************************************/
+
+/*******************************************************************************
+ *
+ * Copyright (c) 2011 InvenSense Corporation, All Rights Reserved.
+ *
+ ******************************************************************************/
+//#include <linux/conio.h>
+//#include <fcntl.h>
+//#include <termios.h>
+//#include <unistd.h>
+
+//#if 0
+#include <stdio.h>
+#include <time.h>
+#include <sys/select.h>
+#include <unistd.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <stdlib.h>
+#include <linux/input.h>
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "console_test"
+
+#include "mlos.h"
+#include "invensense.h"
+#include "invensense_adv.h"
+#include "inv_sysfs_utils.h"
+#include "ml_stored_data.h"
+#include "ml_math_func.h"
+#include "ml_load_dmp.h"
+/*#else
+#include <unistd.h>
+#include <dirent.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <errno.h>
+#include <sys/stat.h>
+#include <stdlib.h>
+#include <features.h>
+#include <dirent.h>
+#include <string.h>
+#include <poll.h>
+#include <stddef.h>
+#include <linux/input.h>
+#include <time.h>
+#include <linux/time.h>
+#include "inv_sysfs_utils.h"
+#include "invensense.h"
+#include "invensense_adv.h"
+#include "ml_stored_data.h"
+#include "ml_math_func.h"
+#include "ml_load_dmp.h"
+#include "log.h"
+#endif*/
+
+/* TODO: add devices as needed. */
+#define ITG3500 (0)
+#define MPU6050 (1)
+#define BMA250 (2)
+#define NUM_DEVICES (ITG3500 + MPU6050 + BMA250)
+
+#define DEVICE MPU6050
+#define DMP_IMAGE dmp_firmware_200_latest
+#define SIX_AXES 6
+#define NINE_AXES 9
+
+#if 0
+struct input_event {
+ struct timeval time;
+ __u16 type;
+ __u16 code;
+ __s32 value;
+};
+#endif
+
+/* TODO: Add paths to other attributes.
+ * TODO: Input device paths depend on the module installation order.
+ */
+const struct inv_sysfs_names_s filenames[NUM_DEVICES] = {
+ { /* ITG3500 */
+ .buffer = "/dev/input/event0",
+ .enable = "/sys/bus/i2c/devices/4-0068/inv_gyro/fifo_enable",
+ .raw_data = "/sys/bus/i2c/devices/4-0068/inv_gyro/raw_gyro",
+ .temperature = "/sys/bus/i2c/devices/4-0068/inv_gyro/temperature",
+ .fifo_rate = "/sys/bus/i2c/devices/4-0068/inv_gyro/fifo_rate",
+ .power_state = "/sys/bus/i2c/devices/4-0068/inv_gyro/power_state",
+ .fsr = "/sys/bus/i2c/devices/4-0068/inv_gyro/FSR",
+ .lpf = "/sys/bus/i2c/devices/4-0068/inv_gyro/lpf",
+ .scale = "/sys/bus/i2c/devices/4-0068/inv_gyro/gyro_scale",
+ .temp_scale = "/sys/bus/i2c/devices/4-0068/inv_gyro/temp_scale",
+ .temp_offset = "/sys/bus/i2c/devices/4-0068/inv_gyro/temp_offset",
+ .self_test = "/sys/bus/i2c/devices/4-0068/inv_gyro/self_test",
+ .accel_en = NULL,
+ .accel_fifo_en = NULL,
+ .accel_fs = NULL,
+ .clock_source = NULL,
+ .early_suspend_en = NULL,
+ .firmware_loaded = NULL,
+ .gyro_en = NULL,
+ .gyro_fifo_en = NULL,
+ .key = NULL,
+ .raw_accel = NULL,
+ .reg_dump = NULL,
+ .tap_on = NULL,
+ .dmp_firmware = NULL
+ },
+
+ { /* MPU6050 */
+ .buffer = "/dev/input/event0",
+ .enable = "/sys/class/invensense/mpu/enable",
+ .raw_data = "/sys/class/invensense/mpu/raw_gyro",
+ .temperature = "/sys/class/invensense/mpu/temperature",
+ .fifo_rate = "/sys/class/invensense/mpu/fifo_rate",
+ .power_state = "/sys/class/invensense/mpu/power_state",
+ .fsr = "/sys/class/invensense/mpu/FSR",
+ .lpf = "/sys/class/invensense/mpu/lpf",
+ .scale = "/sys/class/invensense/mpu/gyro_scale",
+ .temp_scale = "/sys/class/invensense/mpu/temp_scale",
+ .temp_offset = "/sys/class/invensense/mpu/temp_offset",
+ .self_test = "/sys/class/invensense/mpu/self_test",
+ .accel_en = "/sys/class/invensense/mpu/accl_enable",
+ .accel_fifo_en = "/sys/class/invensense/mpu/accl_fifo_enable",
+ .accel_fs = "/sys/class/invensense/mpu/accl_fs",
+ .clock_source = "/sys/class/invensense/mpu/clock_source",
+ .early_suspend_en = "/sys/class/invensense/mpu/early_suspend_enable",
+ .firmware_loaded = "/sys/class/invensense/mpu/firmware_loaded",
+ .gyro_en = "/sys/class/invensense/mpu/gyro_enable",
+ .gyro_fifo_en = "/sys/class/invensense/mpu/gyro_fifo_enable",
+ .key = "/sys/class/invensense/mpu/key",
+ .raw_accel = "/sys/class/invensense/mpu/raw_accl",
+ .reg_dump = "/sys/class/invensense/mpu/reg_dump",
+ .tap_on = "/sys/class/invensense/mpu/tap_on",
+ .dmp_firmware = "/sys/class/invensense/mpu/dmp_firmware"
+ },
+
+ { /* BMA250 */
+ .buffer = "/dev/input/input/event1",
+ .enable = "/sys/devices/virtual/input/input1/enable",
+ .raw_data = "/sys/devices/virtual/input/input1/value",
+ .temperature = NULL,
+ .fifo_rate = NULL,
+ .power_state = NULL,
+ .fsr = NULL,
+ .lpf = NULL,
+ .scale = NULL,
+ .temp_scale = NULL,
+ .temp_offset = NULL,
+ .self_test = NULL,
+ .accel_en = NULL,
+ .accel_fifo_en = NULL,
+ .accel_fs = NULL,
+ .clock_source = NULL,
+ .early_suspend_en = NULL,
+ .firmware_loaded = NULL,
+ .gyro_en = NULL,
+ .gyro_fifo_en = NULL,
+ .key = NULL,
+ .raw_accel = NULL,
+ .reg_dump = NULL,
+ .tap_on = NULL,
+ .dmp_firmware = NULL
+ }
+};
+
+static void (*s_func_cb) (void);
+
+int inv_read_data(char *names, char *data)
+{
+ char str[8];
+ int count;
+ short s_data;
+
+ count = inv_sysfs_read((char*)names, sizeof(str), str);
+ if (count < 0)
+ return count;
+ count = sscanf(str, "%hd", &s_data);
+ *data = s_data;
+ if (count < 1)
+ return -EAGAIN;
+ return count;
+
+}
+
+void fifoCB(void)
+{
+ if (1) {
+ float gyro[3];
+ float accel[3];
+ float orient[3];
+ float rv[3];
+
+ int8_t accuracy;
+ inv_time_t timestamp;
+
+ printf("/*************************************************\n");
+ inv_get_sensor_type_gyroscope(gyro, &accuracy, &timestamp);
+ printf("Gyro %13.6f %13.4f %13.4f %5d %9lld\n",
+ gyro[0],
+ gyro[1],
+ gyro[2],
+ accuracy,
+ timestamp);
+
+ inv_get_sensor_type_accelerometer(accel, &accuracy, &timestamp);
+ printf("Accel %13.6f %13.4f %13.4f %5d %9lld\n",
+ accel[0],
+ accel[1],
+ accel[2],
+ accuracy,
+ timestamp);
+
+ inv_get_sensor_type_rotation_vector(rv, &accuracy, &timestamp);
+ printf("RV %7.3f %7.3f %7.3f %5d %9lld\n",
+ rv[0],rv[1],rv[2],accuracy,timestamp);
+
+ inv_get_sensor_type_orientation(orient, &accuracy, &timestamp);
+ printf("Orientation %7.3f %7.3f %7.3f %5d %9lld\n",
+ orient[0],orient[1],orient[2],accuracy,timestamp);
+ printf("/*************************************************\n");
+ }
+}
+
+unsigned short orient;
+signed char g_gyro_orientation[9] = {1, 0, 0,
+ 0, 1, 0,
+ 0, 0, 1};
+
+signed char g_accel_orientation[9] = {-1, 0, 0,
+ 0, -1, 0,
+ 0, 0, 1};
+float scale;
+float range;
+long sens;
+
+
+
+short mTempOffset = 0;
+short mTempScale = 0;
+bool mFirstRead = 1;
+
+/******************* FUNCTIONS *******************************/
+#if 0
+static unsigned short inv_row_2_scale(const signed char *row)
+{
+ unsigned short b;
+
+ if (row[0] > 0)
+ b = 0;
+ else if (row[0] < 0)
+ b = 4;
+ else if (row[1] > 0)
+ b = 1;
+ else if (row[1] < 0)
+ b = 5;
+ else if (row[2] > 0)
+ b = 2;
+ else if (row[2] < 0)
+ b = 6;
+ else
+ b = 7; // error
+ return b;
+}
+#endif
+
+inv_error_t inv_set_fifo_processed_callback(void (*func_cb)(void))
+{
+ s_func_cb = func_cb;
+ return INV_SUCCESS;
+}
+
+/**
+ * @brief Keyboard hit function.
+ */
+int kbhit(void)
+{
+#if 1
+ struct timeval tv;
+ fd_set read_fd;
+
+ tv.tv_sec=0;
+ tv.tv_usec=0;
+ FD_ZERO(&read_fd);
+ FD_SET(0,&read_fd);
+
+ if (select(1, &read_fd, NULL, NULL, &tv) == -1)
+ return 0;
+
+ if (FD_ISSET(0,&read_fd))
+ return 1;
+
+ return 0;
+#else
+ struct timeval tv;
+ fd_set rdfs;
+
+ tv.tv_sec = 0;
+ tv.tv_usec = 0;
+
+ FD_ZERO(&rdfs);
+ FD_SET (STDIN_FILENO, &rdfs);
+
+ select(STDIN_FILENO+1, &rdfs, NULL, NULL, &tv);
+ return FD_ISSET(STDIN_FILENO, &rdfs);
+#endif
+}
+
+inv_error_t inv_constructor_default_enable()
+{
+ inv_error_t result;
+
+ result = inv_enable_quaternion();
+ if (result) {
+ if (result == INV_ERROR_NOT_AUTHORIZED) {
+ LOGE("Enable Quaternion failed: not authorized");
+ }
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ result = inv_enable_motion_no_motion();
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ result = inv_enable_gyro_tc();
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ result = inv_enable_hal_outputs();
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ result = inv_enable_9x_sensor_fusion();
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ return result;
+}
+
+int read_attribute_sensor(int fd, char *data, unsigned int size)
+{
+ int count = 0;
+ if (fd >=0) {
+ count = read(fd, data, size);
+ if(count < 0) {
+ MPL_LOGE("read fails with error code=%d", count);
+ }
+ close(fd);
+ }
+ return count;
+}
+
+int inv_read_temperature(long long *data)
+{
+ int count = 0;
+ int fd;
+
+ if(mFirstRead) {
+ char buf[4];
+ fd = open(filenames[ITG3500].temp_scale, O_RDONLY);
+ if(fd < 0) {
+ MPL_LOGE("errors opening tempscale");
+ return -1;
+ }
+
+ memset(buf, 0, sizeof(buf));
+
+ count = read_attribute_sensor(fd, buf, sizeof(buf));
+ if(count < 0) {
+ MPL_LOGE("errors reading temp_scale");
+ return -1;
+ }
+
+ count = sscanf(buf, "%hd", &mTempScale);
+ if(count < 1)
+ return -1;
+ MPL_LOGI("temp scale = %d", mTempScale);
+
+ fd = open(filenames[ITG3500].temp_offset, O_RDONLY);
+ if(fd < 0) {
+ MPL_LOGE("errors opening tempoffset");
+ return -1;
+ }
+
+ memset(buf, 0, sizeof(buf));
+
+ count = read_attribute_sensor(fd, buf, sizeof(buf));
+ if(count < 0) {
+ MPL_LOGE("errors reading temp_offset");
+ return -1;
+ }
+
+ count = sscanf(buf, "%hd", &mTempOffset);
+ if(count < 1)
+ return -1;
+ MPL_LOGI("temp offset = %d", mTempOffset);
+
+ mFirstRead = false;
+ }
+
+ char raw_buf[25];
+ short raw;
+ long long timestamp;
+ fd = open(filenames[ITG3500].temperature, O_RDONLY);
+ if(fd < 0) {
+ MPL_LOGE("errors opening temperature");
+ return -1;
+ }
+
+ memset(raw_buf, 0, sizeof(raw_buf));
+
+ count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf));
+ if(count < 0) {
+ MPL_LOGE("errors reading temperature");
+ return -1;
+ }
+ count = sscanf(raw_buf, "%hd%lld", &raw, &timestamp);
+ if(count < -1)
+ return -1;
+ MPL_LOGI("temperature raw = %d, timestamp = %lld", raw, timestamp);
+ MPL_LOGI("temperature offset = %d", mTempOffset);
+ MPL_LOGI("temperature scale = %d", mTempScale);
+ int adjuster = 35 + ((raw-mTempOffset)/mTempScale);
+ MPL_LOGI("pre-scaled temperature = %d", adjuster);
+ MPL_LOGI("adjusted temperature = %d", adjuster*65536);
+ //data[0] = adjuster * 65536;
+ data[0] = (35 + ((raw - mTempOffset) / mTempScale)) * 65536.f;
+ data[1] = timestamp;
+ return 0;
+}
+
+int self_test(void)
+{
+ int err = 0;
+ char str[50];
+ char x[9], y[9], z[9];
+ char pass[2];
+ int fd;
+
+ fd = open((char*)filenames[DEVICE].self_test, O_RDONLY);
+ if(fd < 0) {
+ return fd;
+ }
+ memset(str, 0, sizeof(str));
+ err = read_attribute_sensor(fd, str, sizeof(str));
+ if(err < 0) {
+ return err;
+ }
+ MPL_LOGI("self_test result: %s", str);
+ printf("Self test result: %s ", str);
+ err = sscanf(str, "%[^','],%[^','],%[^','],%[^',']", x, y, z, pass);
+ if(err < 1) {
+ return err;
+ }
+ MPL_LOGI("Bias : X:Y:Z (%ld, %ld, %ld)", atol(x), atol(y), atol(z));
+ //printf("Bias : X:Y:Z (%ld, %ld, %ld)", atol(x), atol(y), atol(z));
+ if (atoi(pass)) {
+ MPL_LOGI("Result : PASS (1)");
+ printf("----> PASS (1)\n");
+ } else {
+ MPL_LOGI("Result : FAIL (0)");
+ printf("----> FAIL (0)\n");
+ }
+ return err;
+}
+
+/*******************************************************************************
+ ******************************* MAIN ******************************************
+ ******************************************************************************/
+
+/**
+ * @brief Main function
+ */
+int main(int argc, char *argv[])
+{
+ int key = 0;
+ int ready;
+ long accel[3];
+ short gyro[3];
+ long long timestamp = 0;
+ inv_error_t result;
+
+ char data;
+ unsigned char i;
+ int fd, bytes_read;
+ struct pollfd pfd;
+ unsigned long long time_stamp;
+ unsigned int time_H;
+ struct input_event ev[100];
+#ifdef INV_PLAYBACK_DBG
+ int logging = false;
+ FILE *logfile = NULL;
+#endif
+
+ result = inv_init_mpl();
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+ // Master Enabling. This also turns on power_state
+ if (inv_sysfs_write((char *)filenames[DEVICE].enable, 1) < 0)
+ printf("ERR- Failed to enable event generation\n");
+ else {
+ inv_read_data((char *)filenames[DEVICE].enable, &data);
+ printf("Event enable= %d\n", data);
+ }
+
+ // Power ON - No need after master enable above but do it anyway
+ if (inv_sysfs_write((char *)filenames[DEVICE].power_state, 1) < 0)
+ printf("ERR- Failed to set power state=1\n");
+ else {
+ inv_read_data((char *)filenames[DEVICE].power_state, &data);
+ printf("Power state: %d\n", data);
+ }
+
+ // Turn on tap
+ if (inv_sysfs_write((char *)filenames[DEVICE].tap_on, 1) < 0) {
+ printf("ERR- Failed to enable Tap On\n");
+ }
+ else {
+ inv_read_data((char *)filenames[DEVICE].tap_on, &data);
+ printf("Tap-on: %d\n", data);
+ }
+
+ // Program DMP code. No longer required to enable tap-on first
+ if ((result =
+ inv_sysfs_write((char *)filenames[DEVICE].firmware_loaded, 0)) < 0) {
+ printf("ERR- Failed to initiate DMP re-programming %d\n",result);
+ } else {
+ if ((fd = open(filenames[DEVICE].dmp_firmware, O_WRONLY)) < 0 ) {
+ printf("ERR- Failed file open to write DMP\n");
+ close(fd);
+ exit(0);
+ } else {
+ // Program 200Hz version
+ //result = write(fd, DMP_IMAGE, sizeof(DMP_IMAGE));
+ //printf("Downloaded %d byte(s) to DMP\n", result);
+ result = inv_load_dmp(fd);
+ //LOG_RESULT_LOCATION(result);
+ close(fd);
+ }
+ }
+
+ // Query DMP running. For now check by 'firmware_loaded' status
+ if (inv_read_data((char *)filenames[DEVICE].firmware_loaded, &data) < 0) {
+ printf("ERR- Failed to read 'firmware_loaded'\n");
+ } else {
+ printf("Firmware Loaded/ DMP running: %d\n", data);
+ }
+
+ inv_set_fifo_processed_callback(fifoCB);
+ result = inv_constructor_default_enable();
+ result = inv_start_mpl();
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ } else {
+ printf ("MPL started\n");
+ }
+
+ /* Gyro Setup */
+ orient = inv_orientation_matrix_to_scalar(g_gyro_orientation);
+ inv_set_gyro_orientation_and_scale(orient,2000L<<15);
+
+ /* Accel Setup */
+ orient = inv_orientation_matrix_to_scalar(g_accel_orientation);
+ /* NOTE: sens expected to be 2 (FSR) * 1L<<15 for 16 bit hardware data.
+ * The BMA250 only uses a 10 bit ADC, so we shift the data by 6 bits.
+ * 2 * 1L<<15 * 1<<6 == 1LL<<22
+ */
+ inv_set_accel_orientation_and_scale(orient, 1LL<<22);
+
+ // Enable Gyro
+ if (inv_sysfs_write((char *)filenames[DEVICE].gyro_en, 1) <0)
+ printf("ERR- Failed to enable Gyro\n");
+ else {
+ inv_read_data((char *)filenames[DEVICE].gyro_en, &data);
+ printf("Gyro enable: %d\n", data);
+ }
+
+ // Enable Accel
+ if (inv_sysfs_write((char *)filenames[DEVICE].accel_en, 1) <0)
+ printf("ERR- Failed to enable Accel\n");
+ else {
+ inv_read_data((char *)filenames[DEVICE].accel_en, &data);
+ printf("Accel enable: %d\n", data);
+ }
+
+ // polling for data
+ fd = open(filenames[DEVICE].buffer, O_RDONLY);
+ if(fd < 0) {
+ MPL_LOGE("Cannot open device event buffer");
+ }
+
+ pfd.fd = fd;
+ pfd.events = POLLIN;
+
+ while (1) {
+
+ result = kbhit();
+ if (result) {
+ key = getchar();
+ } else {
+ key = 0;
+ }
+ if (key == 'l') {
+ MPL_LOGI(" 'l' - load calibration file");
+ inv_load_calibration();
+ }
+ if (key == 't') {
+ MPL_LOGI(" 't' - self test");
+ self_test();
+ }
+ if (key == 'q') {
+ MPL_LOGI(" 'q' - store calibration file");
+ inv_store_calibration();
+ break;
+ }
+#ifdef INV_PLAYBACK_DBG
+ if (key == 's') {
+ if (!logging) {
+ MPL_LOGI(" 's' - toggle logging on");
+ logfile = fopen("/data/playback.bin", "wb");
+ if (logfile) {
+ inv_turn_on_data_logging(logfile);
+ logging = true;
+ } else {
+ MPL_LOGI("Error : "
+ "cannot open log file '/data/playback.bin'");
+ }
+ } else {
+ MPL_LOGI(" 's' - toggle logging off");
+ inv_turn_off_data_logging();
+ fclose(logfile);
+ logging = false;
+ }
+ break;
+ }
+#endif
+
+ ready = poll(&pfd, 1, 100);
+ if (ready) {
+ bytes_read = read_attribute_sensor(fd, (char *)ev,
+ sizeof(struct input_event) * SIX_AXES);
+ //bytes_read= read(fd, &ev, sizeof(struct input_event) * SIX_AXES);
+ if (bytes_read > 0) {
+ int executed;
+
+ for (i = 0; i < bytes_read / sizeof(struct input_event); i++) {
+ if (ev[i].type == EV_REL) {
+ switch (ev[i].code) {
+ case REL_X:
+ printf("REL_X\n");
+ gyro[0]= ev[i].value; //Gyro X
+ printf("Gyro X:%5d ", gyro[0]);
+ break;
+ case REL_Y:
+ printf("REL_Y\n");
+ gyro[1]= ev[i].value; //Gyro Y
+ printf("Gyro Y:%5d ", gyro[1]);
+ break;
+ case REL_Z:
+ printf("REL_Z\n");
+ gyro[2]= ev[i].value; //Gyro Z
+ printf("Gyro Z:%5d ", gyro[2]);
+ break;
+ case REL_RX:
+ printf("REL_RX\n");
+ accel[0]= ev[i].value; //Accel X
+ printf("Accl X:%5ld ", accel[0]);
+ break;
+ case REL_RY:
+ printf("REL_RY\n");
+ accel[1]= ev[i].value; //Accel Y
+ printf("Accl Y:%5ld ", accel[1]);
+ break;
+ case REL_RZ:
+ printf("REL_RZ\n");
+ accel[2]= ev[i].value; //Accel Z
+ printf("Accl Z:%5ld ", accel[2]);
+ break;
+ case REL_MISC:
+ time_H= ev[i].value;
+ break;
+ case REL_WHEEL:
+ time_stamp = ((unsigned long long)(time_H) << 32) +
+ (unsigned int)ev[i].value;
+ break;
+ default:
+ printf("ERR- Un-recognized event code: %5d ", ev[i].code);
+ break;
+ }
+ } else {
+#if 0
+ clock_gettime(CLOCK_MONOTONIC, &timer);
+ curr_time= timer.tv_nsec + timer.tv_sec * 1000000000LL;
+ printf("Curr time= %lld, Dev time stamp= %lld, Time diff= %d ms\n", curr_time, time_stamp, (curr_time-time_stamp)/1000000LL);
+#endif
+ }
+ }
+
+ // build & process gyro + accel data
+ result = inv_build_gyro(gyro, (inv_time_t)timestamp, &executed);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ } else if ((result = inv_build_accel(accel, 0,
+ (inv_time_t)timestamp,
+ &executed))) {
+ LOG_RESULT_LOCATION(result);
+ }
+ if (executed) {
+ printf("Exec on data Ok\n");
+ s_func_cb();
+ }
+
+ } else {
+ //printf ("ERR- No data!\n");
+ }
+
+ } else { MPL_LOGV("Device not ready"); }
+ }
+ close(fd);
+
+#ifdef INV_PLAYBACK_DBG
+ if (logging) {
+ inv_turn_off_data_logging();
+ fclose(logfile);
+ }
+#endif
+
+ return 0;
+}
generated by cgit v1.2.3 (git 2.25.1) at 2024-06-24 15:59:44 +0000