1c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/* 2c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro $License: 3c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro Copyright (C) 2012 InvenSense Corporation, All Rights Reserved. 4c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro $ 5c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 6c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 7c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/******************************************************************************* 8c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * 9c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * $Id:$ 10c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * 11c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ******************************************************************************/ 12c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 13c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/* 14c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro Includes, Defines, and Macros 15c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 16c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 17c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#undef MPL_LOG_NDEBUG 18c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define MPL_LOG_NDEBUG 0 /* turn to 0 to enable verbose logging */ 19c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 20c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "log.h" 21c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#undef MPL_LOG_TAG 22c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define MPL_LOG_TAG "MPL-playback" 23c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 24c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "and_constructor.h" 25c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "mlos.h" 26c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "invensense.h" 27c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "invensense_adv.h" 28c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 29c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/* 30c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro Typedef 31c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 32c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostruct inv_construct_t { 33c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int product; /**< Gyro Product Number */ 34c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int debug_mode; 35c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int last_mode; 36c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro FILE *file; 37c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int dmp_version; 38c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int left_in_buffer; 39c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define FIFO_READ_SIZE 100 40c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro unsigned char fifo_data[FIFO_READ_SIZE]; 41c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int gyro_enable; 42c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int accel_enable; 43c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int compass_enable; 44c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int quat_enable; 45c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro}; 46c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 47c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/* 48c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro Globals 49c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 50c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic struct inv_construct_t inv_construct = {0}; 51c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void (*s_func_cb)(void); 52c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic char playback_filename[101] = "/data/playback.bin"; 53c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostruct fifo_dmp_config fifo_dmp_cfg = {0}; 54c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 55c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/* 56c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro Functions 57c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 58c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_playback_filename(char *filename, int length) 59c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 60c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (length > 100) { 61c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGE("Error : file name and path too long, 100 characters limit\n"); 62c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return; 63c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 64c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro strncpy(playback_filename, filename, length); 65c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 66c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 67c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_constructor_setup(void) 68c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 69c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro unsigned short orient; 70c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro extern signed char g_gyro_orientation[9]; 71c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro extern signed char g_accel_orientation[9]; 72c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro extern signed char g_compass_orientation[9]; 73c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro float scale = 2.f; 74c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long sens; 75c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 76c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // gyro setup 77c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro orient = inv_orientation_matrix_to_scalar(g_gyro_orientation); 78c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_gyro_orientation_and_scale(orient, 2000L << 15); 79c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 80c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // accel setup 81c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro orient = inv_orientation_matrix_to_scalar(g_accel_orientation); 82c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro scale = 2.f; 83c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro sens = (long)(scale * (1L << 15)); 84c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_accel_orientation_and_scale(orient, sens); 85c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 86c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // compass setup 87c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro orient = inv_orientation_matrix_to_scalar(g_compass_orientation); 88c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // scale is the max value of the compass in micro Tesla. 89c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro scale = 5000.f; 90c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro sens = (long)(scale * (1L << 15)); 91c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_compass_orientation_and_scale(orient, sens); 92c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 93c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 94c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 95c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 96c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_set_fifo_processed_callback(void (*func_cb)(void)) 97c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 98c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro s_func_cb = func_cb; 99c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 100c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 101c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 102c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid int32_to_long(int32_t in[], long out[], int length) 103c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 104c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int ii; 105c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro for (ii = 0; ii < length; ii++) 106c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro out[ii] = (long)in[ii]; 107c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 108c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 109c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_playback(void) 110c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 111c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_rd_dbg_states type; 112c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t ts; 113c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int32_t buffer[4]; 114c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro short gyro[3]; 115c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro size_t r = 1; 116c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int32_t orientation; 117c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int32_t sensitivity, sample_rate_us = 0; 118c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 119c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // Check to make sure we were request to playback 120c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (inv_construct.debug_mode != RD_PLAYBACK) { 121c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGE("%s|%s|%d error: debug_mode != RD_PLAYBACK\n", 122c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro __FILE__, __func__, __LINE__); 123c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_ERROR; 124c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 125c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 126c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (inv_construct.file == NULL) { 127c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_construct.file = fopen(playback_filename, "rb"); 128c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (!inv_construct.file) { 129c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGE("Error : cannot find or open playback file '%s'\n", 130c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro playback_filename); 131c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_ERROR_FILE_OPEN; 132c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 133c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 134c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 135c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro while (1) { 136c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(&type, sizeof(type), 1, inv_construct.file); 137c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (r == 0) { 138c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("read 0 bytes, PLAYBACK file closed\n"); 139c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_construct.debug_mode = RD_NO_DEBUG; 140c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro fclose(inv_construct.file); 141c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 142c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 143c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro //MPL_LOGV("TYPE : %d, %d\n", type); 144c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro switch (type) { 145c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case PLAYBACK_DBG_TYPE_GYRO: 146c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(gyro, sizeof(gyro[0]), 3, inv_construct.file); 147c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(&ts, sizeof(ts), 1, inv_construct.file); 148c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_build_gyro(gyro, ts); 149c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("PLAYBACK_DBG_TYPE_GYRO, %+d, %+d, %+d, %+lld\n", 150c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro gyro[0], gyro[1], gyro[2], ts); 151c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 152c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case PLAYBACK_DBG_TYPE_ACCEL: 153c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro { 154c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long accel[3]; 155c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(buffer, sizeof(buffer[0]), 3, inv_construct.file); 156c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(&ts, sizeof(ts), 1, inv_construct.file); 157c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int32_to_long(buffer, accel, 3); 158c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_build_accel(accel, 0, ts); 159c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("PLAYBACK_DBG_TYPE_ACCEL, %+d, %+d, %+d, %lld\n", 160c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro buffer[0], buffer[1], buffer[2], ts); 161c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 162c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 163c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case PLAYBACK_DBG_TYPE_COMPASS: 164c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro { 165c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long compass[3]; 166c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(buffer, sizeof(buffer[0]), 3, inv_construct.file); 167c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(&ts, sizeof(ts), 1, inv_construct.file); 168c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int32_to_long(buffer, compass, 3); 169c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_build_compass(compass, 0, ts); 170c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("PLAYBACK_DBG_TYPE_COMPASS, %+d, %+d, %+d, %lld\n", 171c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro buffer[0], buffer[1], buffer[2], ts); 172c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 173c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 174c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case PLAYBACK_DBG_TYPE_TEMPERATURE: 175c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(buffer, sizeof(buffer[0]), 1, inv_construct.file); 176c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(&ts, sizeof(ts), 1, inv_construct.file); 177c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_build_temp(buffer[0], ts); 178c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("PLAYBACK_DBG_TYPE_TEMPERATURE, %+d, %lld\n", 179c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro buffer[0], ts); 180c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 181c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case PLAYBACK_DBG_TYPE_QUAT: 182c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro { 183c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long quat[4]; 184c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(buffer, sizeof(buffer[0]), 4, inv_construct.file); 185c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(&ts, sizeof(ts), 1, inv_construct.file); 186c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int32_to_long(buffer, quat, 4); 187c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_build_quat(quat, INV_BIAS_APPLIED, ts); 188c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("PLAYBACK_DBG_TYPE_QUAT, %+d, %+d, %+d, %+d, %lld\n", 189c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro buffer[0], buffer[1], buffer[2], buffer[3], ts); 190c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 191c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 192c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case PLAYBACK_DBG_TYPE_EXECUTE: 193c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("PLAYBACK_DBG_TYPE_EXECUTE\n"); 194c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_execute_on_data(); 195c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (s_func_cb) 196c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro s_func_cb(); 197c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro //done = 1; 198c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 199c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 200c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case PLAYBACK_DBG_TYPE_G_ORIENT: 201c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("PLAYBACK_DBG_TYPE_G_ORIENT\n"); 202c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(&orientation, sizeof(orientation), 1, inv_construct.file); 203c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(&sensitivity, sizeof(sensitivity), 1, inv_construct.file); 204c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_gyro_orientation_and_scale(orientation, sensitivity); 205c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 206c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case PLAYBACK_DBG_TYPE_A_ORIENT: 207c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("PLAYBACK_DBG_TYPE_A_ORIENT\n"); 208c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(&orientation, sizeof(orientation), 1, inv_construct.file); 209c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(&sensitivity, sizeof(sensitivity), 1, inv_construct.file); 210c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_accel_orientation_and_scale(orientation, sensitivity); 211c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 212c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case PLAYBACK_DBG_TYPE_C_ORIENT: 213c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("PLAYBACK_DBG_TYPE_C_ORIENT\n"); 214c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(&orientation, sizeof(orientation), 1, inv_construct.file); 215c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(&sensitivity, sizeof(sensitivity), 1, inv_construct.file); 216c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_compass_orientation_and_scale(orientation, sensitivity); 217c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 218c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 219c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case PLAYBACK_DBG_TYPE_G_SAMPLE_RATE: 220c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(&sample_rate_us, sizeof(sample_rate_us), 221c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 1, inv_construct.file); 222c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_gyro_sample_rate(sample_rate_us); 223c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("PLAYBACK_DBG_TYPE_G_SAMPLE_RATE => %d\n", 224c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro sample_rate_us); 225c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 226c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case PLAYBACK_DBG_TYPE_A_SAMPLE_RATE: 227c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(&sample_rate_us, sizeof(sample_rate_us), 228c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 1, inv_construct.file); 229c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_accel_sample_rate(sample_rate_us); 230c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("PLAYBACK_DBG_TYPE_A_SAMPLE_RATE => %d\n", 231c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro sample_rate_us); 232c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 233c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case PLAYBACK_DBG_TYPE_C_SAMPLE_RATE: 234c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(&sample_rate_us, sizeof(sample_rate_us), 235c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 1, inv_construct.file); 236c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_compass_sample_rate(sample_rate_us); 237c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("PLAYBACK_DBG_TYPE_C_SAMPLE_RATE => %d\n", 238c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro sample_rate_us); 239c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 240c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 241c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case PLAYBACK_DBG_TYPE_GYRO_OFF: 242c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("PLAYBACK_DBG_TYPE_GYRO_OFF\n"); 243c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_gyro_was_turned_off(); 244c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 245c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case PLAYBACK_DBG_TYPE_ACCEL_OFF: 246c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("PLAYBACK_DBG_TYPE_ACCEL_OFF\n"); 247c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_accel_was_turned_off(); 248c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 249c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case PLAYBACK_DBG_TYPE_COMPASS_OFF: 250c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("PLAYBACK_DBG_TYPE_COMPASS_OFF\n"); 251c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_compass_was_turned_off(); 252c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 253c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case PLAYBACK_DBG_TYPE_QUAT_OFF: 254c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("PLAYBACK_DBG_TYPE_QUAT_OFF\n"); 255c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_quaternion_sensor_was_turned_off(); 256c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 257c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 258c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE: 259c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGV("PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE\n"); 260c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro r = fread(&sample_rate_us, sizeof(sample_rate_us), 261c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 1, inv_construct.file); 262c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_quat_sample_rate(sample_rate_us); 263c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 264c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro default: 265c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro //MPL_LOGV("PLAYBACK file closed\n"); 266c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro fclose(inv_construct.file); 267c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro MPL_LOGE("%s|%s|%d error: unrecognized log type '%d', " 268c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro "PLAYBACK file closed\n", 269c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro __FILE__, __func__, __LINE__, type); 270c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_ERROR; 271c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 272c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 273c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro msleep(1); 274c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 275c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_construct.debug_mode = RD_NO_DEBUG; 276c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro fclose(inv_construct.file); 277c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 278c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 279c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 280c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 281c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Turns on/off playback and record modes 282c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param mode Turn on recording mode with RD_RECORD and turn off recording mode with 283c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* RD_NO_DBG. Turn on playback mode with RD_PLAYBACK. 284c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 285c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_debug_mode(rd_dbg_mode mode) 286c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 287c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#ifdef INV_PLAYBACK_DBG 288c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_construct.debug_mode = mode; 289c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#endif 290c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 291c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 292c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_constructor_start(void) 293c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 294c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_error_t result; 295c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro unsigned char divider; 296c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro //int gest_enabled = inv_get_gesture_enable(); 297c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 298c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // start the software 299c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro result = inv_start_mpl(); 300c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (result) { 301c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro LOG_RESULT_LOCATION(result); 302c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return result; 303c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 304c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 305c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro /* 306c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (inv_construct.dmp_version == WIN8_DMP_VERSION) { 307c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int fifo_divider; 308c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro divider = 4; // 4 means 200Hz DMP 309c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro fifo_divider = 3; 310c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // Set Gyro Sample Rate in MPL in micro seconds 311c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_gyro_sample_rate(1000L*(divider+1)*(fifo_divider+1)); 312c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 313c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // Set Gyro Sample Rate in MPL in micro seconds 314c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_quat_sample_rate(1000L*(divider+1)*(fifo_divider+1)); 315c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 316c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // Set Compass Sample Rate in MPL in micro seconds 317c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_compass_sample_rate(1000L*(divider+1)*(fifo_divider+1)); 318c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 319c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // Set Accel Sample Rate in MPL in micro seconds 320c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_accel_sample_rate(1000L*(divider+1)*(fifo_divider+1)); 321c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else if (gest_enabled) { 322c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int fifo_divider; 323c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro unsigned char mpl_divider; 324c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 325c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_send_interrupt_word(); 326c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_send_sensor_data(INV_ALL & INV_GYRO_ACC_MASK); 327c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_send_quaternion(); 328c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 329c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro divider = fifo_dmp_cfg.sample_divider; 330c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro mpl_divider = fifo_dmp_cfg.mpl_divider; 331c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 332c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // Set Gyro Sample Rate in MPL in micro seconds 333c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_gyro_sample_rate(1000L*(mpl_divider+1)); 334c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 335c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // Set Gyro Sample Rate in MPL in micro seconds 336c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_quat_sample_rate(1000L*(mpl_divider+1)); 337c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 338c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // Set Compass Sample Rate in MPL in micro seconds 339c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_compass_sample_rate(1000L*(mpl_divider+1)); 340c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 341c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // Set Accel Sample Rate in MPL in micro seconds 342c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_accel_sample_rate(1000L*(mpl_divider+1)); 343c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else 344c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 345c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro { 346c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro divider = 9; 347c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // set gyro sample sate in MPL in micro seconds 348c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_gyro_sample_rate(1000L*(divider+1)); 349c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // set compass sample rate in MPL in micro seconds 350c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_compass_sample_rate(1000L*(divider+1)); 351c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // set accel sample rate in MPL in micro seconds 352c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_accel_sample_rate(1000L*(divider+1)); 353c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 354c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 355c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // setup the scale factors and orientations and other parameters 356c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro result = inv_constructor_setup(); 357c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 358c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return result; 359c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 360c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 361c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_constructor_default_enable() 362c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 363c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro INV_ERROR_CHECK(inv_enable_quaternion()); 364c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro INV_ERROR_CHECK(inv_enable_fast_nomot()); 365c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro INV_ERROR_CHECK(inv_enable_heading_from_gyro()); 366c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro INV_ERROR_CHECK(inv_enable_compass_bias_w_gyro()); 367c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro INV_ERROR_CHECK(inv_enable_hal_outputs()); 368c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro INV_ERROR_CHECK(inv_enable_vector_compass_cal()); 369c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro INV_ERROR_CHECK(inv_enable_9x_sensor_fusion()); 370c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro INV_ERROR_CHECK(inv_enable_gyro_tc()); 371c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro INV_ERROR_CHECK(inv_enable_no_gyro_fusion()); 372c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro INV_ERROR_CHECK(inv_enable_in_use_auto_calibration()); 373c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro INV_ERROR_CHECK(inv_enable_magnetic_disturbance()); 374c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 375c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 376c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 377c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** 378c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @} 379c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 380