1 /* 2 $License: 3 Copyright 2011 InvenSense, Inc. 4 5 Licensed under the Apache License, Version 2.0 (the "License"); 6 you may not use this file except in compliance with the License. 7 You may obtain a copy of the License at 8 9 http://www.apache.org/licenses/LICENSE-2.0 10 11 Unless required by applicable law or agreed to in writing, software 12 distributed under the License is distributed on an "AS IS" BASIS, 13 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 See the License for the specific language governing permissions and 15 limitations under the License. 16 $ 17 */ 18 19 /****************************************************************************** 20 * 21 * $Id: mputest.c 5637 2011-06-14 01:13:53Z mcaramello $ 22 * 23 *****************************************************************************/ 24 25 /* 26 * MPU Self Test functions 27 * Version 2.4 28 * May 13th, 2011 29 */ 30 31 /** 32 * @defgroup MPU_SELF_TEST 33 * @brief MPU Self Test functions 34 * 35 * These functions provide an in-site test of the MPU 3xxx chips. The main 36 * entry point is the inv_mpu_test function. 37 * This runs the tests (as described in the accompanying documentation) and 38 * writes a configuration file containing initial calibration data. 39 * inv_mpu_test returns INV_SUCCESS if the chip passes the tests. 40 * Otherwise, an error code is returned. 41 * The functions in this file rely on MLSL and MLOS: refer to the MPL 42 * documentation for more information regarding the system interface 43 * files. 44 * 45 * @{ 46 * @file mputest.c 47 * @brief MPU Self Test routines for assessing gyro sensor status 48 * after surface mount has happened on the target host platform. 49 */ 50 51 #include <stdio.h> 52 #include <time.h> 53 #include <string.h> 54 #include <math.h> 55 #include <stdlib.h> 56 #ifdef LINUX 57 #include <unistd.h> 58 #endif 59 60 #include "mpu.h" 61 #include "mldl.h" 62 #include "mldl_cfg.h" 63 #include "accel.h" 64 #include "mlFIFO.h" 65 #include "slave.h" 66 #include "ml.h" 67 #include "ml_stored_data.h" 68 #include "checksum.h" 69 70 #include "mlsl.h" 71 #include "mlos.h" 72 73 #include "log.h" 74 #undef MPL_LOG_TAG 75 #define MPL_LOG_TAG "MPL-mpust" 76 77 #ifdef __cplusplus 78 extern "C" { 79 #endif 80 81 /* 82 Defines 83 */ 84 85 #define VERBOSE_OUT 0 86 87 /*#define TRACK_IDS*/ 88 89 /*--- Test parameters defaults. See set_test_parameters for more details ---*/ 90 91 #define DEF_MPU_ADDR (0x68) /* I2C address of the mpu */ 92 93 #if (USE_SENSE_PATH_TEST == 1) /* gyro full scale dps */ 94 #define DEF_GYRO_FULLSCALE (2000) 95 #else 96 #define DEF_GYRO_FULLSCALE (250) 97 #endif 98 99 #define DEF_GYRO_SENS (32768.f / DEF_GYRO_FULLSCALE) 100 /* gyro sensitivity LSB/dps */ 101 #define DEF_PACKET_THRESH (75) /* 600 ms / 8ms / sample */ 102 #define DEF_TOTAL_TIMING_TOL (.03f) /* 3% = 2 pkts + 1% proc tol. */ 103 #define DEF_BIAS_THRESH (40 * DEF_GYRO_SENS) 104 /* 40 dps in LSBs */ 105 #define DEF_RMS_THRESH (0.4f * DEF_GYRO_SENS) 106 /* 0.4 dps-rms in LSB-rms */ 107 #define DEF_SP_SHIFT_THRESH_CUST (.05f) /* 5% */ 108 #define DEF_TEST_TIME_PER_AXIS (600) /* ms of time spent collecting 109 data for each axis, 110 multiple of 600ms */ 111 #define DEF_N_ACCEL_SAMPLES (20) /* num of accel samples to 112 average from, if applic. */ 113 114 #define ML_INIT_CAL_LEN (36) /* length in bytes of 115 calibration data file */ 116 117 /* 118 Macros 119 */ 120 121 #define CHECK_TEST_ERROR(x) \ 122 if (x) { \ 123 MPL_LOGI("error %d @ %s|%d\n", x, __func__, __LINE__); \ 124 return (-1); \ 125 } 126 127 #define SHORT_TO_TEMP_C(shrt) (((shrt+13200.f)/280.f)+35.f) 128 129 #define USHORT_TO_CHARS(chr,shrt) (chr)[0]=(unsigned char)(shrt>>8); \ 130 (chr)[1]=(unsigned char)(shrt); 131 132 #define UINT_TO_CHARS(chr,ui) (chr)[0]=(unsigned char)(ui>>24); \ 133 (chr)[1]=(unsigned char)(ui>>16); \ 134 (chr)[2]=(unsigned char)(ui>>8); \ 135 (chr)[3]=(unsigned char)(ui); 136 137 #define FLOAT_TO_SHORT(f) ( \ 138 (fabs(f-(short)f)>=0.5) ? ( \ 139 ((short)f)+(f<0?(-1):(+1))) : \ 140 ((short)f) \ 141 ) 142 143 #define CHARS_TO_SHORT(d) ((((short)(d)[0])<<8)+(d)[1]) 144 #define CHARS_TO_SHORT_SWAPPED(d) ((((short)(d)[1])<<8)+(d)[0]) 145 146 #define ACCEL_UNPACK(d) d[0], d[1], d[2], d[3], d[4], d[5] 147 148 #define CHECK_NACKS(d) ( \ 149 d[0]==0xff && d[1]==0xff && \ 150 d[2]==0xff && d[3]==0xff && \ 151 d[4]==0xff && d[5]==0xff \ 152 ) 153 154 /* 155 Prototypes 156 */ 157 158 static inv_error_t test_get_data( 159 void *mlsl_handle, 160 struct mldl_cfg *mputestCfgPtr, 161 short *vals); 162 163 /* 164 Types 165 */ 166 typedef struct { 167 float gyro_sens; 168 int gyro_fs; 169 int packet_thresh; 170 float total_timing_tol; 171 int bias_thresh; 172 float rms_threshSq; 173 float sp_shift_thresh; 174 unsigned int test_time_per_axis; 175 unsigned short accel_samples; 176 } tTestSetup; 177 178 /* 179 Global variables 180 */ 181 static unsigned char dataout[20]; 182 static unsigned char dataStore[ML_INIT_CAL_LEN]; 183 184 static tTestSetup test_setup = { 185 DEF_GYRO_SENS, 186 DEF_GYRO_FULLSCALE, 187 DEF_PACKET_THRESH, 188 DEF_TOTAL_TIMING_TOL, 189 (int)DEF_BIAS_THRESH, 190 DEF_RMS_THRESH * DEF_RMS_THRESH, 191 DEF_SP_SHIFT_THRESH_CUST, 192 DEF_TEST_TIME_PER_AXIS, 193 DEF_N_ACCEL_SAMPLES 194 }; 195 196 static float adjGyroSens; 197 static char a_name[3][2] = {"X", "Y", "Z"}; 198 199 /* 200 NOTE : modify get_slave_descr parameter below to reflect 201 the DEFAULT accelerometer in use. The accelerometer in use 202 can be modified at run-time using the inv_test_setup_accel API. 203 NOTE : modify the expected z axis orientation (Z axis pointing 204 upward or downward) 205 */ 206 207 signed char g_z_sign = +1; 208 struct mldl_cfg *mputestCfgPtr = NULL; 209 210 #ifndef LINUX 211 /** 212 * @internal 213 * @brief usec precision sleep function. 214 * @param number of micro seconds (us) to sleep for. 215 */ 216 static void usleep(unsigned long t) 217 { 218 unsigned long start = inv_get_tick_count(); 219 while (inv_get_tick_count()-start < t / 1000); 220 } 221 #endif 222 223 /** 224 * @brief Modify the self test limits from their default values. 225 * 226 * @param slave_addr 227 * the slave address the MPU device is setup to respond at. 228 * The default is DEF_MPU_ADDR = 0x68. 229 * @param sensitivity 230 * the read sensitivity of the device in LSB/dps as it is trimmed. 231 * NOTE : if using the self test as part of the MPL, the 232 * sensitivity the different sensitivity trims are already 233 * taken care of. 234 * @param p_thresh 235 * number of packets expected to be received in a 600 ms period. 236 * Depends on the sampling frequency of choice (set by default to 237 * 125 Hz) and low pass filter cut-off frequency selection (set 238 * to 42 Hz). 239 * The default is DEF_PACKET_THRESH = 75 packets. 240 * @param total_time_tol 241 * time skew tolerance, taking into account imprecision in turning 242 * the FIFO on and off and the processor time imprecision (for 243 * 1 GHz processor). 244 * The default is DEF_TOTAL_TIMING_TOL = 3 %, about 2 packets. 245 * @param bias_thresh 246 * bias level threshold, the maximun acceptable no motion bias 247 * for a production quality part. 248 * The default is DEF_BIAS_THRESH = 40 dps. 249 * @param rms_thresh 250 * the limit standard deviation (=~ RMS) set to assess whether 251 * the noise level on the part is acceptable. 252 * The default is DEF_RMS_THRESH = 0.2 dps-rms. 253 * @param sp_shift_thresh 254 * the limit shift applicable to the Sense Path self test 255 * calculation. 256 */ 257 void inv_set_test_parameters(unsigned int slave_addr, float sensitivity, 258 int p_thresh, float total_time_tol, 259 int bias_thresh, float rms_thresh, 260 float sp_shift_thresh, 261 unsigned short accel_samples) 262 { 263 mputestCfgPtr->addr = slave_addr; 264 test_setup.gyro_sens = sensitivity; 265 test_setup.gyro_fs = (int)(32768.f / sensitivity); 266 test_setup.packet_thresh = p_thresh; 267 test_setup.total_timing_tol = total_time_tol; 268 test_setup.bias_thresh = bias_thresh; 269 test_setup.rms_threshSq = rms_thresh * rms_thresh; 270 test_setup.sp_shift_thresh = sp_shift_thresh; 271 test_setup.accel_samples = accel_samples; 272 } 273 274 #define X (0) 275 #define Y (1) 276 #define Z (2) 277 278 #ifdef CONFIG_MPU_SENSORS_MPU3050 279 /** 280 * @brief Test the gyroscope sensor. 281 * Implements the core logic of the MPU Self Test. 282 * Produces the PASS/FAIL result. Loads the calculated gyro biases 283 * and temperature datum into the corresponding pointers. 284 * @param mlsl_handle 285 * serial interface handle to allow serial communication with the 286 * device, both gyro and accelerometer. 287 * @param gyro_biases 288 * output pointer to store the initial bias calculation provided 289 * by the MPU Self Test. Requires 3 elements for gyro X, Y, 290 * and Z. 291 * @param temp_avg 292 * output pointer to store the initial average temperature as 293 * provided by the MPU Self Test. 294 * @param perform_full_test 295 * If 1: 296 * calculates offset, drive frequency, and noise and compare it 297 * against set thresholds. 298 * Report also the final result using a bit-mask like error code 299 * as explained in return value description. 300 * When 0: 301 * skip the noise and drive frequency calculation and pass/fail 302 * assessment; simply calculates the gyro biases. 303 * 304 * @return 0 on success. 305 * On error, the return value is a bitmask representing: 306 * 0, 1, 2 Failures with PLLs on X, Y, Z gyros respectively 307 * (decimal values will be 1, 2, 4 respectively). 308 * 3, 4, 5 Excessive offset with X, Y, Z gyros respectively 309 * (decimal values will be 8, 16, 32 respectively). 310 * 6, 7, 8 Excessive noise with X, Y, Z gyros respectively 311 * (decimal values will be 64, 128, 256 respectively). 312 * 9 If any of the RMS noise values is zero, it could be 313 * due to a non-functional gyro or FIFO/register failure. 314 * (decimal value will be 512). 315 * (decimal values will be 1024, 2048, 4096 respectively). 316 */ 317 int inv_test_gyro_3050(void *mlsl_handle, 318 short gyro_biases[3], short *temp_avg, 319 uint_fast8_t perform_full_test) 320 { 321 int retVal = 0; 322 inv_error_t result; 323 324 int total_count = 0; 325 int total_count_axis[3] = {0, 0, 0}; 326 int packet_count; 327 short x[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; 328 short y[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; 329 short z[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; 330 unsigned char regs[7]; 331 332 int temperature; 333 float Avg[3]; 334 float RMS[3]; 335 int i, j, tmp; 336 char tmpStr[200]; 337 338 temperature = 0; 339 340 /* sample rate = 8ms */ 341 result = inv_serial_single_write( 342 mlsl_handle, mputestCfgPtr->addr, 343 MPUREG_SMPLRT_DIV, 0x07); 344 CHECK_TEST_ERROR(result); 345 346 regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */ 347 switch (DEF_GYRO_FULLSCALE) { 348 case 2000: 349 regs[0] |= 0x18; 350 break; 351 case 1000: 352 regs[0] |= 0x10; 353 break; 354 case 500: 355 regs[0] |= 0x08; 356 break; 357 case 250: 358 default: 359 regs[0] |= 0x00; 360 break; 361 } 362 result = inv_serial_single_write( 363 mlsl_handle, mputestCfgPtr->addr, 364 MPUREG_DLPF_FS_SYNC, regs[0]); 365 CHECK_TEST_ERROR(result); 366 result = inv_serial_single_write( 367 mlsl_handle, mputestCfgPtr->addr, 368 MPUREG_INT_CFG, 0x00); 369 CHECK_TEST_ERROR(result); 370 371 /* 1st, timing test */ 372 for (j = 0; j < 3; j++) { 373 374 MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]); 375 376 /* turn on all gyros, use gyro X for clocking 377 Set to Y and Z for 2nd and 3rd iteration */ 378 result = inv_serial_single_write( 379 mlsl_handle, mputestCfgPtr->addr, 380 MPUREG_PWR_MGM, j + 1); 381 CHECK_TEST_ERROR(result); 382 383 /* wait for 2 ms after switching clock source */ 384 usleep(2000); 385 386 /* we will enable XYZ gyro in FIFO and nothing else */ 387 result = inv_serial_single_write( 388 mlsl_handle, mputestCfgPtr->addr, 389 MPUREG_FIFO_EN2, 0x00); 390 CHECK_TEST_ERROR(result); 391 /* enable/reset FIFO */ 392 result = inv_serial_single_write( 393 mlsl_handle, mputestCfgPtr->addr, 394 MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST); 395 CHECK_TEST_ERROR(result); 396 397 tmp = (int)(test_setup.test_time_per_axis / 600); 398 while (tmp-- > 0) { 399 /* enable XYZ gyro in FIFO and nothing else */ 400 result = inv_serial_single_write(mlsl_handle, 401 mputestCfgPtr->addr, MPUREG_FIFO_EN1, 402 BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT); 403 CHECK_TEST_ERROR(result); 404 405 /* wait for 600 ms for data */ 406 usleep(600000); 407 408 /* stop storing gyro in the FIFO */ 409 result = inv_serial_single_write( 410 mlsl_handle, mputestCfgPtr->addr, 411 MPUREG_FIFO_EN1, 0x00); 412 CHECK_TEST_ERROR(result); 413 414 /* Getting number of bytes in FIFO */ 415 result = inv_serial_read( 416 mlsl_handle, mputestCfgPtr->addr, 417 MPUREG_FIFO_COUNTH, 2, dataout); 418 CHECK_TEST_ERROR(result); 419 /* number of 6 B packets in the FIFO */ 420 packet_count = CHARS_TO_SHORT(dataout) / 6; 421 sprintf(tmpStr, "Packet Count: %d - ", packet_count); 422 423 if ( abs(packet_count - test_setup.packet_thresh) 424 <= /* Within +/- total_timing_tol % range */ 425 test_setup.total_timing_tol * test_setup.packet_thresh) { 426 for (i = 0; i < packet_count; i++) { 427 /* getting FIFO data */ 428 result = inv_serial_read_fifo(mlsl_handle, 429 mputestCfgPtr->addr, 6, dataout); 430 CHECK_TEST_ERROR(result); 431 x[total_count + i] = CHARS_TO_SHORT(&dataout[0]); 432 y[total_count + i] = CHARS_TO_SHORT(&dataout[2]); 433 z[total_count + i] = CHARS_TO_SHORT(&dataout[4]); 434 if (VERBOSE_OUT) { 435 MPL_LOGI("Gyros %-4d : %+13d %+13d %+13d\n", 436 total_count + i, x[total_count + i], 437 y[total_count + i], z[total_count + i]); 438 } 439 } 440 total_count += packet_count; 441 total_count_axis[j] += packet_count; 442 sprintf(tmpStr, "%sOK", tmpStr); 443 } else { 444 if (perform_full_test) 445 retVal |= 1 << j; 446 sprintf(tmpStr, "%sNOK - samples ignored", tmpStr); 447 } 448 MPL_LOGI("%s\n", tmpStr); 449 } 450 451 /* remove gyros from FIFO */ 452 result = inv_serial_single_write( 453 mlsl_handle, mputestCfgPtr->addr, 454 MPUREG_FIFO_EN1, 0x00); 455 CHECK_TEST_ERROR(result); 456 457 /* Read Temperature */ 458 result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, 459 MPUREG_TEMP_OUT_H, 2, dataout); 460 CHECK_TEST_ERROR(result); 461 temperature += (short)CHARS_TO_SHORT(dataout); 462 } 463 464 MPL_LOGI("\n"); 465 MPL_LOGI("Total %d samples\n", total_count); 466 MPL_LOGI("\n"); 467 468 /* 2nd, check bias from X and Y PLL clock source */ 469 tmp = total_count != 0 ? total_count : 1; 470 for (i = 0, 471 Avg[X] = .0f, Avg[Y] = .0f, Avg[Z] = .0f; 472 i < total_count; i++) { 473 Avg[X] += 1.f * x[i] / tmp; 474 Avg[Y] += 1.f * y[i] / tmp; 475 Avg[Z] += 1.f * z[i] / tmp; 476 } 477 MPL_LOGI("bias : %+13.3f %+13.3f %+13.3f (LSB)\n", 478 Avg[X], Avg[Y], Avg[Z]); 479 if (VERBOSE_OUT) { 480 MPL_LOGI(" : %+13.3f %+13.3f %+13.3f (dps)\n", 481 Avg[X] / adjGyroSens, 482 Avg[Y] / adjGyroSens, 483 Avg[Z] / adjGyroSens); 484 } 485 if(perform_full_test) { 486 for (j = 0; j < 3; j++) { 487 if (fabs(Avg[j]) > test_setup.bias_thresh) { 488 MPL_LOGI("%s-Gyro bias (%.0f) exceeded threshold " 489 "(threshold = %d)\n", 490 a_name[j], Avg[j], test_setup.bias_thresh); 491 retVal |= 1 << (3+j); 492 } 493 } 494 } 495 496 /* 3rd, check RMS */ 497 if (perform_full_test) { 498 for (i = 0, 499 RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f; 500 i < total_count; i++) { 501 RMS[X] += (x[i] - Avg[X]) * (x[i] - Avg[X]); 502 RMS[Y] += (y[i] - Avg[Y]) * (y[i] - Avg[Y]); 503 RMS[Z] += (z[i] - Avg[Z]) * (z[i] - Avg[Z]); 504 } 505 for (j = 0; j < 3; j++) { 506 if (RMS[j] > test_setup.rms_threshSq * total_count) { 507 MPL_LOGI("%s-Gyro RMS (%.2f) exceeded threshold " 508 "(threshold = %.2f)\n", 509 a_name[j], sqrt(RMS[j] / total_count), 510 sqrt(test_setup.rms_threshSq)); 511 retVal |= 1 << (6+j); 512 } 513 } 514 515 MPL_LOGI("RMS : %+13.3f %+13.3f %+13.3f (LSB-rms)\n", 516 sqrt(RMS[X] / total_count), 517 sqrt(RMS[Y] / total_count), 518 sqrt(RMS[Z] / total_count)); 519 if (VERBOSE_OUT) { 520 MPL_LOGI("RMS ^ 2 : %+13.3f %+13.3f %+13.3f\n", 521 RMS[X] / total_count, 522 RMS[Y] / total_count, 523 RMS[Z] / total_count); 524 } 525 526 if (RMS[X] == 0 || RMS[Y] == 0 || RMS[Z] == 0) { 527 /* If any of the RMS noise value returns zero, 528 then we might have dead gyro or FIFO/register failure, 529 the part is sleeping, or the part is not responsive */ 530 retVal |= 1 << 9; 531 } 532 } 533 534 /* 4th, temperature average */ 535 temperature /= 3; 536 if (VERBOSE_OUT) 537 MPL_LOGI("Temperature : %+13.3f %13s %13s (deg. C)\n", 538 SHORT_TO_TEMP_C(temperature), "", ""); 539 540 /* load into final storage */ 541 *temp_avg = (short)temperature; 542 gyro_biases[X] = FLOAT_TO_SHORT(Avg[X]); 543 gyro_biases[Y] = FLOAT_TO_SHORT(Avg[Y]); 544 gyro_biases[Z] = FLOAT_TO_SHORT(Avg[Z]); 545 546 return retVal; 547 } 548 549 #else /* CONFIG_MPU_SENSORS_MPU3050 */ 550 551 /** 552 * @brief Test the gyroscope sensor. 553 * Implements the core logic of the MPU Self Test but does not provide 554 * a PASS/FAIL output as in the MPU-3050 implementation. 555 * @param mlsl_handle 556 * serial interface handle to allow serial communication with the 557 * device, both gyro and accelerometer. 558 * @param gyro_biases 559 * output pointer to store the initial bias calculation provided 560 * by the MPU Self Test. Requires 3 elements for gyro X, Y, 561 * and Z. 562 * @param temp_avg 563 * output pointer to store the initial average temperature as 564 * provided by the MPU Self Test. 565 * 566 * @return 0 on success. 567 * A non-zero error code on error. 568 */ 569 int inv_test_gyro_6050(void *mlsl_handle, 570 short gyro_biases[3], short *temp_avg) 571 { 572 inv_error_t result; 573 574 int total_count = 0; 575 int total_count_axis[3] = {0, 0, 0}; 576 int packet_count; 577 short x[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; 578 short y[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; 579 short z[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; 580 unsigned char regs[7]; 581 582 int temperature = 0; 583 float Avg[3]; 584 int i, j, tmp; 585 char tmpStr[200]; 586 587 /* sample rate = 8ms */ 588 result = inv_serial_single_write( 589 mlsl_handle, mputestCfgPtr->addr, 590 MPUREG_SMPLRT_DIV, 0x07); 591 if (result) { 592 LOG_RESULT_LOCATION(result); 593 return result; 594 } 595 596 regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */ 597 switch (DEF_GYRO_FULLSCALE) { 598 case 2000: 599 regs[0] |= 0x18; 600 break; 601 case 1000: 602 regs[0] |= 0x10; 603 break; 604 case 500: 605 regs[0] |= 0x08; 606 break; 607 case 250: 608 default: 609 regs[0] |= 0x00; 610 break; 611 } 612 result = inv_serial_single_write( 613 mlsl_handle, mputestCfgPtr->addr, 614 MPUREG_CONFIG, regs[0]); 615 if (result) { 616 LOG_RESULT_LOCATION(result); 617 return result; 618 } 619 result = inv_serial_single_write( 620 mlsl_handle, mputestCfgPtr->addr, 621 MPUREG_INT_ENABLE, 0x00); 622 if (result) { 623 LOG_RESULT_LOCATION(result); 624 return result; 625 } 626 627 /* 1st, timing test */ 628 for (j = 0; j < 3; j++) { 629 MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]); 630 631 /* turn on all gyros, use gyro X for clocking 632 Set to Y and Z for 2nd and 3rd iteration */ 633 #ifdef CONFIG_MPU_SENSORS_MPU6050A2 634 result = inv_serial_single_write( 635 mlsl_handle, mputestCfgPtr->addr, 636 MPUREG_PWR_MGMT_1, BITS_PWRSEL | (j + 1)); 637 #else 638 result = inv_serial_single_write( 639 mlsl_handle, mputestCfgPtr->addr, 640 MPUREG_PWR_MGMT_1, j + 1); 641 #endif 642 if (result) { 643 LOG_RESULT_LOCATION(result); 644 return result; 645 } 646 647 /* wait for 2 ms after switching clock source */ 648 usleep(2000); 649 650 /* enable/reset FIFO */ 651 result = inv_serial_single_write( 652 mlsl_handle, mputestCfgPtr->addr, 653 MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST); 654 if (result) { 655 LOG_RESULT_LOCATION(result); 656 return result; 657 } 658 659 tmp = (int)(test_setup.test_time_per_axis / 600); 660 while (tmp-- > 0) { 661 /* enable XYZ gyro in FIFO and nothing else */ 662 result = inv_serial_single_write(mlsl_handle, 663 mputestCfgPtr->addr, MPUREG_FIFO_EN, 664 BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT); 665 if (result) { 666 LOG_RESULT_LOCATION(result); 667 return result; 668 } 669 670 /* wait for 600 ms for data */ 671 usleep(600000); 672 /* stop storing gyro in the FIFO */ 673 result = inv_serial_single_write( 674 mlsl_handle, mputestCfgPtr->addr, 675 MPUREG_FIFO_EN, 0x00); 676 if (result) { 677 LOG_RESULT_LOCATION(result); 678 return result; 679 } 680 /* Getting number of bytes in FIFO */ 681 result = inv_serial_read( 682 mlsl_handle, mputestCfgPtr->addr, 683 MPUREG_FIFO_COUNTH, 2, dataout); 684 if (result) { 685 LOG_RESULT_LOCATION(result); 686 return result; 687 } 688 /* number of 6 B packets in the FIFO */ 689 packet_count = CHARS_TO_SHORT(dataout) / 6; 690 sprintf(tmpStr, "Packet Count: %d - ", packet_count); 691 692 if (abs(packet_count - test_setup.packet_thresh) 693 <= /* Within +/- total_timing_tol % range */ 694 test_setup.total_timing_tol * test_setup.packet_thresh) { 695 for (i = 0; i < packet_count; i++) { 696 /* getting FIFO data */ 697 result = inv_serial_read_fifo(mlsl_handle, 698 mputestCfgPtr->addr, 6, dataout); 699 if (result) { 700 LOG_RESULT_LOCATION(result); 701 return result; 702 } 703 x[total_count + i] = CHARS_TO_SHORT(&dataout[0]); 704 y[total_count + i] = CHARS_TO_SHORT(&dataout[2]); 705 z[total_count + i] = CHARS_TO_SHORT(&dataout[4]); 706 if (VERBOSE_OUT) { 707 MPL_LOGI("Gyros %-4d : %+13d %+13d %+13d\n", 708 total_count + i, x[total_count + i], 709 y[total_count + i], z[total_count + i]); 710 } 711 } 712 total_count += packet_count; 713 total_count_axis[j] += packet_count; 714 sprintf(tmpStr, "%sOK", tmpStr); 715 } else { 716 sprintf(tmpStr, "%sNOK - samples ignored", tmpStr); 717 } 718 MPL_LOGI("%s\n", tmpStr); 719 } 720 721 /* remove gyros from FIFO */ 722 result = inv_serial_single_write( 723 mlsl_handle, mputestCfgPtr->addr, 724 MPUREG_FIFO_EN, 0x00); 725 if (result) { 726 LOG_RESULT_LOCATION(result); 727 return result; 728 } 729 730 /* Read Temperature */ 731 result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, 732 MPUREG_TEMP_OUT_H, 2, dataout); 733 if (result) { 734 LOG_RESULT_LOCATION(result); 735 return result; 736 } 737 temperature += (short)CHARS_TO_SHORT(dataout); 738 } 739 740 MPL_LOGI("\n"); 741 MPL_LOGI("Total %d samples\n", total_count); 742 MPL_LOGI("\n"); 743 744 /* 2nd, check bias from X and Y PLL clock source */ 745 tmp = total_count != 0 ? total_count : 1; 746 for (i = 0, 747 Avg[X] = .0f, Avg[Y] = .0f, Avg[Z] = .0f; 748 i < total_count; i++) { 749 Avg[X] += 1.f * x[i] / tmp; 750 Avg[Y] += 1.f * y[i] / tmp; 751 Avg[Z] += 1.f * z[i] / tmp; 752 } 753 MPL_LOGI("bias : %+13.3f %+13.3f %+13.3f (LSB)\n", 754 Avg[X], Avg[Y], Avg[Z]); 755 if (VERBOSE_OUT) { 756 MPL_LOGI(" : %+13.3f %+13.3f %+13.3f (dps)\n", 757 Avg[X] / adjGyroSens, 758 Avg[Y] / adjGyroSens, 759 Avg[Z] / adjGyroSens); 760 } 761 762 temperature /= 3; 763 if (VERBOSE_OUT) 764 MPL_LOGI("Temperature : %+13.3f %13s %13s (deg. C)\n", 765 SHORT_TO_TEMP_C(temperature), "", ""); 766 767 /* load into final storage */ 768 *temp_avg = (short)temperature; 769 gyro_biases[X] = FLOAT_TO_SHORT(Avg[X]); 770 gyro_biases[Y] = FLOAT_TO_SHORT(Avg[Y]); 771 gyro_biases[Z] = FLOAT_TO_SHORT(Avg[Z]); 772 773 return INV_SUCCESS; 774 } 775 776 #endif /* CONFIG_MPU_SENSORS_MPU3050 */ 777 778 #ifdef TRACK_IDS 779 /** 780 * @internal 781 * @brief Retrieve the unique MPU device identifier from the internal OTP 782 * bank 0 memory. 783 * @param mlsl_handle 784 * serial interface handle to allow serial communication with the 785 * device, both gyro and accelerometer. 786 * @return 0 on success, a non-zero error code from the serial layer on error. 787 */ 788 static inv_error_t test_get_mpu_id(void *mlsl_handle) 789 { 790 inv_error_t result; 791 unsigned char otp0[8]; 792 793 794 result = 795 inv_serial_read_mem(mlsl_handle, mputestCfgPtr->addr, 796 (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0) << 8 | 797 0x00, 6, otp0); 798 if (result) 799 goto close; 800 801 MPL_LOGI("\n"); 802 MPL_LOGI("DIE_ID : %06X\n", 803 ((int)otp0[1] << 8 | otp0[0]) & 0x1fff); 804 MPL_LOGI("WAFER_ID : %06X\n", 805 (((int)otp0[2] << 8 | otp0[1]) & 0x03ff ) >> 5); 806 MPL_LOGI("A_LOT_ID : %06X\n", 807 ( ((int)otp0[4] << 16 | (int)otp0[3] << 8 | 808 otp0[2]) & 0x3ffff) >> 2); 809 MPL_LOGI("W_LOT_ID : %06X\n", 810 ( ((int)otp0[5] << 8 | otp0[4]) & 0x3fff) >> 2); 811 MPL_LOGI("WP_ID : %06X\n", 812 ( ((int)otp0[6] << 8 | otp0[5]) & 0x03ff) >> 7); 813 MPL_LOGI("REV_ID : %06X\n", otp0[6] >> 2); 814 MPL_LOGI("\n"); 815 816 close: 817 result = 818 inv_serial_single_write(mlsl_handle, mputestCfgPtr->addr, MPUREG_BANK_SEL, 0x00); 819 return result; 820 } 821 #endif /* TRACK_IDS */ 822 823 /** 824 * @brief If requested via inv_test_setup_accel(), test the accelerometer biases 825 * and calculate the necessary bias correction. 826 * @param mlsl_handle 827 * serial interface handle to allow serial communication with the 828 * device, both gyro and accelerometer. 829 * @param bias 830 * output pointer to store the initial bias calculation provided 831 * by the MPU Self Test. Requires 3 elements to store accel X, Y, 832 * and Z axis bias. 833 * @param perform_full_test 834 * If 1: 835 * calculates offsets and noise and compare it against set 836 * thresholds. The final exist status will reflect if any of the 837 * value is outside of the expected range. 838 * When 0; 839 * skip the noise calculation and pass/fail assessment; simply 840 * calculates the accel biases. 841 * 842 * @return 0 on success. A non-zero error code on error. 843 */ 844 int inv_test_accel(void *mlsl_handle, 845 short *bias, long gravity, 846 uint_fast8_t perform_full_test) 847 { 848 int i; 849 850 short *p_vals; 851 float x = 0.f, y = 0.f, z = 0.f, zg = 0.f; 852 float RMS[3]; 853 float accelRmsThresh = 1000000.f; /* enourmous so that the test always 854 passes - future deployment */ 855 unsigned short res; 856 unsigned long orig_requested_sensors; 857 struct mpu_platform_data *mputestPData = mputestCfgPtr->pdata; 858 859 p_vals = (short*)inv_malloc(sizeof(short) * 3 * test_setup.accel_samples); 860 861 /* load the slave descr from the getter */ 862 if (mputestPData->accel.get_slave_descr == NULL) { 863 MPL_LOGI("\n"); 864 MPL_LOGI("No accelerometer configured\n"); 865 return 0; 866 } 867 if (mputestCfgPtr->accel == NULL) { 868 MPL_LOGI("\n"); 869 MPL_LOGI("No accelerometer configured\n"); 870 return 0; 871 } 872 873 /* resume the accel */ 874 orig_requested_sensors = mputestCfgPtr->requested_sensors; 875 mputestCfgPtr->requested_sensors = INV_THREE_AXIS_ACCEL | INV_THREE_AXIS_GYRO; 876 res = inv_mpu_resume(mputestCfgPtr, 877 mlsl_handle, NULL, NULL, NULL, 878 mputestCfgPtr->requested_sensors); 879 if(res != INV_SUCCESS) 880 goto accel_error; 881 882 /* wait at least a sample cycle for the 883 accel data to be retrieved by MPU */ 884 inv_sleep(inv_mpu_get_sampling_period_us(mputestCfgPtr) / 1000); 885 886 /* collect the samples */ 887 for(i = 0; i < test_setup.accel_samples; i++) { 888 short *vals = &p_vals[3 * i]; 889 if (test_get_data(mlsl_handle, mputestCfgPtr, vals)) { 890 goto accel_error; 891 } 892 x += 1.f * vals[X] / test_setup.accel_samples; 893 y += 1.f * vals[Y] / test_setup.accel_samples; 894 z += 1.f * vals[Z] / test_setup.accel_samples; 895 } 896 897 mputestCfgPtr->requested_sensors = orig_requested_sensors; 898 res = inv_mpu_suspend(mputestCfgPtr, 899 mlsl_handle, NULL, NULL, NULL, 900 INV_ALL_SENSORS); 901 if (res != INV_SUCCESS) 902 goto accel_error; 903 904 MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (LSB)\n", x, y, z); 905 if (VERBOSE_OUT) { 906 MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (gee)\n", 907 x / gravity, y / gravity, z / gravity); 908 } 909 910 bias[0] = FLOAT_TO_SHORT(x); 911 bias[1] = FLOAT_TO_SHORT(y); 912 zg = z - g_z_sign * gravity; 913 bias[2] = FLOAT_TO_SHORT(zg); 914 915 MPL_LOGI("Accel correct.: %+13d %+13d %+13d (LSB)\n", 916 bias[0], bias[1], bias[2]); 917 if (VERBOSE_OUT) { 918 MPL_LOGI("Accel correct.: " 919 "%+13.3f %+13.3f %+13.3f (gee)\n", 920 1.f * bias[0] / gravity, 921 1.f * bias[1] / gravity, 922 1.f * bias[2] / gravity); 923 } 924 925 if (perform_full_test) { 926 /* accel RMS - for now the threshold is only indicative */ 927 for (i = 0, 928 RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f; 929 i < test_setup.accel_samples; i++) { 930 short *vals = &p_vals[3 * i]; 931 RMS[X] += (vals[X] - x) * (vals[X] - x); 932 RMS[Y] += (vals[Y] - y) * (vals[Y] - y); 933 RMS[Z] += (vals[Z] - z) * (vals[Z] - z); 934 } 935 for (i = 0; i < 3; i++) { 936 if (RMS[i] > accelRmsThresh * accelRmsThresh 937 * test_setup.accel_samples) { 938 MPL_LOGI("%s-Accel RMS (%.2f) exceeded threshold " 939 "(threshold = %.2f)\n", 940 a_name[i], sqrt(RMS[i] / test_setup.accel_samples), 941 accelRmsThresh); 942 goto accel_error; 943 } 944 } 945 MPL_LOGI("RMS : %+13.3f %+13.3f %+13.3f (LSB-rms)\n", 946 sqrt(RMS[X] / DEF_N_ACCEL_SAMPLES), 947 sqrt(RMS[Y] / DEF_N_ACCEL_SAMPLES), 948 sqrt(RMS[Z] / DEF_N_ACCEL_SAMPLES)); 949 } 950 951 return 0; /* success */ 952 953 accel_error: /* error */ 954 bias[0] = bias[1] = bias[2] = 0; 955 return 1; 956 } 957 958 /** 959 * @brief an user-space substitute of the power management function(s) 960 * in mldl_cfg.c for self test usage. 961 * Wake up and sleep the device, whether that is MPU3050, MPU6050A2, 962 * or MPU6050B1. 963 * @param mlsl_handle 964 * a file handle for the serial communication port used to 965 * communicate with the MPU device. 966 * @param power_level 967 * the power state to change the device into. Currently only 0 or 968 * 1 are supported, for sleep and full-power respectively. 969 * @return 0 on success; a non-zero error code on error. 970 */ 971 static inv_error_t inv_device_power_mgmt(void *mlsl_handle, 972 uint_fast8_t power_level) 973 { 974 inv_error_t result; 975 static unsigned char pwr_mgm; 976 unsigned char b; 977 978 if (power_level != 0 && power_level != 1) { 979 return INV_ERROR_INVALID_PARAMETER; 980 } 981 982 if (power_level) { 983 result = inv_serial_read( 984 mlsl_handle, mputestCfgPtr->addr, 985 MPUREG_PWR_MGM, 1, &pwr_mgm); 986 if (result) { 987 LOG_RESULT_LOCATION(result); 988 return result; 989 } 990 991 /* reset */ 992 result = inv_serial_single_write( 993 mlsl_handle, mputestCfgPtr->addr, 994 MPUREG_PWR_MGM, pwr_mgm | BIT_H_RESET); 995 #ifndef CONFIG_MPU_SENSORS_MPU6050A2 996 if (result) { 997 LOG_RESULT_LOCATION(result); 998 return result; 999 } 1000 #endif 1001 inv_sleep(5); 1002 1003 /* re-read power mgmt reg - 1004 it may have reset after H_RESET is applied */ 1005 result = inv_serial_read( 1006 mlsl_handle, mputestCfgPtr->addr, 1007 MPUREG_PWR_MGM, 1, &b); 1008 if (result) { 1009 LOG_RESULT_LOCATION(result); 1010 return result; 1011 } 1012 1013 /* wake up */ 1014 #ifdef CONFIG_MPU_SENSORS_MPU6050A2 1015 if ((b & BITS_PWRSEL) != BITS_PWRSEL) { 1016 result = inv_serial_single_write( 1017 mlsl_handle, mputestCfgPtr->addr, 1018 MPUREG_PWR_MGM, BITS_PWRSEL); 1019 if (result) { 1020 LOG_RESULT_LOCATION(result); 1021 return result; 1022 } 1023 } 1024 #else 1025 if (pwr_mgm & BIT_SLEEP) { 1026 result = inv_serial_single_write( 1027 mlsl_handle, mputestCfgPtr->addr, 1028 MPUREG_PWR_MGM, 0x00); 1029 if (result) { 1030 LOG_RESULT_LOCATION(result); 1031 return result; 1032 } 1033 } 1034 #endif 1035 inv_sleep(60); 1036 1037 #if defined(CONFIG_MPU_SENSORS_MPU6050A2) || \ 1038 defined(CONFIG_MPU_SENSORS_MPU6050B1) 1039 result = inv_serial_single_write( 1040 mlsl_handle, mputestCfgPtr->addr, 1041 MPUREG_INT_PIN_CFG, BIT_BYPASS_EN); 1042 if (result) { 1043 LOG_RESULT_LOCATION(result); 1044 return result; 1045 } 1046 #endif 1047 } else { 1048 /* restore the power state the part was found in */ 1049 #ifdef CONFIG_MPU_SENSORS_MPU6050A2 1050 if ((pwr_mgm & BITS_PWRSEL) != BITS_PWRSEL) { 1051 result = inv_serial_single_write( 1052 mlsl_handle, mputestCfgPtr->addr, 1053 MPUREG_PWR_MGM, pwr_mgm); 1054 if (result) { 1055 LOG_RESULT_LOCATION(result); 1056 return result; 1057 } 1058 } 1059 #else 1060 if (pwr_mgm & BIT_SLEEP) { 1061 result = inv_serial_single_write( 1062 mlsl_handle, mputestCfgPtr->addr, 1063 MPUREG_PWR_MGM, pwr_mgm); 1064 if (result) { 1065 LOG_RESULT_LOCATION(result); 1066 return result; 1067 } 1068 } 1069 #endif 1070 } 1071 1072 return INV_SUCCESS; 1073 } 1074 1075 /** 1076 * @brief The main entry point of the MPU Self Test, triggering the run of 1077 * the single tests, for gyros and accelerometers. 1078 * Prepares the MPU for the test, taking the device out of low power 1079 * state if necessary, switching the MPU secondary I2C interface into 1080 * bypass mode and restoring the original power state at the end of 1081 * the test. 1082 * This function is also responsible for encoding the output of each 1083 * test in the correct format as it is stored on the file/medium of 1084 * choice (according to inv_serial_write_cal() function). 1085 * The format needs to stay perfectly consistent with the one expected 1086 * by the corresponding loader in ml_stored_data.c; currectly the 1087 * loaded in use is inv_load_cal_V1 (record type 1 - initial 1088 * calibration). 1089 * 1090 * @param mlsl_handle 1091 * serial interface handle to allow serial communication with the 1092 * device, both gyro and accelerometer. 1093 * @param provide_result 1094 * If 1: 1095 * perform and analyze the offset, drive frequency, and noise 1096 * calculation and compare it against set threshouds. Report 1097 * also the final result using a bit-mask like error code as 1098 * described in the inv_test_gyro() function. 1099 * When 0: 1100 * skip the noise and drive frequency calculation and pass/fail 1101 * assessment. It simply calculates the gyro and accel biases. 1102 * NOTE: for MPU6050 devices, this parameter is currently 1103 * ignored. 1104 * 1105 * @return 0 on success. A non-zero error code on error. 1106 * Propagates the errors from the tests up to the caller. 1107 */ 1108 int inv_mpu_test(void *mlsl_handle, uint_fast8_t provide_result) 1109 { 1110 int result = 0; 1111 1112 short temp_avg; 1113 short gyro_biases[3] = {0, 0, 0}; 1114 short accel_biases[3] = {0, 0, 0}; 1115 1116 unsigned long testStart = inv_get_tick_count(); 1117 long accelSens[3] = {0}; 1118 int ptr; 1119 int tmp; 1120 long long lltmp; 1121 uint32_t chk; 1122 1123 MPL_LOGI("Collecting %d groups of 600 ms samples for each axis\n", 1124 DEF_TEST_TIME_PER_AXIS / 600); 1125 MPL_LOGI("\n"); 1126 1127 result = inv_device_power_mgmt(mlsl_handle, TRUE); 1128 1129 #ifdef TRACK_IDS 1130 result = test_get_mpu_id(mlsl_handle); 1131 if (result != INV_SUCCESS) { 1132 MPL_LOGI("Could not read the device's unique ID\n"); 1133 MPL_LOGI("\n"); 1134 return result; 1135 } 1136 #endif 1137 1138 /* adjust the gyro sensitivity according to the gyro_sens_trim value */ 1139 adjGyroSens = test_setup.gyro_sens * mputestCfgPtr->gyro_sens_trim / 131.f; 1140 test_setup.gyro_fs = (int)(32768.f / adjGyroSens); 1141 1142 /* collect gyro and temperature data */ 1143 #ifdef CONFIG_MPU_SENSORS_MPU3050 1144 result = inv_test_gyro_3050(mlsl_handle, 1145 gyro_biases, &temp_avg, provide_result); 1146 #else 1147 MPL_LOGW_IF(provide_result, 1148 "Self Test for MPU-6050 devices is for bias correction only: " 1149 "no test PASS/FAIL result will be provided\n"); 1150 result = inv_test_gyro_6050(mlsl_handle, gyro_biases, &temp_avg); 1151 #endif 1152 1153 MPL_LOGI("\n"); 1154 MPL_LOGI("Test time : %ld ms\n", inv_get_tick_count() - testStart); 1155 if (result) 1156 return result; 1157 1158 /* collect accel data. if this step is skipped, 1159 ensure the array still contains zeros. */ 1160 if (mputestCfgPtr->accel != NULL) { 1161 float fs; 1162 RANGE_FIXEDPOINT_TO_FLOAT(mputestCfgPtr->accel->range, fs); 1163 accelSens[0] = (long)(32768L / fs); 1164 accelSens[1] = (long)(32768L / fs); 1165 accelSens[2] = (long)(32768L / fs); 1166 #if defined CONFIG_MPU_SENSORS_MPU6050B1 1167 if (MPL_PROD_KEY(mputestCfgPtr->product_id, 1168 mputestCfgPtr->product_revision) == MPU_PRODUCT_KEY_B1_E1_5) { 1169 accelSens[2] /= 2; 1170 } else { 1171 unsigned short trim_corr = 16384 / mputestCfgPtr->accel_sens_trim; 1172 accelSens[0] /= trim_corr; 1173 accelSens[1] /= trim_corr; 1174 accelSens[2] /= trim_corr; 1175 } 1176 #endif 1177 } else { 1178 /* would be 0, but 1 to avoid divide-by-0 below */ 1179 accelSens[0] = accelSens[1] = accelSens[2] = 1; 1180 } 1181 #ifdef CONFIG_MPU_SENSORS_MPU3050 1182 result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2], 1183 provide_result); 1184 #else 1185 result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2], 1186 FALSE); 1187 #endif 1188 if (result) 1189 return result; 1190 1191 result = inv_device_power_mgmt(mlsl_handle, FALSE); 1192 if (result) 1193 return result; 1194 1195 ptr = 0; 1196 dataStore[ptr++] = 0; /* total len of factory cal */ 1197 dataStore[ptr++] = 0; 1198 dataStore[ptr++] = 0; 1199 dataStore[ptr++] = ML_INIT_CAL_LEN; 1200 dataStore[ptr++] = 0; 1201 dataStore[ptr++] = 5; /* record type 5 - initial calibration */ 1202 1203 tmp = temp_avg; /* temperature */ 1204 if (tmp < 0) tmp += 2 << 16; 1205 USHORT_TO_CHARS(&dataStore[ptr], tmp); 1206 ptr += 2; 1207 1208 /* NOTE : 2 * test_setup.gyro_fs == 65536 / (32768 / test_setup.gyro_fs) */ 1209 lltmp = (long)gyro_biases[0] * 2 * test_setup.gyro_fs; /* x gyro avg */ 1210 if (lltmp < 0) lltmp += 1LL << 32; 1211 UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); 1212 ptr += 4; 1213 lltmp = (long)gyro_biases[1] * 2 * test_setup.gyro_fs; /* y gyro avg */ 1214 if (lltmp < 0) lltmp += 1LL << 32; 1215 UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); 1216 ptr += 4; 1217 lltmp = (long)gyro_biases[2] * 2 * test_setup.gyro_fs; /* z gyro avg */ 1218 if (lltmp < 0) lltmp += 1LL << 32; 1219 UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); 1220 ptr += 4; 1221 1222 lltmp = (long)accel_biases[0] * 65536L / accelSens[0]; /* x accel avg */ 1223 if (lltmp < 0) lltmp += 1LL << 32; 1224 UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); 1225 ptr += 4; 1226 lltmp = (long)accel_biases[1] * 65536L / accelSens[1]; /* y accel avg */ 1227 if (lltmp < 0) lltmp += 1LL << 32; 1228 UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); 1229 ptr += 4; 1230 lltmp = (long)accel_biases[2] * 65536L / accelSens[2]; /* z accel avg */ 1231 if (lltmp < 0) lltmp += 1LL << 32; 1232 UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); 1233 ptr += 4; 1234 1235 /* add a checksum for data */ 1236 chk = inv_checksum( 1237 dataStore + INV_CAL_HDR_LEN, 1238 ML_INIT_CAL_LEN - INV_CAL_HDR_LEN - INV_CAL_CHK_LEN); 1239 UINT_TO_CHARS(&dataStore[ptr], chk); 1240 ptr += 4; 1241 1242 if (ptr != ML_INIT_CAL_LEN) { 1243 MPL_LOGI("Invalid calibration data length: exp %d, got %d\n", 1244 ML_INIT_CAL_LEN, ptr); 1245 return -1; 1246 } 1247 1248 return result; 1249 } 1250 1251 /** 1252 * @brief The main test API. Runs the MPU Self Test and, if successful, 1253 * stores the encoded initial calibration data on the final storage 1254 * medium of choice (cfr. inv_serial_write_cal() and the MLCAL_FILE 1255 * define in your mlsl implementation). 1256 * 1257 * @param mlsl_handle 1258 * serial interface handle to allow serial communication with the 1259 * device, both gyro and accelerometer. 1260 * @param provide_result 1261 * If 1: 1262 * perform and analyze the offset, drive frequency, and noise 1263 * calculation and compare it against set threshouds. Report 1264 * also the final result using a bit-mask like error code as 1265 * described in the inv_test_gyro() function. 1266 * When 0: 1267 * skip the noise and drive frequency calculation and pass/fail 1268 * assessment. It simply calculates the gyro and accel biases. 1269 * 1270 * @return 0 on success or a non-zero error code from the callees on error. 1271 */ 1272 inv_error_t inv_factory_calibrate(void *mlsl_handle, 1273 uint_fast8_t provide_result) 1274 { 1275 int result; 1276 1277 result = inv_mpu_test(mlsl_handle, provide_result); 1278 if (provide_result) { 1279 MPL_LOGI("\n"); 1280 if (result == 0) { 1281 MPL_LOGI("Test : PASSED\n"); 1282 } else { 1283 MPL_LOGI("Test : FAILED %d/%04X - Biases NOT stored\n", result, result); 1284 return result; /* abort writing the calibration if the 1285 test is not successful */ 1286 } 1287 MPL_LOGI("\n"); 1288 } else { 1289 MPL_LOGI("\n"); 1290 if (result) { 1291 LOG_RESULT_LOCATION(result); 1292 return result; 1293 } 1294 } 1295 1296 result = inv_serial_write_cal(dataStore, ML_INIT_CAL_LEN); 1297 if (result) { 1298 MPL_LOGI("Error : cannot write calibration on file - error %d\n", 1299 result); 1300 return result; 1301 } 1302 1303 return INV_SUCCESS; 1304 } 1305 1306 1307 1308 /* ----------------------------------------------------------------------- 1309 accel interface functions 1310 -----------------------------------------------------------------------*/ 1311 1312 /** 1313 * @internal 1314 * @brief Reads data for X, Y, and Z axis from the accelerometer device. 1315 * Used only if an accelerometer has been setup using the 1316 * inv_test_setup_accel() API. 1317 * Takes care of the accelerometer endianess according to how the 1318 * device has been described in the corresponding accelerometer driver 1319 * file. 1320 * @param mlsl_handle 1321 * serial interface handle to allow serial communication with the 1322 * device, both gyro and accelerometer. 1323 * @param slave 1324 * a pointer to the descriptor of the slave accelerometer device 1325 * in use. Contains the necessary information to operate, read, 1326 * and communicate with the accelerometer device of choice. 1327 * See the declaration of struct ext_slave_descr in mpu.h. 1328 * @param pdata 1329 * a pointer to the platform info of the slave accelerometer 1330 * device in use. Describes how the device is oriented and 1331 * mounted on host platform's PCB. 1332 * @param vals 1333 * output pointer to return the accelerometer's X, Y, and Z axis 1334 * sensor data collected. 1335 * @return 0 on success or a non-zero error code on error. 1336 */ 1337 static inv_error_t test_get_data( 1338 void *mlsl_handle, 1339 struct mldl_cfg *mputestCfgPtr, 1340 short *vals) 1341 { 1342 inv_error_t result; 1343 unsigned char data[20]; 1344 struct ext_slave_descr *slave = mputestCfgPtr->accel; 1345 #ifndef CONFIG_MPU_SENSORS_MPU3050 1346 struct ext_slave_platform_data *pdata = &mputestCfgPtr->pdata->accel; 1347 #endif 1348 1349 #ifdef CONFIG_MPU_SENSORS_MPU3050 1350 result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, 0x23, 1351 6, data); 1352 #else 1353 result = inv_serial_read(mlsl_handle, pdata->address, slave->read_reg, 1354 slave->read_len, data); 1355 #endif 1356 if (result) { 1357 LOG_RESULT_LOCATION(result); 1358 return result; 1359 } 1360 1361 if (VERBOSE_OUT) { 1362 MPL_LOGI("Accel : 0x%02X%02X 0x%02X%02X 0x%02X%02X (raw)\n", 1363 ACCEL_UNPACK(data)); 1364 } 1365 1366 if (CHECK_NACKS(data)) { 1367 MPL_LOGI("Error fetching data from the accelerometer : " 1368 "all bytes read 0xff\n"); 1369 return INV_ERROR_SERIAL_READ; 1370 } 1371 1372 if (slave->endian == EXT_SLAVE_BIG_ENDIAN) { 1373 vals[0] = CHARS_TO_SHORT(&data[0]); 1374 vals[1] = CHARS_TO_SHORT(&data[2]); 1375 vals[2] = CHARS_TO_SHORT(&data[4]); 1376 } else { 1377 vals[0] = CHARS_TO_SHORT_SWAPPED(&data[0]); 1378 vals[1] = CHARS_TO_SHORT_SWAPPED(&data[2]); 1379 vals[2] = CHARS_TO_SHORT_SWAPPED(&data[4]); 1380 } 1381 1382 if (VERBOSE_OUT) { 1383 MPL_LOGI("Accel : %+13d %+13d %+13d (LSB)\n", 1384 vals[0], vals[1], vals[2]); 1385 } 1386 return INV_SUCCESS; 1387 } 1388 1389 #ifdef __cplusplus 1390 } 1391 #endif 1392 1393 /** 1394 * @} 1395 */ 1396 1397
