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 ---*/
check_motion_event(void)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 */
compass_state_name(char * out,int state)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 */
component_name(char * out,int comp_id)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 */
get_counter(__int64 * counter_start,double * pc_freq)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
start_counter(double * pc_freq,__int64 * counter_start)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
get_counter(void)284 unsigned long get_counter(void)
285 {
286 return (inv_get_tick_count() - counter_start);
287 }
288
start_counter(void)289 void start_counter(void)
290 {
291 counter_start = inv_get_tick_count();
292 }
293
294 #endif
295
296 /* processed data callback */
fifo_callback(void)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
print_help(char * exename)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
output_filename_datetimestamp(char * out)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
components_parser(char pname[],char requested[],int length)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
main(int argc,char * argv[])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