Jean-Baptiste Queru | 4233185 | 2011-08-01 10:20:23 -0700 | [diff] [blame] | 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 | |