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