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*/
32struct 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*/
50static struct inv_construct_t inv_construct = {0};
51static void (*s_func_cb)(void);
52static char playback_filename[101] = "/data/playback.bin";
53struct fifo_dmp_config fifo_dmp_cfg = {0};
54
55/*
56    Functions
57*/
58void 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
67inv_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
96inv_error_t inv_set_fifo_processed_callback(void (*func_cb)(void))
97{
98    s_func_cb = func_cb;
99    return INV_SUCCESS;
100}
101
102void 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
109inv_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*/
285void inv_set_debug_mode(rd_dbg_mode mode)
286{
287#ifdef INV_PLAYBACK_DBG
288    inv_construct.debug_mode = mode;
289#endif
290}
291
292inv_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
361inv_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