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