1 /******************************************************************************* 2 * Copyright (c) 2012 InvenSense Corporation, All Rights Reserved. 3 ******************************************************************************/ 4 5 /******************************************************************************* 6 * 7 * $Id: main.c 6146 2011-10-04 18:33:51Z jcalizo $ 8 * 9 ******************************************************************************/ 10 11 #include <stdio.h> 12 #include <stdlib.h> 13 #include <time.h> 14 15 #include "invensense.h" 16 #include "invensense_adv.h" 17 #include "and_constructor.h" 18 #include "ml_math_func.h" 19 #include "datalogger_outputs.h" 20 21 #include "console_helper.h" 22 23 #include "mlos.h" 24 #include "mlsl.h" 25 26 #include "testsupport.h" 27 28 #include "log.h" 29 #undef MPL_LOG_TAG 30 #define MPL_LOG_TAG "MPL-playback" 31 32 /* 33 Defines & Macros 34 */ 35 #define UNPACK_3ELM_ARRAY(a) (a)[0], (a)[1], (a)[2] 36 #define UNPACK_4ELM_ARRAY(a) UNPACK_3ELM_ARRAY(a), (a)[3] 37 #define COMPONENT_NAME_MAX_LEN (30) 38 #define DEF_NAME(x) (#x) 39 40 #define PRINT_ON_CONSOLE(...) \ 41 if (print_on_screen) \ 42 printf(__VA_ARGS__) 43 #define PRINT_ON_FILE(...) \ 44 if(stream_file) \ 45 fprintf(stream_file, __VA_ARGS__) 46 47 #define PRINT(...) \ 48 PRINT_ON_CONSOLE(__VA_ARGS__); \ 49 PRINT_ON_FILE(__VA_ARGS__) 50 #define PRINT_FLOAT(width, prec, data) \ 51 PRINT_ON_CONSOLE("%+*.*f", \ 52 width, prec, data); \ 53 PRINT_ON_FILE("%+f", data) 54 #define PRINT_INT(width, data) \ 55 PRINT_ON_CONSOLE("%+*d", width, data); \ 56 PRINT_ON_FILE("%+d", data); 57 #define PRINT_LONG(width, data) \ 58 PRINT_ON_CONSOLE("%+*ld", width, data); \ 59 PRINT_ON_FILE("%+ld", data); 60 61 #define PRINT_3ELM_ARRAY_FLOAT(w, p, data) \ 62 PRINT_FLOAT(w, p, data[0]); \ 63 PRINT(", "); \ 64 PRINT_FLOAT(w, p, data[1]); \ 65 PRINT(", "); \ 66 PRINT_FLOAT(w, p, data[2]); \ 67 PRINT(", "); 68 #define PRINT_4ELM_ARRAY_FLOAT(w, p, data) \ 69 PRINT_3ELM_ARRAY_FLOAT(w, p, data); \ 70 PRINT_FLOAT(w, p, data[3]); \ 71 PRINT(", "); 72 73 #define PRINT_3ELM_ARRAY_LONG(w, data) \ 74 PRINT_LONG(w, data[0]); \ 75 PRINT(", "); \ 76 PRINT_LONG(w, data[1]); \ 77 PRINT(", "); \ 78 PRINT_LONG(w, data[2]); \ 79 PRINT(", "); 80 #define PRINT_4ELM_ARRAY_LONG(w, data) \ 81 PRINT_3ELM_ARRAY_LONG(w, data); \ 82 PRINT_LONG(w, data[3]); \ 83 PRINT(", "); 84 85 #define PRINT_3ELM_ARRAY_INT(w, data) \ 86 PRINT_INT(w, data[0]); \ 87 PRINT(", "); \ 88 PRINT_INT(w, data[1]); \ 89 PRINT(", "); \ 90 PRINT_INT(w, data[2]); \ 91 PRINT(", "); 92 #define PRINT_4ELM_ARRAY_INT(w, data) \ 93 PRINT_3ELM_ARRAY_LONG(w, data); \ 94 PRINT_INT(w, data[3]); \ 95 PRINT(", "); 96 97 98 #define CASE_NAME(CODE) \ 99 case CODE: \ 100 return #CODE 101 102 #define CALL_CHECK_N_PRINT(f) { \ 103 MPL_LOGI("\n"); \ 104 MPL_LOGI("################################################\n"); \ 105 MPL_LOGI("# %s\n", #f); \ 106 MPL_LOGI("################################################\n"); \ 107 MPL_LOGI("\n"); \ 108 CALL_N_CHECK(f); \ 109 } 110 111 /* 112 Types 113 */ 114 /* A badly named enum type to track state of user input for tracker menu. */ 115 typedef enum { 116 STATE_SELECT_A_TRACKER, 117 STATE_SET_TRACKER_STATE, /* I'm running out of ideas here. */ 118 STATE_COUNT 119 } user_state_t; 120 121 /* bias trackers. */ 122 typedef enum { 123 BIAS_FROM_NO_MOTION, 124 FAST_NO_MOTION, 125 BIAS_FROM_GRAVITY, 126 BIAS_FROM_TEMPERATURE, 127 BIAS_FROM_LPF, 128 DEAD_ZONE, 129 NUM_TRACKERS 130 } bias_t; 131 132 enum comp_ids { 133 TIME = 0, 134 CALIBRATED_GYROSCOPE, 135 CALIBRATED_ACCELEROMETER, 136 CALIBRATED_COMPASS, 137 RAW_GYROSCOPE, 138 RAW_GYROSCOPE_BODY, 139 RAW_ACCELEROMETER, 140 RAW_COMPASS, 141 QUATERNION_9_AXIS, 142 QUATERNION_6_AXIS, 143 GRAVITY, 144 HEADING, 145 COMPASS_BIAS_ERROR, 146 COMPASS_STATE, 147 TEMPERATURE, 148 TEMP_COMP_SLOPE, 149 LINEAR_ACCELERATION, 150 ROTATION_VECTOR, 151 MOTION_STATE, 152 153 NUM_OF_IDS 154 }; 155 156 struct component_list { 157 char name[COMPONENT_NAME_MAX_LEN]; 158 int order; 159 }; 160 161 /* 162 Globals 163 */ 164 static int print_on_screen = true; 165 static int one_time_print = true; 166 static FILE *stream_file = NULL; 167 static unsigned long sample_count = 0; 168 static int enabled_9x = true; 169 170 signed char g_gyro_orientation[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; 171 signed char g_accel_orientation[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; 172 signed char g_compass_orientation[9] = {-1, 0, 0, 0, 1, 0, 0, 0, -1}; 173 174 #ifdef WIN32 175 static double pc_freq; 176 static __int64 counter_start; 177 #else 178 static inv_time_t counter_start; 179 #endif 180 181 struct component_list components[NUM_OF_IDS]; 182 183 /* 184 Prototypes 185 */ 186 void print_tracker_states(bias_t tracker); 187 188 /* 189 Callbacks 190 */ 191 /*--- motion / no motion callback function ---*/ 192 void check_motion_event(void) 193 { 194 long msg = inv_get_message_level_0(1); 195 if (msg) { 196 if (msg & INV_MSG_MOTION_EVENT) { 197 MPL_LOGI("################################################\n"); 198 MPL_LOGI("## Motion\n"); 199 MPL_LOGI("################################################\n"); 200 } 201 if (msg & INV_MSG_NO_MOTION_EVENT) { 202 MPL_LOGI("################################################\n"); 203 MPL_LOGI("## No Motion\n"); 204 MPL_LOGI("################################################\n"); 205 } 206 } 207 } 208 209 /* number to string coversion */ 210 char *compass_state_name(char* out, int state) 211 { 212 switch(state) { 213 CASE_NAME(SF_NORMAL); 214 CASE_NAME(SF_DISTURBANCE); 215 CASE_NAME(SF_FAST_SETTLE); 216 CASE_NAME(SF_SLOW_SETTLE); 217 CASE_NAME(SF_STARTUP_SETTLE); 218 CASE_NAME(SF_UNCALIBRATED); 219 } 220 221 #define UNKNOWN_ERROR_CODE 1234 222 return ERROR_NAME(UNKNOWN_ERROR_CODE); 223 } 224 225 /* component ID to name convertion */ 226 char *component_name(char *out, int comp_id) 227 { 228 switch (comp_id) { 229 CASE_NAME(TIME); 230 CASE_NAME(CALIBRATED_GYROSCOPE); 231 CASE_NAME(CALIBRATED_ACCELEROMETER); 232 CASE_NAME(CALIBRATED_COMPASS); 233 CASE_NAME(RAW_GYROSCOPE); 234 CASE_NAME(RAW_GYROSCOPE_BODY); 235 CASE_NAME(RAW_ACCELEROMETER); 236 CASE_NAME(RAW_COMPASS); 237 CASE_NAME(QUATERNION_9_AXIS); 238 CASE_NAME(QUATERNION_6_AXIS); 239 CASE_NAME(GRAVITY); 240 CASE_NAME(HEADING); 241 CASE_NAME(COMPASS_BIAS_ERROR); 242 CASE_NAME(COMPASS_STATE); 243 CASE_NAME(TEMPERATURE); 244 CASE_NAME(TEMP_COMP_SLOPE); 245 CASE_NAME(LINEAR_ACCELERATION); 246 CASE_NAME(ROTATION_VECTOR); 247 CASE_NAME(MOTION_STATE); 248 } 249 250 return "UNKNOWN"; 251 } 252 253 254 #ifdef WIN32 255 256 /* 257 Karthik Implementation. 258 http://stackoverflow.com/questions/1739259/how-to-use-queryperformancecounter 259 */ 260 double get_counter(__int64 *counter_start, double *pc_freq) 261 { 262 LARGE_INTEGER li; 263 double x; 264 QueryPerformanceCounter(&li); 265 x = (double) (li.QuadPart - (*counter_start)); 266 x = x / (*pc_freq); 267 return(x); 268 } 269 270 void start_counter(double *pc_freq, __int64 *counter_start) 271 { 272 LARGE_INTEGER li; 273 double x; 274 if(!QueryPerformanceFrequency(&li)) 275 printf("QueryPerformanceFrequency failed!\n"); 276 x = (double)(li.QuadPart); 277 *pc_freq = x / 1000.0; 278 QueryPerformanceCounter(&li); 279 *counter_start = li.QuadPart; 280 } 281 282 #else 283 284 unsigned long get_counter(void) 285 { 286 return (inv_get_tick_count() - counter_start); 287 } 288 289 void start_counter(void) 290 { 291 counter_start = inv_get_tick_count(); 292 } 293 294 #endif 295 296 /* processed data callback */ 297 void fifo_callback(void) 298 { 299 int print_on_screen_saved = print_on_screen; 300 int i; 301 302 /* one_time_print causes the data labels to be printed on screen */ 303 if (one_time_print) { 304 print_on_screen = true; 305 } 306 for (i = 0; i < NUM_OF_IDS; i++) { 307 if (components[TIME].order == i) { 308 if (one_time_print) { 309 PRINT("TIME,"); 310 } else { 311 #ifdef WIN32 312 double time_ms; 313 static int first_value = 0; 314 if(first_value == 0){ 315 first_value = 1; 316 start_counter(&pc_freq, &counter_start); 317 time_ms = 0; 318 } else { 319 time_ms = get_counter(&counter_start, &pc_freq); 320 } 321 PRINT("%6.0f, ", time_ms); 322 #else 323 unsigned long time_ms; 324 static int first_value = 0; 325 if(first_value == 0){ 326 first_value = 1; 327 start_counter(); 328 time_ms = 0; 329 } else { 330 time_ms = get_counter(); 331 } 332 PRINT("%6ld, ", time_ms); 333 #endif 334 } 335 } else if (components[CALIBRATED_GYROSCOPE].order == i) { 336 if (one_time_print) { 337 PRINT("CALIBRATED_GYROSCOPE_X," 338 "CALIBRATED_GYROSCOPE_Y," 339 "CALIBRATED_GYROSCOPE_Z,"); 340 /* 341 PRINT("CALIBRATED_GYROSCOPE_X_AVERAGE," 342 "CALIBRATED_GYROSCOPE_Y_AVERAGE," 343 "CALIBRATED_GYROSCOPE_Z_AVERAGE,"); 344 */ 345 } else { 346 /* 347 #define window 20 348 static int cnt = 0; 349 static int valid = 0; 350 static float gyro_keep[window][3]; 351 int kk, jj; 352 float avg[3]; 353 */ 354 float gyro[3]; 355 inv_get_gyro_float(gyro); 356 PRINT_3ELM_ARRAY_FLOAT(10, 5, gyro); 357 PRINT(" "); 358 /* 359 memcpy(gyro_keep[cnt], gyro, sizeof(float) * 3); 360 cnt= (cnt + 1) % window; 361 if (cnt == window - 1) 362 valid = 1; 363 if (valid) { 364 memset(avg, 0, sizeof(float) * 3); 365 jj = (cnt + 1) % window; 366 for (kk = 0; kk < window; kk++) { 367 avg[0] += gyro_keep[jj][0] / window; 368 avg[1] += gyro_keep[jj][1] / window; 369 avg[2] += gyro_keep[jj][2] / window; 370 jj = (jj + 1) % window; 371 } 372 PRINT("%+f, %+f, %+f, ", 373 UNPACK_3ELM_ARRAY(avg)); 374 PRINT_3ELM_ARRAY_FLOAT(10, 5, avg); 375 PRINT(" "); 376 } 377 */ 378 } 379 } else if (components[CALIBRATED_ACCELEROMETER].order == i) { 380 if (one_time_print) { 381 PRINT("CALIBRATED_ACCELEROMETER_X," 382 "CALIBRATED_ACCELEROMETER_Y," 383 "CALIBRATED_ACCELEROMETER_Z,"); 384 } else { 385 float accel[3]; 386 inv_get_accel_float(accel); 387 PRINT_3ELM_ARRAY_FLOAT(10, 5, accel); 388 PRINT(" "); 389 } 390 } else if (components[CALIBRATED_COMPASS].order == i) { 391 if (one_time_print) { 392 PRINT("CALIBRATED_COMPASS_X," 393 "CALIBRATED_COMPASS_Y," 394 "CALIBRATED_COMPASS_Z,"); 395 } else { 396 long lcompass[3]; 397 float compass[3]; 398 inv_get_compass_set(lcompass, 0, 0); 399 compass[0] = inv_q16_to_float(lcompass[0]); 400 compass[1] = inv_q16_to_float(lcompass[1]); 401 compass[2] = inv_q16_to_float(lcompass[2]); 402 PRINT_3ELM_ARRAY_FLOAT(10, 5, compass); 403 PRINT(" "); 404 } 405 } else if (components[RAW_GYROSCOPE].order == i) { 406 if (one_time_print) { 407 PRINT("RAW_GYROSCOPE_X," 408 "RAW_GYROSCOPE_Y," 409 "RAW_GYROSCOPE_Z,"); 410 } else { 411 short raw[3]; 412 inv_get_sensor_type_gyro_raw_short(raw, NULL); 413 PRINT_3ELM_ARRAY_INT(6, raw); 414 PRINT(" "); 415 } 416 } else if (components[RAW_GYROSCOPE_BODY].order == i) { 417 if (one_time_print) { 418 PRINT("RAW_GYROSCOPE_BODY_X," 419 "RAW_GYROSCOPE_BODY_Y," 420 "RAW_GYROSCOPE_BODY_Z,"); 421 } else { 422 float raw_body[3]; 423 inv_get_sensor_type_gyro_raw_body_float(raw_body, NULL); 424 PRINT_3ELM_ARRAY_FLOAT(10, 5, raw_body); 425 PRINT(" "); 426 } 427 } else if (components[RAW_ACCELEROMETER].order == i) { 428 if (one_time_print) { 429 PRINT("RAW_ACCELEROMETER_X," 430 "RAW_ACCELEROMETER_Y," 431 "RAW_ACCELEROMETER_Z,"); 432 } else { 433 short raw[3]; 434 inv_get_sensor_type_accel_raw_short(raw, NULL); 435 PRINT_3ELM_ARRAY_INT(6, raw); 436 PRINT(" "); 437 } 438 } else if (components[RAW_COMPASS].order == i) { 439 if (one_time_print) { 440 PRINT("RAW_COMPASS_X," 441 "RAW_COMPASS_Y," 442 "RAW_COMPASS_Z,"); 443 } else { 444 short raw[3]; 445 inv_get_sensor_type_compass_raw_short(raw, NULL); 446 PRINT_3ELM_ARRAY_INT(6, raw); 447 PRINT(" "); 448 } 449 } else if (components[QUATERNION_9_AXIS].order == i) { 450 if (one_time_print) { 451 PRINT("QUATERNION_9_AXIS_X," 452 "QUATERNION_9_AXIS_Y," 453 "QUATERNION_9_AXIS_Z," 454 "QUATERNION_9_AXIS_w,"); 455 } else { 456 float quat[4]; 457 inv_get_quaternion_float(quat); 458 PRINT_4ELM_ARRAY_FLOAT(10, 5, quat); 459 PRINT(" "); 460 } 461 } else if (components[QUATERNION_6_AXIS].order == i) { 462 if (one_time_print) { 463 PRINT("QUATERNION_6_AXIS_X," 464 "QUATERNION_6_AXIS_Y," 465 "QUATERNION_6_AXIS_Z," 466 "QUATERNION_6_AXIS_w,"); 467 } else { 468 float quat[4]; 469 long temp[4]; 470 int j; 471 inv_time_t timestamp; 472 inv_get_6axis_quaternion(temp, ×tamp); 473 for (j = 0; j < 4; j++) 474 quat[j] = (float)temp[j] / (1 << 30); 475 PRINT_4ELM_ARRAY_FLOAT(10, 5, quat); 476 PRINT(" "); 477 } 478 } else if (components[HEADING].order == i) { 479 if (one_time_print) { 480 PRINT("HEADING,"); 481 } else { 482 float heading[1]; 483 inv_get_sensor_type_compass_float(heading, NULL, NULL, 484 NULL, NULL); 485 PRINT_FLOAT(10, 5, heading[0]); 486 PRINT(", "); 487 } 488 } else if (components[GRAVITY].order == i) { 489 if (one_time_print) { 490 PRINT("GRAVITY_X," 491 "GRAVITY_Y," 492 "GRAVITY_Z,"); 493 } else { 494 float gravity[3]; 495 inv_get_sensor_type_gravity_float(gravity, NULL, NULL); 496 PRINT_3ELM_ARRAY_FLOAT(10, 5, gravity); 497 PRINT(" "); 498 } 499 } else if (components[COMPASS_STATE].order == i) { 500 if (one_time_print) { 501 PRINT("COMPASS_STATE," 502 "GOT_COARSE_HEADING,"); 503 } else { 504 PRINT_INT(1, inv_get_compass_state()); 505 PRINT(", "); 506 PRINT_INT(1, inv_got_compass_bias()); 507 PRINT(", "); 508 } 509 } else if (components[COMPASS_BIAS_ERROR].order == i) { 510 if (one_time_print) { 511 PRINT("COMPASS_BIAS_ERROR_X," 512 "COMPASS_BIAS_ERROR_Y," 513 "COMPASS_BIAS_ERROR_Z,"); 514 } else { 515 long mbe[3]; 516 inv_get_compass_bias_error(mbe); 517 PRINT_3ELM_ARRAY_LONG(6, mbe); 518 } 519 } else if (components[TEMPERATURE].order == i) { 520 if (one_time_print) { 521 PRINT("TEMPERATURE,"); 522 } else { 523 float temp; 524 inv_get_sensor_type_temperature_float(&temp, NULL); 525 PRINT_FLOAT(8, 4, temp); 526 PRINT(", "); 527 } 528 } else if (components[TEMP_COMP_SLOPE].order == i) { 529 if (one_time_print) { 530 PRINT("TEMP_COMP_SLOPE_X," 531 "TEMP_COMP_SLOPE_Y," 532 "TEMP_COMP_SLOPE_Z,"); 533 } else { 534 long temp_slope[3]; 535 float temp_slope_f[3]; 536 (void)inv_get_gyro_ts(temp_slope); 537 temp_slope_f[0] = inv_q16_to_float(temp_slope[0]); 538 temp_slope_f[1] = inv_q16_to_float(temp_slope[1]); 539 temp_slope_f[2] = inv_q16_to_float(temp_slope[2]); 540 PRINT_3ELM_ARRAY_FLOAT(10, 5, temp_slope_f); 541 PRINT(" "); 542 } 543 } else if (components[LINEAR_ACCELERATION].order == i) { 544 if (one_time_print) { 545 PRINT("LINEAR_ACCELERATION_X," 546 "LINEAR_ACCELERATION_Y," 547 "LINEAR_ACCELERATION_Z,"); 548 } else { 549 float lin_acc[3]; 550 inv_get_linear_accel_float(lin_acc); 551 PRINT_3ELM_ARRAY_FLOAT(10, 5, lin_acc); 552 PRINT(" "); 553 } 554 } else if (components[ROTATION_VECTOR].order == i) { 555 if (one_time_print) { 556 PRINT("ROTATION_VECTOR_X," 557 "ROTATION_VECTOR_Y," 558 "ROTATION_VECTOR_Z,"); 559 } else { 560 float rot_vec[3]; 561 inv_get_sensor_type_rotation_vector_float(rot_vec, NULL, NULL); 562 PRINT_3ELM_ARRAY_FLOAT(10, 5, rot_vec); 563 PRINT(" "); 564 } 565 } else if (components[MOTION_STATE].order == i) { 566 if (one_time_print) { 567 PRINT("MOTION_STATE,"); 568 } else { 569 unsigned int counter; 570 PRINT_INT(1, inv_get_motion_state(&counter)); 571 PRINT(", "); 572 } 573 } else { 574 ; 575 } 576 } 577 PRINT("\n"); 578 579 if (one_time_print) { 580 one_time_print = false; 581 print_on_screen = print_on_screen_saved; 582 } 583 sample_count++; 584 } 585 586 void print_help(char *exename) 587 { 588 printf( 589 "\n" 590 "Usage:\n" 591 "\t%s [options]\n" 592 "\n" 593 "Options:\n" 594 " [-h|--help] = shows this help\n" 595 " [-o|--output PREFIX] = to dump data on csv file whose file\n" 596 " prefix is specified by the parameter,\n" 597 " e.g. '<PREFIX>-<timestamp>.csv'\n" 598 " [-i|--input NAME] = to read the provided playback.bin file\n" 599 " [-c|--comp C] = enable the following components in the\n" 600 " given order:\n" 601 " t = TIME\n" 602 " T = TEMPERATURE,\n" 603 " s = TEMP_COMP_SLOPE,\n" 604 " G = CALIBRATED_GYROSCOPE,\n" 605 " A = CALIBRATED_ACCELEROMETER,\n" 606 " C = CALIBRATED_COMPASS,\n" 607 " g = RAW_GYROSCOPE,\n" 608 " b = RAW_GYROSCOPE_BODY,\n" 609 " a = RAW_ACCELEROMETER,\n" 610 " c = RAW_COMPASS,\n" 611 " Q = QUATERNION_9_AXIS,\n" 612 " 6 = QUATERNION_6_AXIS,\n" 613 " V = GRAVITY,\n" 614 " L = LINEAR_ACCELERATION,\n" 615 " R = ROTATION_VECTOR,\n" 616 " H = HEADING,\n" 617 " E = COMPASS_BIAS_ERROR,\n" 618 " S = COMPASS_STATE,\n" 619 " M = MOTION_STATE.\n" 620 "\n" 621 "Note on compass state values:\n" 622 " SF_NORMAL = 0\n" 623 " SF_DISTURBANCE = 1\n" 624 " SF_FAST_SETTLE = 2\n" 625 " SF_SLOW_SETTLE = 3\n" 626 " SF_STARTUP_SETTLE = 4\n" 627 " SF_UNCALIBRATED = 5\n" 628 "\n", 629 exename); 630 } 631 632 char *output_filename_datetimestamp(char *out) 633 { 634 time_t t; 635 struct tm *now; 636 t = time(NULL); 637 now = localtime(&t); 638 639 sprintf(out, 640 "%02d%02d%02d_%02d%02d%02d.csv", 641 now->tm_year - 100, now->tm_mon + 1, now->tm_mday, 642 now->tm_hour, now->tm_min, now->tm_sec); 643 return out; 644 } 645 646 int components_parser(char pname[], char requested[], int length) 647 { 648 int j; 649 650 /* forcibly disable all */ 651 for (j = 0; j < NUM_OF_IDS; j++) 652 components[j].order = -1; 653 654 /* parse each character one a time */ 655 for (j = 0; j < length; j++) { 656 switch (requested[j]) { 657 case 'T': 658 components[TEMPERATURE].order = j; 659 break; 660 case 'G': 661 components[CALIBRATED_GYROSCOPE].order = j; 662 break; 663 case 'A': 664 components[CALIBRATED_ACCELEROMETER].order = j; 665 break; 666 case 'g': 667 components[RAW_GYROSCOPE].order = j; 668 break; 669 case 'b': 670 components[RAW_GYROSCOPE_BODY].order = j; 671 break; 672 case 'a': 673 components[RAW_ACCELEROMETER].order = j; 674 break; 675 case 'Q': 676 components[QUATERNION_9_AXIS].order = j; 677 break; 678 case '6': 679 components[QUATERNION_6_AXIS].order = j; 680 break; 681 case 'V': 682 components[GRAVITY].order = j; 683 break; 684 case 'L': 685 components[LINEAR_ACCELERATION].order = j; 686 break; 687 case 'R': 688 components[ROTATION_VECTOR].order = j; 689 break; 690 691 /* these components don't need to be enabled */ 692 case 't': 693 components[TIME].order = j; 694 break; 695 case 'C': 696 components[CALIBRATED_COMPASS].order = j; 697 break; 698 case 'c': 699 components[RAW_COMPASS].order = j; 700 break; 701 case 'H': 702 components[HEADING].order = j; 703 break; 704 case 'E': 705 components[COMPASS_BIAS_ERROR].order = j; 706 break; 707 case 'S': 708 components[COMPASS_STATE].order = j; 709 break; 710 case 'M': 711 components[MOTION_STATE].order = j; 712 break; 713 714 default: 715 MPL_LOGI("Error : unrecognized component '%c'\n", 716 requested[j]); 717 print_help(pname); 718 return 1; 719 break; 720 } 721 } 722 return 0; 723 } 724 725 int main(int argc, char *argv[]) 726 { 727 #ifndef INV_PLAYBACK_DBG 728 MPL_LOGI("Error : this application was not compiled with the " 729 "INV_PLAYBACK_DBG define and cannot work.\n"); 730 MPL_LOGI(" Recompile the libmllite libraries with #define " 731 "INV_PLAYBACK_DBG uncommented\n"); 732 MPL_LOGI(" in 'software/core/mllite/data_builder.h'.\n"); 733 return INV_ERROR; 734 #endif 735 736 inv_time_t start_time; 737 double total_time; 738 char req_component_list[50] = "tQGACH"; 739 char input_filename[101] = "/data/playback.bin"; 740 int i = 0; 741 char *ver_str; 742 /* flags */ 743 int use_nm_detection = true; 744 745 /* make sure there is no buffering of the print messages */ 746 setvbuf(stdout, NULL, _IONBF, 0); 747 748 /* forcibly disable all and populate the names */ 749 for (i = 0; i < NUM_OF_IDS; i++) { 750 char str_tmp[COMPONENT_NAME_MAX_LEN]; 751 strcpy(components[i].name, component_name(str_tmp, i)); 752 components[i].order = -1; 753 } 754 755 CALL_N_CHECK( inv_get_version(&ver_str) ); 756 MPL_LOGI("\n"); 757 MPL_LOGI("%s\n", ver_str); 758 MPL_LOGI("\n"); 759 760 for (i = 1; i < argc; i++) { 761 if(strcmp(argv[i], "-h") == 0 762 || strcmp(argv[i], "--help") == 0) { 763 print_help(argv[0]); 764 return INV_SUCCESS; 765 766 } else if(strcmp(argv[i], "-o") == 0 767 || strcmp(argv[i], "--output") == 0) { 768 char output_filename[200]; 769 char end[50] = ""; 770 i++; 771 772 snprintf(output_filename, sizeof(output_filename), "%s-%s", 773 argv[i], output_filename_datetimestamp(end)); 774 stream_file = fopen(output_filename, "w+"); 775 if (!stream_file) { 776 printf("Unable to open file '%s'\n", output_filename); 777 return INV_ERROR; 778 } 779 MPL_LOGI("-- Output on file '%s'\n", output_filename); 780 781 } else if(strcmp(argv[i], "-i") == 0 782 || strcmp(argv[i], "--input") == 0) { 783 i++; 784 strncpy(input_filename, argv[i], sizeof(input_filename)); 785 MPL_LOGI("-- Playing back file '%s'\n", input_filename); 786 787 } else if(strcmp(argv[i], "-n") == 0 788 || strcmp(argv[i], "--nm") == 0) { 789 i++; 790 if (!strcmp(argv[i], "none")) { 791 use_nm_detection = 0; 792 } else if (!strcmp(argv[i], "regular")) { 793 use_nm_detection = 1; 794 } else if (!strcmp(argv[i], "fast")) { 795 use_nm_detection = 2; 796 } else { 797 MPL_LOGI("Error : unrecognized no-motion type '%s'\n", 798 argv[i]); 799 return INV_ERROR_INVALID_PARAMETER; 800 } 801 MPL_LOGI("-- No motion algorithm : '%s', %d\n", 802 argv[i], use_nm_detection); 803 804 } else if(strcmp(argv[i], "-9") == 0 805 || strcmp(argv[i], "--nine") == 0) { 806 MPL_LOGI("-- using 9 axis sensor fusion by default\n"); 807 enabled_9x = true; 808 809 } else if(strcmp(argv[i], "-c") == 0 810 || strcmp(argv[i], "--comp") == 0) { 811 i++; 812 strcpy(req_component_list, argv[i]); 813 814 } else { 815 MPL_LOGI("Unrecognized command-line parameter '%s'\n", argv[i]); 816 return INV_ERROR_INVALID_PARAMETER; 817 } 818 } 819 820 CALL_CHECK_N_RETURN_ERROR( 821 components_parser( 822 argv[0], 823 req_component_list, strlen(req_component_list))); 824 825 /* set up callbacks */ 826 CALL_N_CHECK(inv_set_fifo_processed_callback(fifo_callback)); 827 828 /* algorithm init */ 829 CALL_N_CHECK(inv_enable_quaternion()); 830 if (use_nm_detection == 1) { 831 CALL_N_CHECK(inv_enable_motion_no_motion()); 832 } else if (use_nm_detection == 2) { 833 CALL_N_CHECK(inv_enable_fast_nomot()); 834 } 835 CALL_N_CHECK(inv_enable_gyro_tc()); 836 CALL_N_CHECK(inv_enable_in_use_auto_calibration()); 837 CALL_N_CHECK(inv_enable_no_gyro_fusion()); 838 CALL_N_CHECK(inv_enable_results_holder()); 839 if (enabled_9x) { 840 CALL_N_CHECK(inv_enable_heading_from_gyro()); 841 CALL_N_CHECK(inv_enable_compass_bias_w_gyro()); 842 CALL_N_CHECK(inv_enable_vector_compass_cal()); 843 CALL_N_CHECK(inv_enable_9x_sensor_fusion()); 844 } 845 846 CALL_CHECK_N_RETURN_ERROR(inv_enable_datalogger_outputs()); 847 CALL_CHECK_N_RETURN_ERROR(inv_constructor_start()); 848 849 /* load persistent data */ 850 { 851 FILE *fc = fopen("invcal.bin", "rb"); 852 if (fc != NULL) { 853 size_t sz, rd; 854 inv_error_t result = 0; 855 // Read amount of memory we need to hold data 856 rd = fread(&sz, sizeof(size_t), 1, fc); 857 if (rd == sizeof(size_t)) { 858 unsigned char *cal_data = (unsigned char *)malloc(sizeof(sz)); 859 unsigned char *cal_ptr; 860 cal_ptr = cal_data; 861 *(size_t *)cal_ptr = sz; 862 cal_ptr += sizeof(sz); 863 /* read rest of the file */ 864 fread(cal_ptr, sz - sizeof(size_t), 1, fc); 865 //result = inv_load_mpl_states(cal_data, sz); 866 if (result) { 867 MPL_LOGE("Cal Load error\n"); 868 } 869 MPL_LOGI("inv_load_mpl_states()\n"); 870 free(cal_data); 871 } else { 872 MPL_LOGE("Cal Load error with size read\n"); 873 } 874 fclose(fc); 875 } 876 } 877 878 sample_count = 0; 879 start_time = inv_get_tick_count(); 880 881 /* playback data that was recorded */ 882 inv_set_playback_filename(input_filename, strlen(input_filename) + 1); 883 inv_set_debug_mode(RD_PLAYBACK); 884 CALL_N_CHECK(inv_playback()); 885 886 total_time = (1.0 * inv_get_tick_count() - start_time) / 1000; 887 if (total_time > 0) { 888 MPL_LOGI("\nPlayed back %ld samples in %.2f s (%.1f Hz)\n", 889 sample_count, total_time , 1.0 * sample_count / total_time); 890 } 891 892 if (stream_file) 893 fclose(stream_file); 894 895 return INV_SUCCESS; 896 } 897 898 899
