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. */
115typedef 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. */
122typedef 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
132enum 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
156struct component_list {
157    char name[COMPONENT_NAME_MAX_LEN];
158    int order;
159};
160
161/*
162    Globals
163*/
164static int print_on_screen = true;
165static int one_time_print = true;
166static FILE *stream_file = NULL;
167static unsigned long sample_count = 0;
168static int enabled_9x = true;
169
170signed char g_gyro_orientation[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
171signed char g_accel_orientation[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
172signed char g_compass_orientation[9] = {-1, 0, 0, 0, 1, 0, 0, 0, -1};
173
174#ifdef WIN32
175static double pc_freq;
176static __int64 counter_start;
177#else
178static inv_time_t counter_start;
179#endif
180
181struct component_list components[NUM_OF_IDS];
182
183/*
184    Prototypes
185*/
186void print_tracker_states(bias_t tracker);
187
188/*
189    Callbacks
190*/
191/*--- motion / no motion callback function ---*/
192void 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 */
210char *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 */
226char *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*/
260double 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
270void 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
284unsigned long get_counter(void)
285{
286    return (inv_get_tick_count() - counter_start);
287}
288
289void start_counter(void)
290{
291    counter_start = inv_get_tick_count();
292}
293
294#endif
295
296/* processed data callback */
297void 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, &timestamp);
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
586void 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
632char *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
646int 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
725int 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