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 */ 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 */ 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 */ 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 */ 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 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 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 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 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 ******************************************************************************/ 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
