1 /**
2 * Self Test application for Invensense's MPU6xxx/MPU9xxx.
3 */
4
5 #include <unistd.h>
6 #include <dirent.h>
7 #include <fcntl.h>
8 #include <stdio.h>
9 #include <errno.h>
10 #include <sys/stat.h>
11 #include <stdlib.h>
12 #include <features.h>
13 #include <dirent.h>
14 #include <string.h>
15 #include <poll.h>
16 #include <stddef.h>
17 #include <linux/input.h>
18 #include <time.h>
19 #include <linux/time.h>
20
21 #include "invensense.h"
22 #include "ml_math_func.h"
23 #include "storage_manager.h"
24 #include "ml_stored_data.h"
25 #include "ml_sysfs_helper.h"
26 #include "data_builder.h"
27
28 #ifndef ABS
29 #define ABS(x)(((x) >= 0) ? (x) : -(x))
30 #endif
31
32 #define DEBUG_PRINT /* Uncomment to print Gyro & Accel read from Driver */
33
34 #define MAX_SYSFS_NAME_LEN (100)
35 #define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*))
36 #define IIO_SYSFS_PATH "/sys/bus/iio/devices/iio:device0"
37 #define IIO_HUB_NAME "inv_hub"
38
39 #define GYRO_PASS_STATUS_BIT 0x01
40 #define ACCEL_PASS_STATUS_BIT 0x02
41 #define COMPASS_PASS_STATUS_BIT 0x04
42
43 typedef union {
44 long l;
45 int i;
46 } bias_dtype;
47
48 char *sysfs_names_ptr;
49 struct sysfs_attrbs {
50 char *name;
51 char *enable;
52 int enable_value;
53 char *power_state;
54 int power_state_value;
55 char *dmp_on;
56 int dmp_on_value;
57 char *self_test;
58 char *temperature;
59 char *gyro_enable;
60 int gyro_enable_value;
61 char *gyro_x_calibbias;
62 char *gyro_y_calibbias;
63 char *gyro_z_calibbias;
64 char *gyro_scale;
65 char *gyro_x_offset;
66 char *gyro_y_offset;
67 char *gyro_z_offset;
68 char *accel_enable;
69 int accel_enable_value;
70 char *accel_x_calibbias;
71 char *accel_y_calibbias;
72 char *accel_z_calibbias;
73 char *accel_scale;
74 char *accel_x_offset;
75 char *accel_y_offset;
76 char *accel_z_offset;
77 char *compass_enable;
78 int compass_enable_value;
79 } mpu;
80
81 static struct inv_db_save_t save_data;
82 static bool write_biases_immediately = false;
83
84 /** This function receives the data that was stored in non-volatile memory
85 between power off */
inv_db_load_func(const unsigned char * data)86 static inv_error_t inv_db_load_func(const unsigned char *data)
87 {
88 memcpy(&save_data, data, sizeof(save_data));
89 return INV_SUCCESS;
90 }
91
92 /** This function returns the data to be stored in non-volatile memory between
93 power off */
inv_db_save_func(unsigned char * data)94 static inv_error_t inv_db_save_func(unsigned char *data)
95 {
96 memcpy(data, &save_data, sizeof(save_data));
97 return INV_SUCCESS;
98 }
99
100 /** read a sysfs entry that represents an integer */
read_sysfs_int(char * filename,int * var)101 int read_sysfs_int(char *filename, int *var)
102 {
103 int res=0;
104 FILE *fp;
105
106 fp = fopen(filename, "r");
107 if (fp != NULL) {
108 fscanf(fp, "%d\n", var);
109 fclose(fp);
110 } else {
111 MPL_LOGE("inv_self_test: ERR open file to read");
112 res= -1;
113 }
114 return res;
115 }
116
117 /** write a sysfs entry that represents an integer */
write_sysfs_int(char * filename,int data)118 int write_sysfs_int(char *filename, int data)
119 {
120 int res = 0;
121 FILE *fp;
122
123 fp = fopen(filename, "w");
124 if (fp != NULL) {
125 fprintf(fp, "%d\n", data);
126 fclose(fp);
127 } else {
128 MPL_LOGE("inv_self_test: ERR open file to write");
129 res= -1;
130 }
131 return res;
132 }
133
android_hub(void)134 int android_hub(void)
135 {
136 char dev_name[8];
137 FILE *fp;
138
139 fp = fopen(mpu.name, "r");
140 fgets(dev_name, 8, fp);
141 fclose (fp);
142
143 if (!strncmp(dev_name, IIO_HUB_NAME, sizeof(IIO_HUB_NAME)))
144 return true;
145 else
146 return false;
147 }
148
save_n_write_sysfs_int(char * filename,int data,int * old_value)149 int save_n_write_sysfs_int(char *filename, int data, int *old_value)
150 {
151 int res;
152 res = read_sysfs_int(filename, old_value);
153 if (res < 0) {
154 return res;
155 }
156 //printf("saved %s = %d\n", filename, *old_value);
157 res = write_sysfs_int(filename, data);
158 if (res < 0) {
159 return res;
160 }
161 return 0;
162 }
163
inv_init_sysfs_attributes(void)164 int inv_init_sysfs_attributes(void)
165 {
166 unsigned char i = 0;
167 char sysfs_path[MAX_SYSFS_NAME_LEN];
168 char *sptr;
169 char **dptr;
170
171 sysfs_names_ptr =
172 (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
173 sptr = sysfs_names_ptr;
174 if (sptr != NULL) {
175 dptr = (char**)&mpu;
176 do {
177 *dptr++ = sptr;
178 sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
179 } while (++i < MAX_SYSFS_ATTRB);
180 } else {
181 MPL_LOGE("inv_self_test: couldn't alloc mem for sysfs paths");
182 return -1;
183 }
184
185 // Setup IIO sysfs path & build MPU's sysfs paths
186 sprintf(sysfs_path, "%s", IIO_SYSFS_PATH);
187
188 sprintf(mpu.name, "%s%s", sysfs_path, "/name");
189 sprintf(mpu.enable, "%s%s", sysfs_path, "/buffer/enable");
190 sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
191 sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
192 sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test");
193 sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature");
194
195 sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable");
196 sprintf(mpu.gyro_x_calibbias, "%s%s",
197 sysfs_path, "/in_anglvel_x_calibbias");
198 sprintf(mpu.gyro_y_calibbias, "%s%s",
199 sysfs_path, "/in_anglvel_y_calibbias");
200 sprintf(mpu.gyro_z_calibbias, "%s%s",
201 sysfs_path, "/in_anglvel_z_calibbias");
202 sprintf(mpu.gyro_scale, "%s%s", sysfs_path, "/in_anglvel_self_test_scale");
203 sprintf(mpu.gyro_x_offset, "%s%s", sysfs_path, "/in_anglvel_x_offset");
204 sprintf(mpu.gyro_y_offset, "%s%s", sysfs_path, "/in_anglvel_y_offset");
205 sprintf(mpu.gyro_z_offset, "%s%s", sysfs_path, "/in_anglvel_z_offset");
206
207 sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accel_enable");
208 sprintf(mpu.accel_x_calibbias, "%s%s", sysfs_path, "/in_accel_x_calibbias");
209 sprintf(mpu.accel_y_calibbias, "%s%s", sysfs_path, "/in_accel_y_calibbias");
210 sprintf(mpu.accel_z_calibbias, "%s%s", sysfs_path, "/in_accel_z_calibbias");
211 sprintf(mpu.accel_scale, "%s%s", sysfs_path, "/in_accel_self_test_scale");
212 sprintf(mpu.accel_x_offset, "%s%s", sysfs_path, "/in_accel_x_offset");
213 sprintf(mpu.accel_y_offset, "%s%s", sysfs_path, "/in_accel_y_offset");
214 sprintf(mpu.accel_z_offset, "%s%s", sysfs_path, "/in_accel_z_offset");
215
216 sprintf(mpu.compass_enable, "%s%s", sysfs_path, "/compass_enable");
217
218 #if 0
219 // test print sysfs paths
220 dptr = (char**)&mpu;
221 for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
222 printf("inv_self_test: sysfs path: %s\n", *dptr++);
223 }
224 #endif
225 return 0;
226 }
227
print_cal_file_content(struct inv_db_save_t * data)228 void print_cal_file_content(struct inv_db_save_t *data)
229 {
230 printf("------------------------------\n");
231 printf("-- Calibration file content --\n");
232 printf(" factory_gyro_bias[3] = %+ld %+ld %+ld\n",
233 data->factory_gyro_bias[0], data->factory_gyro_bias[1],
234 data->factory_gyro_bias[2]);
235 printf(" factory_accel_bias[3] = %+ld %+ld %+ld\n",
236 data->factory_accel_bias[0], data->factory_accel_bias[1],
237 data->factory_accel_bias[2]);
238 printf(" compass_bias[3] = %+ld %+ld %+ld\n",
239 data->compass_bias[0], data->compass_bias[1], data->compass_bias[2]);
240 printf(" gyro_temp = %+ld\n", data->gyro_temp);
241 printf(" gyro_bias_tc_set = %+d\n", data->gyro_bias_tc_set);
242 printf(" accel_temp = %+ld\n", data->accel_temp);
243 printf(" gyro_accuracy = %d\n", data->gyro_accuracy);
244 printf(" accel_accuracy = %d\n", data->accel_accuracy);
245 printf(" compass_accuracy = %d\n", data->compass_accuracy);
246 printf("------------------------------\n");
247 }
248
249 /*******************************************************************************
250 * M a i n
251 ******************************************************************************/
main(int argc,char ** argv)252 int main(int argc, char **argv)
253 {
254 FILE *fptr;
255 int self_test_status = 0;
256 inv_error_t result;
257 bias_dtype gyro_bias[3];
258 bias_dtype gyro_scale;
259 bias_dtype accel_bias[3];
260 bias_dtype accel_scale;
261 int scale_ratio;
262 size_t packet_sz;
263 int axis_sign = 1;
264 unsigned char *buffer;
265 long timestamp;
266 long long temperature = 0;
267 bool compass_present = true;
268 int c;
269
270 result= inv_init_sysfs_attributes();
271 if (result)
272 return -1;
273
274 // Self-test for Non-Hub
275 inv_init_storage_manager();
276
277 // Register packet to be saved.
278 result = inv_register_load_store(
279 inv_db_load_func, inv_db_save_func,
280 sizeof(save_data), INV_DB_SAVE_KEY);
281
282 // Self-test for Android Hub
283 if (android_hub() == true) {
284 fptr = fopen(mpu.self_test, "r");
285 if (fptr) {
286 fscanf(fptr, "%d", &self_test_status);
287 printf("\nSelf-Test:Hub:Self test result - "
288 "Gyro passed= %x, Accel passed= %x, Compass passed= %x\n",
289 (self_test_status & GYRO_PASS_STATUS_BIT),
290 (self_test_status & ACCEL_PASS_STATUS_BIT) >> 1,
291 (self_test_status & COMPASS_PASS_STATUS_BIT) >> 2);
292 fclose(fptr);
293 result = 0;
294 } else {
295 printf("Hub-Self-Test:ERR-Couldn't invoke self-test\n");
296 result = -1;
297 }
298
299 free(sysfs_names_ptr);
300 return result;
301 }
302
303 /* testing hook: if the command-line parameter is '-l' as in 'load',
304 the system will read out the MLCAL_FILE */
305 while ((c = getopt(argc, argv, "lw")) != -1) {
306 switch (c) {
307 case 'l':
308 inv_get_mpl_state_size(&packet_sz);
309
310 buffer = (unsigned char *)malloc(packet_sz + 10);
311 if (buffer == NULL) {
312 printf("Self-Test:Can't allocate buffer\n");
313 return -1;
314 }
315
316 fptr= fopen(MLCAL_FILE, "rb");
317 if (!fptr) {
318 printf("Self-Test:ERR- Can't open cal file to read - %s\n",
319 MLCAL_FILE);
320 result = -1;
321 }
322 fread(buffer, 1, packet_sz, fptr);
323 fclose(fptr);
324
325 result = inv_load_mpl_states(buffer, packet_sz);
326 if (result) {
327 printf("Self-Test:ERR - "
328 "Cannot load MPL states from cal file - error %d\n",
329 result);
330 free(buffer);
331 return -1;
332 }
333 free(buffer);
334
335 print_cal_file_content(&save_data);
336 return 0;
337 break;
338
339 case 'w':
340 write_biases_immediately = true;
341 break;
342
343 case '?':
344 return -1;
345 }
346 }
347
348 // Clear out data.
349 memset(&save_data, 0, sizeof(save_data));
350 memset(gyro_bias,0, sizeof(gyro_bias));
351 memset(accel_bias,0, sizeof(accel_bias));
352
353 // enable the device
354 if (save_n_write_sysfs_int(mpu.power_state, 1,
355 &mpu.power_state_value) < 0) {
356 printf("Self-Test:ERR-Failed to set power_state=1\n");
357 }
358 if (save_n_write_sysfs_int(mpu.enable, 0, &mpu.enable_value) < 0) {
359 printf("Self-Test:ERR-Failed to disable master enable\n");
360 }
361 if (save_n_write_sysfs_int(mpu.dmp_on, 0, &mpu.dmp_on_value) < 0) {
362 printf("Self-Test:ERR-Failed to disable DMP\n");
363 }
364 if (save_n_write_sysfs_int(mpu.accel_enable, 1,
365 &mpu.accel_enable_value) < 0) {
366 printf("Self-Test:ERR-Failed to enable accel\n");
367 }
368 if (save_n_write_sysfs_int(mpu.gyro_enable, 1,
369 &mpu.gyro_enable_value) < 0) {
370 printf("Self-Test:ERR-Failed to enable gyro\n");
371 }
372 if (save_n_write_sysfs_int(mpu.compass_enable, 1,
373 &mpu.compass_enable_value) < 0) {
374 #ifdef DEBUG_PRINT
375 printf("Self-Test:ERR-Failed to enable compass\n");
376 #endif
377 compass_present = false;
378 }
379
380 fptr = fopen(mpu.self_test, "r");
381 if (!fptr) {
382 printf("Self-Test:ERR-Couldn't invoke self-test\n");
383 result = -1;
384 goto free_sysfs_storage;
385 }
386
387 // Invoke self-test
388 fscanf(fptr, "%d", &self_test_status);
389 if (compass_present == true) {
390 printf("Self-Test:Self test result- "
391 "Gyro passed = %x, Accel passed = %x, Compass passed = %x\n",
392 (self_test_status & GYRO_PASS_STATUS_BIT),
393 (self_test_status & ACCEL_PASS_STATUS_BIT) >> 1,
394 (self_test_status & COMPASS_PASS_STATUS_BIT) >> 2);
395 } else {
396 printf("Self-Test:Self test result- "
397 "Gyro passed = %x, Accel passed = %x\n",
398 (self_test_status & GYRO_PASS_STATUS_BIT),
399 (self_test_status & ACCEL_PASS_STATUS_BIT) >> 1);
400 }
401 fclose(fptr);
402
403 if (self_test_status & GYRO_PASS_STATUS_BIT) {
404 // Read Gyro Bias
405 if (read_sysfs_int(mpu.gyro_x_calibbias, &gyro_bias[0].i) < 0 ||
406 read_sysfs_int(mpu.gyro_y_calibbias, &gyro_bias[1].i) < 0 ||
407 read_sysfs_int(mpu.gyro_z_calibbias, &gyro_bias[2].i) < 0 ||
408 read_sysfs_int(mpu.gyro_scale, &gyro_scale.i) < 0) {
409 memset(gyro_bias, 0, sizeof(gyro_bias));
410 gyro_scale.i = 0;
411 printf("Self-Test:Failed to read Gyro bias\n");
412 } else {
413 save_data.gyro_accuracy = 3;
414 #ifdef DEBUG_PRINT
415 printf("Self-Test:Gyro bias[0..2] = [%d %d %d]\n",
416 gyro_bias[0].i, gyro_bias[1].i, gyro_bias[2].i);
417 #endif
418 }
419 } else {
420 printf("Self-Test:Failed Gyro self-test\n");
421 }
422
423 if (self_test_status & ACCEL_PASS_STATUS_BIT) {
424 // Read Accel Bias
425 if (read_sysfs_int(mpu.accel_x_calibbias, &accel_bias[0].i) < 0 ||
426 read_sysfs_int(mpu.accel_y_calibbias, &accel_bias[1].i) < 0 ||
427 read_sysfs_int(mpu.accel_z_calibbias, &accel_bias[2].i) < 0 ||
428 read_sysfs_int(mpu.accel_scale, &accel_scale.i) < 0) {
429 memset(accel_bias, 0, sizeof(accel_bias));
430 accel_scale.i = 0;
431 printf("Self-Test:Failed to read Accel bias\n");
432 } else {
433 save_data.accel_accuracy = 3;
434 #ifdef DEBUG_PRINT
435 printf("Self-Test:Accel bias[0..2] = [%d %d %d]\n",
436 accel_bias[0].i, accel_bias[1].i, accel_bias[2].i);
437 #endif
438 }
439 } else {
440 printf("Self-Test:Failed Accel self-test\n");
441 }
442
443 if (!(self_test_status & (GYRO_PASS_STATUS_BIT | ACCEL_PASS_STATUS_BIT))) {
444 printf("Self-Test:Failed Gyro and Accel self-test, "
445 "nothing left to do\n");
446 result = -1;
447 goto restore_settings;
448 }
449
450 // Read temperature
451 fptr = fopen(mpu.temperature, "r");
452 if (fptr != NULL) {
453 fscanf(fptr,"%lld %ld", &temperature, ×tamp);
454 fclose(fptr);
455 } else {
456 printf("Self-Test:ERR-Couldn't read temperature\n");
457 }
458
459 /*
460 When we read gyro bias from sysfs, the bias is in the raw units scaled by
461 1000 at the full scale used during self-test
462 (in_anglvel_self_test_scale).
463 We store the biases in raw units (+/- 2000 dps full scale assumed)
464 scaled by 2^16 therefore the gyro_bias[] have to be divided by the
465 ratio of 2000 / gyro_scale to comply.
466 */
467 // TODO read this from the regular full scale in sysfs
468 scale_ratio = 2000 / gyro_scale.i;
469 save_data.factory_gyro_bias[0] =
470 (long)(gyro_bias[0].l / 1000.f * 65536.f / scale_ratio);
471 save_data.factory_gyro_bias[1] =
472 (long)(gyro_bias[1].l / 1000.f * 65536.f / scale_ratio);
473 save_data.factory_gyro_bias[2] =
474 (long)(gyro_bias[2].l / 1000.f * 65536.f / scale_ratio);
475
476 // Save temperature @ time stored.
477 // Temperature is in degrees Celsius scaled by 2^16
478 save_data.gyro_temp = temperature * (1L << 16);
479 save_data.gyro_bias_tc_set = true;
480 save_data.accel_temp = save_data.gyro_temp;
481
482 // When we read accel bias, the bias is in raw units scaled by 1000.
483 // and it contains the gravity vector.
484 // Find the orientation of the device, by looking for gravity
485 int axis = 0;
486 if (ABS(accel_bias[1].l) > ABS(accel_bias[0].l)) {
487 axis = 1;
488 }
489 if (ABS(accel_bias[2].l) > ABS(accel_bias[axis].l)) {
490 axis = 2;
491 }
492 if (accel_bias[axis].l < 0) {
493 axis_sign = -1;
494 }
495
496 // Convert scaling from raw units scaled by 1000 to raw scaled by 2^16
497 /*
498 When we read accel bias from sysfs, the bias is in the raw units scaled
499 by 1000 at the full scale used during self-test
500 (in_accel_self_test_scale).
501 We store the biases in raw units (+/- 2 gee full scale assumed)
502 scaled by 2^16 therefore the accel_bias[] have to be multipled by the
503 ratio of accel_scale / 2 to comply.
504 */
505 // TODO read this from the regular full scale in sysfs
506 scale_ratio = accel_scale.i / 2;
507 save_data.factory_accel_bias[0] =
508 (long)(accel_bias[0].l / 1000.f * 65536.f * scale_ratio);
509 save_data.factory_accel_bias[1] =
510 (long)(accel_bias[1].l / 1000.f * 65536.f * scale_ratio);
511 save_data.factory_accel_bias[2] =
512 (long)(accel_bias[2].l / 1000.f * 65536.f * scale_ratio);
513
514 #ifdef DEBUG_PRINT
515 printf("Self-Test:Saved Accel bias[0..2] = [%ld %ld %ld]\n",
516 save_data.factory_accel_bias[0], save_data.factory_accel_bias[1],
517 save_data.factory_accel_bias[2]);
518 #endif
519
520 /*
521 Remove gravity, gravity in raw units should be 16384 = 2^14 for a +/-
522 2 gee setting. The data has been saved in Hw units scaled to 2^16,
523 so gravity needs to scale up accordingly.
524 */
525 /* gravity correction for accel_bias format */
526 long gravity = axis_sign * (32768L / accel_scale.i) * 1000L;
527 accel_bias[axis].l -= gravity;
528 /* gravity correction for saved_data.accel_bias format */
529 gravity = axis_sign * (32768L / accel_scale.i) * 65536L;
530 #ifdef DEBUG_PRINT
531 printf("Self-Test:Gravity = %ld\n", gravity);
532 #endif
533 save_data.factory_accel_bias[axis] -= gravity;
534
535 printf("Self-Test:Saved Accel bias (gravity removed) [0..2] = "
536 "[%ld %ld %ld]\n",
537 save_data.factory_accel_bias[0], save_data.factory_accel_bias[1],
538 save_data.factory_accel_bias[2]);
539 /* write the accel biases down to the hardware now */
540 if (write_biases_immediately) {
541 int offsets[3] = {0};
542 /* NOTE: 16 is the gee scale of the dmp_bias settings */
543 scale_ratio = 16 / accel_scale.i;
544 /* NOTE: the 2 factor is to account the halved resolution for the
545 accel offset registers */
546 offsets[0] = -accel_bias[0].l / 1000 / 2 / scale_ratio;
547 if (write_sysfs_int(mpu.accel_x_offset, offsets[0]) < 0) {
548 printf("Self-Test:ERR-Failed to write %s\n", mpu.accel_x_offset);
549 }
550 offsets[1] = -accel_bias[1].l / 1000 / 2 / scale_ratio;
551 if (write_sysfs_int(mpu.accel_y_offset, offsets[1]) < 0) {
552 printf("Self-Test:ERR-Failed to write %s\n", mpu.accel_y_offset);
553 }
554 offsets[2] = -accel_bias[2].l / 1000 / 2 / scale_ratio;
555 if (write_sysfs_int(mpu.accel_z_offset, offsets[2]) < 0) {
556 printf("Self-Test:ERR-Failed to write %s\n", mpu.accel_z_offset);
557 }
558 printf("Self-Test:Written Accel offsets[0..2] = [%d %d %d]\n",
559 offsets[0], offsets[1], offsets[2]);
560 }
561
562 printf("Self-Test:Saved Gyro bias[0..2] = [%ld %ld %ld]\n",
563 save_data.factory_gyro_bias[0], save_data.factory_gyro_bias[1],
564 save_data.factory_gyro_bias[2]);
565 /* write the gyro biases down to the hardware now */
566 if (write_biases_immediately) {
567 int offsets[3] = {0};
568 /* NOTE: 1000 is the dps scale of the offset registers */
569 scale_ratio = 1000 / gyro_scale.i;
570 offsets[0] = -gyro_bias[0].l / 1000 / scale_ratio;
571 if (write_sysfs_int(mpu.gyro_x_offset, offsets[0]) < 0) {
572 printf("Self-Test:ERR-Failed to write %s\n", mpu.gyro_x_offset);
573 }
574 offsets[1] = -gyro_bias[1].l / 1000 / scale_ratio;
575 if (write_sysfs_int(mpu.gyro_y_offset, offsets[1]) < 0) {
576 printf("Self-Test:ERR-Failed to write %s\n", mpu.gyro_y_offset);
577 }
578 offsets[2] = -gyro_bias[2].l / 1000 / scale_ratio;
579 if (write_sysfs_int(mpu.gyro_z_offset, offsets[2]) < 0) {
580 printf("Self-Test:ERR-Failed to write %s\n", mpu.gyro_z_offset);
581 }
582 printf("Self-Test:Written Gyro offsets[0..2] = [%d %d %d]\n",
583 offsets[0], offsets[1], offsets[2]);
584 }
585
586 printf("Self-Test:Gyro temperature @ time stored %ld\n",
587 save_data.gyro_temp);
588 printf("Self-Test:Accel temperature @ time stored %ld\n",
589 save_data.accel_temp);
590
591 // Get size of packet to store.
592 inv_get_mpl_state_size(&packet_sz);
593
594 // Create place to store data
595 buffer = (unsigned char *)malloc(packet_sz + 10);
596 if (buffer == NULL) {
597 printf("Self-Test:Can't allocate buffer\n");
598 result = -1;
599 goto free_sysfs_storage;
600 }
601
602 // Store the data
603 result = inv_save_mpl_states(buffer, packet_sz);
604 if (result) {
605 result = -1;
606 } else {
607 fptr = fopen(MLCAL_FILE, "wb+");
608 if (fptr != NULL) {
609 fwrite(buffer, 1, packet_sz, fptr);
610 fclose(fptr);
611 } else {
612 printf("Self-Test:ERR- Can't open calibration file to write - %s\n",
613 MLCAL_FILE);
614 result= -1;
615 }
616 }
617
618 free(buffer);
619
620 restore_settings:
621 if (write_sysfs_int(mpu.dmp_on, mpu.dmp_on_value) < 0) {
622 printf("Self-Test:ERR-Failed to restore dmp_on\n");
623 }
624 if (write_sysfs_int(mpu.accel_enable, mpu.accel_enable_value) < 0) {
625 printf("Self-Test:ERR-Failed to restore accel_enable\n");
626 }
627 if (write_sysfs_int(mpu.gyro_enable, mpu.gyro_enable_value) < 0) {
628 printf("Self-Test:ERR-Failed to restore gyro_enable\n");
629 }
630 if (compass_present) {
631 if (write_sysfs_int(mpu.compass_enable, mpu.compass_enable_value) < 0) {
632 printf("Self-Test:ERR-Failed to restore compass_enable\n");
633 }
634 }
635 if (write_sysfs_int(mpu.enable, mpu.enable_value) < 0) {
636 printf("Self-Test:ERR-Failed to restore buffer/enable\n");
637 }
638 if (write_sysfs_int(mpu.power_state, mpu.power_state_value) < 0) {
639 printf("Self-Test:ERR-Failed to restore power_state\n");
640 }
641
642 free_sysfs_storage:
643 free(sysfs_names_ptr);
644 return result;
645 }
646
647