diff options
Diffstat (limited to '6515/libsensors_iio/software/simple_apps/playback/linux/and_constructor.c')
| -rw-r--r-- | 6515/libsensors_iio/software/simple_apps/playback/linux/and_constructor.c | 379 |
1 files changed, 379 insertions, 0 deletions
diff --git a/6515/libsensors_iio/software/simple_apps/playback/linux/and_constructor.c b/6515/libsensors_iio/software/simple_apps/playback/linux/and_constructor.c new file mode 100644 index 0000000..c4f09a6 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/playback/linux/and_constructor.c @@ -0,0 +1,379 @@ +/* + $License: + Copyright (C) 2012 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id:$ + * + ******************************************************************************/ + +/* + Includes, Defines, and Macros +*/ + +#undef MPL_LOG_NDEBUG +#define MPL_LOG_NDEBUG 0 /* turn to 0 to enable verbose logging */ + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-playback" + +#include "and_constructor.h" +#include "mlos.h" +#include "invensense.h" +#include "invensense_adv.h" + +/* + Typedef +*/ +struct inv_construct_t { + int product; /**< Gyro Product Number */ + int debug_mode; + int last_mode; + FILE *file; + int dmp_version; + int left_in_buffer; +#define FIFO_READ_SIZE 100 + unsigned char fifo_data[FIFO_READ_SIZE]; + int gyro_enable; + int accel_enable; + int compass_enable; + int quat_enable; +}; + +/* + Globals +*/ +static struct inv_construct_t inv_construct = {0}; +static void (*s_func_cb)(void); +static char playback_filename[101] = "/data/playback.bin"; +struct fifo_dmp_config fifo_dmp_cfg = {0}; + +/* + Functions +*/ +void inv_set_playback_filename(char *filename, int length) +{ + if (length > 100) { + MPL_LOGE("Error : file name and path too long, 100 characters limit\n"); + return; + } + strncpy(playback_filename, filename, length); +} + +inv_error_t inv_constructor_setup(void) +{ + unsigned short orient; + extern signed char g_gyro_orientation[9]; + extern signed char g_accel_orientation[9]; + extern signed char g_compass_orientation[9]; + float scale = 2.f; + long sens; + + // 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); + scale = 2.f; + sens = (long)(scale * (1L << 15)); + inv_set_accel_orientation_and_scale(orient, sens); + + // compass setup + orient = inv_orientation_matrix_to_scalar(g_compass_orientation); + // scale is the max value of the compass in micro Tesla. + scale = 5000.f; + sens = (long)(scale * (1L << 15)); + inv_set_compass_orientation_and_scale(orient, sens); + + return INV_SUCCESS; +} + +inv_error_t inv_set_fifo_processed_callback(void (*func_cb)(void)) +{ + s_func_cb = func_cb; + return INV_SUCCESS; +} + +void int32_to_long(int32_t in[], long out[], int length) +{ + int ii; + for (ii = 0; ii < length; ii++) + out[ii] = (long)in[ii]; +} + +inv_error_t inv_playback(void) +{ + inv_rd_dbg_states type; + inv_time_t ts; + int32_t buffer[4]; + short gyro[3]; + size_t r = 1; + int32_t orientation; + int32_t sensitivity, sample_rate_us = 0; + + // Check to make sure we were request to playback + if (inv_construct.debug_mode != RD_PLAYBACK) { + MPL_LOGE("%s|%s|%d error: debug_mode != RD_PLAYBACK\n", + __FILE__, __func__, __LINE__); + return INV_ERROR; + } + + if (inv_construct.file == NULL) { + inv_construct.file = fopen(playback_filename, "rb"); + if (!inv_construct.file) { + MPL_LOGE("Error : cannot find or open playback file '%s'\n", + playback_filename); + return INV_ERROR_FILE_OPEN; + } + } + + while (1) { + r = fread(&type, sizeof(type), 1, inv_construct.file); + if (r == 0) { + MPL_LOGV("read 0 bytes, PLAYBACK file closed\n"); + inv_construct.debug_mode = RD_NO_DEBUG; + fclose(inv_construct.file); + break; + } + //MPL_LOGV("TYPE : %d, %d\n", type); + switch (type) { + case PLAYBACK_DBG_TYPE_GYRO: + r = fread(gyro, sizeof(gyro[0]), 3, inv_construct.file); + r = fread(&ts, sizeof(ts), 1, inv_construct.file); + inv_build_gyro(gyro, ts); + MPL_LOGV("PLAYBACK_DBG_TYPE_GYRO, %+d, %+d, %+d, %+lld\n", + gyro[0], gyro[1], gyro[2], ts); + break; + case PLAYBACK_DBG_TYPE_ACCEL: + { + long accel[3]; + r = fread(buffer, sizeof(buffer[0]), 3, inv_construct.file); + r = fread(&ts, sizeof(ts), 1, inv_construct.file); + int32_to_long(buffer, accel, 3); + inv_build_accel(accel, 0, ts); + MPL_LOGV("PLAYBACK_DBG_TYPE_ACCEL, %+d, %+d, %+d, %lld\n", + buffer[0], buffer[1], buffer[2], ts); + break; + } + case PLAYBACK_DBG_TYPE_COMPASS: + { + long compass[3]; + r = fread(buffer, sizeof(buffer[0]), 3, inv_construct.file); + r = fread(&ts, sizeof(ts), 1, inv_construct.file); + int32_to_long(buffer, compass, 3); + inv_build_compass(compass, 0, ts); + MPL_LOGV("PLAYBACK_DBG_TYPE_COMPASS, %+d, %+d, %+d, %lld\n", + buffer[0], buffer[1], buffer[2], ts); + break; + } + case PLAYBACK_DBG_TYPE_TEMPERATURE: + r = fread(buffer, sizeof(buffer[0]), 1, inv_construct.file); + r = fread(&ts, sizeof(ts), 1, inv_construct.file); + inv_build_temp(buffer[0], ts); + MPL_LOGV("PLAYBACK_DBG_TYPE_TEMPERATURE, %+d, %lld\n", + buffer[0], ts); + break; + case PLAYBACK_DBG_TYPE_QUAT: + { + long quat[4]; + r = fread(buffer, sizeof(buffer[0]), 4, inv_construct.file); + r = fread(&ts, sizeof(ts), 1, inv_construct.file); + int32_to_long(buffer, quat, 4); + inv_build_quat(quat, INV_BIAS_APPLIED, ts); + MPL_LOGV("PLAYBACK_DBG_TYPE_QUAT, %+d, %+d, %+d, %+d, %lld\n", + buffer[0], buffer[1], buffer[2], buffer[3], ts); + break; + } + case PLAYBACK_DBG_TYPE_EXECUTE: + MPL_LOGV("PLAYBACK_DBG_TYPE_EXECUTE\n"); + inv_execute_on_data(); + if (s_func_cb) + s_func_cb(); + //done = 1; + break; + + case PLAYBACK_DBG_TYPE_G_ORIENT: + MPL_LOGV("PLAYBACK_DBG_TYPE_G_ORIENT\n"); + r = fread(&orientation, sizeof(orientation), 1, inv_construct.file); + r = fread(&sensitivity, sizeof(sensitivity), 1, inv_construct.file); + inv_set_gyro_orientation_and_scale(orientation, sensitivity); + break; + case PLAYBACK_DBG_TYPE_A_ORIENT: + MPL_LOGV("PLAYBACK_DBG_TYPE_A_ORIENT\n"); + r = fread(&orientation, sizeof(orientation), 1, inv_construct.file); + r = fread(&sensitivity, sizeof(sensitivity), 1, inv_construct.file); + inv_set_accel_orientation_and_scale(orientation, sensitivity); + break; + case PLAYBACK_DBG_TYPE_C_ORIENT: + MPL_LOGV("PLAYBACK_DBG_TYPE_C_ORIENT\n"); + r = fread(&orientation, sizeof(orientation), 1, inv_construct.file); + r = fread(&sensitivity, sizeof(sensitivity), 1, inv_construct.file); + inv_set_compass_orientation_and_scale(orientation, sensitivity); + break; + + case PLAYBACK_DBG_TYPE_G_SAMPLE_RATE: + r = fread(&sample_rate_us, sizeof(sample_rate_us), + 1, inv_construct.file); + inv_set_gyro_sample_rate(sample_rate_us); + MPL_LOGV("PLAYBACK_DBG_TYPE_G_SAMPLE_RATE => %d\n", + sample_rate_us); + break; + case PLAYBACK_DBG_TYPE_A_SAMPLE_RATE: + r = fread(&sample_rate_us, sizeof(sample_rate_us), + 1, inv_construct.file); + inv_set_accel_sample_rate(sample_rate_us); + MPL_LOGV("PLAYBACK_DBG_TYPE_A_SAMPLE_RATE => %d\n", + sample_rate_us); + break; + case PLAYBACK_DBG_TYPE_C_SAMPLE_RATE: + r = fread(&sample_rate_us, sizeof(sample_rate_us), + 1, inv_construct.file); + inv_set_compass_sample_rate(sample_rate_us); + MPL_LOGV("PLAYBACK_DBG_TYPE_C_SAMPLE_RATE => %d\n", + sample_rate_us); + break; + + case PLAYBACK_DBG_TYPE_GYRO_OFF: + MPL_LOGV("PLAYBACK_DBG_TYPE_GYRO_OFF\n"); + inv_gyro_was_turned_off(); + break; + case PLAYBACK_DBG_TYPE_ACCEL_OFF: + MPL_LOGV("PLAYBACK_DBG_TYPE_ACCEL_OFF\n"); + inv_accel_was_turned_off(); + break; + case PLAYBACK_DBG_TYPE_COMPASS_OFF: + MPL_LOGV("PLAYBACK_DBG_TYPE_COMPASS_OFF\n"); + inv_compass_was_turned_off(); + break; + case PLAYBACK_DBG_TYPE_QUAT_OFF: + MPL_LOGV("PLAYBACK_DBG_TYPE_QUAT_OFF\n"); + inv_quaternion_sensor_was_turned_off(); + break; + + case PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE: + MPL_LOGV("PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE\n"); + r = fread(&sample_rate_us, sizeof(sample_rate_us), + 1, inv_construct.file); + inv_set_quat_sample_rate(sample_rate_us); + break; + default: + //MPL_LOGV("PLAYBACK file closed\n"); + fclose(inv_construct.file); + MPL_LOGE("%s|%s|%d error: unrecognized log type '%d', " + "PLAYBACK file closed\n", + __FILE__, __func__, __LINE__, type); + return INV_ERROR; + } + } + msleep(1); + + inv_construct.debug_mode = RD_NO_DEBUG; + fclose(inv_construct.file); + + return INV_SUCCESS; +} + +/** Turns on/off playback and record modes +* @param mode Turn on recording mode with RD_RECORD and turn off recording mode with +* RD_NO_DBG. Turn on playback mode with RD_PLAYBACK. +*/ +void inv_set_debug_mode(rd_dbg_mode mode) +{ +#ifdef INV_PLAYBACK_DBG + inv_construct.debug_mode = mode; +#endif +} + +inv_error_t inv_constructor_start(void) +{ + inv_error_t result; + unsigned char divider; + //int gest_enabled = inv_get_gesture_enable(); + + // start the software + result = inv_start_mpl(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* + if (inv_construct.dmp_version == WIN8_DMP_VERSION) { + int fifo_divider; + divider = 4; // 4 means 200Hz DMP + fifo_divider = 3; + // Set Gyro Sample Rate in MPL in micro seconds + inv_set_gyro_sample_rate(1000L*(divider+1)*(fifo_divider+1)); + + // Set Gyro Sample Rate in MPL in micro seconds + inv_set_quat_sample_rate(1000L*(divider+1)*(fifo_divider+1)); + + // Set Compass Sample Rate in MPL in micro seconds + inv_set_compass_sample_rate(1000L*(divider+1)*(fifo_divider+1)); + + // Set Accel Sample Rate in MPL in micro seconds + inv_set_accel_sample_rate(1000L*(divider+1)*(fifo_divider+1)); + } else if (gest_enabled) { + int fifo_divider; + unsigned char mpl_divider; + + inv_send_interrupt_word(); + inv_send_sensor_data(INV_ALL & INV_GYRO_ACC_MASK); + inv_send_quaternion(); + + divider = fifo_dmp_cfg.sample_divider; + mpl_divider = fifo_dmp_cfg.mpl_divider; + + // Set Gyro Sample Rate in MPL in micro seconds + inv_set_gyro_sample_rate(1000L*(mpl_divider+1)); + + // Set Gyro Sample Rate in MPL in micro seconds + inv_set_quat_sample_rate(1000L*(mpl_divider+1)); + + // Set Compass Sample Rate in MPL in micro seconds + inv_set_compass_sample_rate(1000L*(mpl_divider+1)); + + // Set Accel Sample Rate in MPL in micro seconds + inv_set_accel_sample_rate(1000L*(mpl_divider+1)); + } else + */ + { + divider = 9; + // set gyro sample sate in MPL in micro seconds + inv_set_gyro_sample_rate(1000L*(divider+1)); + // set compass sample rate in MPL in micro seconds + inv_set_compass_sample_rate(1000L*(divider+1)); + // set accel sample rate in MPL in micro seconds + inv_set_accel_sample_rate(1000L*(divider+1)); + } + + // setup the scale factors and orientations and other parameters + result = inv_constructor_setup(); + + return result; +} + +inv_error_t inv_constructor_default_enable() +{ + INV_ERROR_CHECK(inv_enable_quaternion()); + INV_ERROR_CHECK(inv_enable_fast_nomot()); + INV_ERROR_CHECK(inv_enable_heading_from_gyro()); + INV_ERROR_CHECK(inv_enable_compass_bias_w_gyro()); + INV_ERROR_CHECK(inv_enable_hal_outputs()); + INV_ERROR_CHECK(inv_enable_vector_compass_cal()); + INV_ERROR_CHECK(inv_enable_9x_sensor_fusion()); + INV_ERROR_CHECK(inv_enable_gyro_tc()); + INV_ERROR_CHECK(inv_enable_no_gyro_fusion()); + INV_ERROR_CHECK(inv_enable_in_use_auto_calibration()); + INV_ERROR_CHECK(inv_enable_magnetic_disturbance()); + return INV_SUCCESS; +} + +/** + * @} + */ |