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
43typedef union {
44    long l;
45    int i;
46} bias_dtype;
47
48char *sysfs_names_ptr;
49struct 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
81static struct inv_db_save_t save_data;
82static bool write_biases_immediately = false;
83
84/** This function receives the data that was stored in non-volatile memory
85    between power off */
86static 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 */
94static 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 */
101int 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 */
118int 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
134int 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
149int 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
164int 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
228void 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 ******************************************************************************/
252int 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, &timestamp);
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
620restore_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
642free_sysfs_storage:
643    free(sysfs_names_ptr);
644    return result;
645}
646
647