diff options
Diffstat (limited to 'libsensors/mlsdk/mlutils/mputest.c')
| -rw-r--r-- | libsensors/mlsdk/mlutils/mputest.c | 1101 |
1 files changed, 0 insertions, 1101 deletions
diff --git a/libsensors/mlsdk/mlutils/mputest.c b/libsensors/mlsdk/mlutils/mputest.c deleted file mode 100644 index 4a598ff..0000000 --- a/libsensors/mlsdk/mlutils/mputest.c +++ /dev/null @@ -1,1101 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/****************************************************************************** - * - * $Id: mputest.c 5637 2011-06-14 01:13:53Z mcaramello $ - * - *****************************************************************************/ - -/* - * MPU Self Test functions - * Version 2.4 - * May 13th, 2011 - */ - -/** - * @defgroup MPU_SELF_TEST - * @brief MPU Self Test functions - * - * These functions provide an in-site test of the MPU 3xxx chips. The main - * entry point is the inv_mpu_test function. - * This runs the tests (as described in the accompanying documentation) and - * writes a configuration file containing initial calibration data. - * inv_mpu_test returns INV_SUCCESS if the chip passes the tests. - * Otherwise, an error code is returned. - * The functions in this file rely on MLSL and MLOS: refer to the MPL - * documentation for more information regarding the system interface - * files. - * - * @{ - * @file mputest.c - * @brief MPU Self Test routines for assessing gyro sensor status - * after surface mount has happened on the target host platform. - */ - -#include <stdio.h> -#include <time.h> -#include <string.h> -#include <math.h> -#include <stdlib.h> -#ifdef LINUX -#include <unistd.h> -#endif - -#include "mpu.h" -#include "mldl.h" -#include "mldl_cfg.h" -#include "accel.h" -#include "mlFIFO.h" -#include "slave.h" -#include "ml.h" -#include "ml_stored_data.h" -#include "checksum.h" - -#include "mlsl.h" -#include "mlos.h" - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-mpust" - -#ifdef __cplusplus -extern "C" { -#endif - -/* - Defines -*/ - -#define VERBOSE_OUT 0 - -/*#define TRACK_IDS*/ - -/*--- Test parameters defaults. See set_test_parameters for more details ---*/ - -#define DEF_MPU_ADDR (0x68) /* I2C address of the mpu */ - -#if (USE_SENSE_PATH_TEST == 1) /* gyro full scale dps */ -#define DEF_GYRO_FULLSCALE (2000) -#else -#define DEF_GYRO_FULLSCALE (250) -#endif - -#define DEF_GYRO_SENS (32768.f / DEF_GYRO_FULLSCALE) - /* gyro sensitivity LSB/dps */ -#define DEF_PACKET_THRESH (75) /* 600 ms / 8ms / sample */ -#define DEF_TOTAL_TIMING_TOL (.03f) /* 3% = 2 pkts + 1% proc tol. */ -#define DEF_BIAS_THRESH (40 * DEF_GYRO_SENS) - /* 40 dps in LSBs */ -#define DEF_RMS_THRESH (0.4f * DEF_GYRO_SENS) - /* 0.4 dps-rms in LSB-rms */ -#define DEF_SP_SHIFT_THRESH_CUST (.05f) /* 5% */ -#define DEF_TEST_TIME_PER_AXIS (600) /* ms of time spent collecting - data for each axis, - multiple of 600ms */ -#define DEF_N_ACCEL_SAMPLES (20) /* num of accel samples to - average from, if applic. */ - -#define ML_INIT_CAL_LEN (36) /* length in bytes of - calibration data file */ - -/* - Macros -*/ - -#define CHECK_TEST_ERROR(x) \ - if (x) { \ - MPL_LOGI("error %d @ %s|%d\n", x, __func__, __LINE__); \ - return (-1); \ - } - -#define SHORT_TO_TEMP_C(shrt) (((shrt+13200.f)/280.f)+35.f) - -#define USHORT_TO_CHARS(chr,shrt) (chr)[0]=(unsigned char)(shrt>>8); \ - (chr)[1]=(unsigned char)(shrt); - -#define UINT_TO_CHARS(chr,ui) (chr)[0]=(unsigned char)(ui>>24); \ - (chr)[1]=(unsigned char)(ui>>16); \ - (chr)[2]=(unsigned char)(ui>>8); \ - (chr)[3]=(unsigned char)(ui); - -#define FLOAT_TO_SHORT(f) ( \ - (fabs(f-(short)f)>=0.5) ? ( \ - ((short)f)+(f<0?(-1):(+1))) : \ - ((short)f) \ - ) - -#define CHARS_TO_SHORT(d) ((((short)(d)[0])<<8)+(d)[1]) -#define CHARS_TO_SHORT_SWAPPED(d) ((((short)(d)[1])<<8)+(d)[0]) - -#define ACCEL_UNPACK(d) d[0], d[1], d[2], d[3], d[4], d[5] - -#define CHECK_NACKS(d) ( \ - d[0]==0xff && d[1]==0xff && \ - d[2]==0xff && d[3]==0xff && \ - d[4]==0xff && d[5]==0xff \ - ) - -/* - Prototypes -*/ - -static inv_error_t test_get_data( - void *mlsl_handle, - struct mldl_cfg *mputestCfgPtr, - short *vals); - -/* - Types -*/ -typedef struct { - float gyro_sens; - int gyro_fs; - int packet_thresh; - float total_timing_tol; - int bias_thresh; - float rms_threshSq; - float sp_shift_thresh; - unsigned int test_time_per_axis; - unsigned short accel_samples; -} tTestSetup; - -/* - Global variables -*/ -static unsigned char dataout[20]; -static unsigned char dataStore[ML_INIT_CAL_LEN]; - -static tTestSetup test_setup = { - DEF_GYRO_SENS, - DEF_GYRO_FULLSCALE, - DEF_PACKET_THRESH, - DEF_TOTAL_TIMING_TOL, - (int)DEF_BIAS_THRESH, - DEF_RMS_THRESH * DEF_RMS_THRESH, - DEF_SP_SHIFT_THRESH_CUST, - DEF_TEST_TIME_PER_AXIS, - DEF_N_ACCEL_SAMPLES -}; - -static float adjGyroSens; -static char a_name[3][2] = {"X", "Y", "Z"}; - -/* - NOTE : modify get_slave_descr parameter below to reflect - the DEFAULT accelerometer in use. The accelerometer in use - can be modified at run-time using the inv_test_setup_accel API. - NOTE : modify the expected z axis orientation (Z axis pointing - upward or downward) -*/ - -signed char g_z_sign = +1; -struct mldl_cfg *mputestCfgPtr = NULL; - -#ifndef LINUX -/** - * @internal - * @brief usec precision sleep function. - * @param number of micro seconds (us) to sleep for. - */ -static void usleep(unsigned long t) -{ - unsigned long start = inv_get_tick_count(); - while (inv_get_tick_count()-start < t / 1000); -} -#endif - -/** - * @brief Modify the self test limits from their default values. - * - * @param slave_addr - * the slave address the MPU device is setup to respond at. - * The default is DEF_MPU_ADDR = 0x68. - * @param sensitivity - * the read sensitivity of the device in LSB/dps as it is trimmed. - * NOTE : if using the self test as part of the MPL, the - * sensitivity the different sensitivity trims are already - * taken care of. - * @param p_thresh - * number of packets expected to be received in a 600 ms period. - * Depends on the sampling frequency of choice (set by default to - * 125 Hz) and low pass filter cut-off frequency selection (set - * to 42 Hz). - * The default is DEF_PACKET_THRESH = 75 packets. - * @param total_time_tol - * time skew tolerance, taking into account imprecision in turning - * the FIFO on and off and the processor time imprecision (for - * 1 GHz processor). - * The default is DEF_TOTAL_TIMING_TOL = 3 %, about 2 packets. - * @param bias_thresh - * bias level threshold, the maximun acceptable no motion bias - * for a production quality part. - * The default is DEF_BIAS_THRESH = 40 dps. - * @param rms_thresh - * the limit standard deviation (=~ RMS) set to assess whether - * the noise level on the part is acceptable. - * The default is DEF_RMS_THRESH = 0.2 dps-rms. - * @param sp_shift_thresh - * the limit shift applicable to the Sense Path self test - * calculation. - */ -void inv_set_test_parameters(unsigned int slave_addr, float sensitivity, - int p_thresh, float total_time_tol, - int bias_thresh, float rms_thresh, - float sp_shift_thresh, - unsigned short accel_samples) -{ - mputestCfgPtr->addr = slave_addr; - test_setup.gyro_sens = sensitivity; - test_setup.gyro_fs = (int)(32768.f / sensitivity); - test_setup.packet_thresh = p_thresh; - test_setup.total_timing_tol = total_time_tol; - test_setup.bias_thresh = bias_thresh; - test_setup.rms_threshSq = rms_thresh * rms_thresh; - test_setup.sp_shift_thresh = sp_shift_thresh; - test_setup.accel_samples = accel_samples; -} - -#define X (0) -#define Y (1) -#define Z (2) - -/** - * @brief Test the gyroscope sensor. - * Implements the core logic of the MPU Self Test. - * Produces the PASS/FAIL result. Loads the calculated gyro biases - * and temperature datum into the corresponding pointers. - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @param gyro_biases - * output pointer to store the initial bias calculation provided - * by the MPU Self Test. Requires 3 elements for gyro X, Y, - * and Z. - * @param temp_avg - * output pointer to store the initial average temperature as - * provided by the MPU Self Test. - * @param perform_full_test - * If 1: - * calculates offset, drive frequency, and noise and compare it - * against set thresholds. - * Report also the final result using a bit-mask like error code - * as explained in return value description. - * When 0: - * skip the noise and drive frequency calculation and pass/fail - * assessment; simply calculates the gyro biases. - * - * @return 0 on success. - * On error, the return value is a bitmask representing: - * 0, 1, 2 Failures with PLLs on X, Y, Z gyros respectively - * (decimal values will be 1, 2, 4 respectively). - * 3, 4, 5 Excessive offset with X, Y, Z gyros respectively - * (decimal values will be 8, 16, 32 respectively). - * 6, 7, 8 Excessive noise with X, Y, Z gyros respectively - * (decimal values will be 64, 128, 256 respectively). - * 9 If any of the RMS noise values is zero, it could be - * due to a non-functional gyro or FIFO/register failure. - * (decimal value will be 512). - * (decimal values will be 1024, 2048, 4096 respectively). - */ -int inv_test_gyro_3050(void *mlsl_handle, - short gyro_biases[3], short *temp_avg, - uint_fast8_t perform_full_test) -{ - int retVal = 0; - inv_error_t result; - - int total_count = 0; - int total_count_axis[3] = {0, 0, 0}; - int packet_count; - short x[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; - short y[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; - short z[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; - unsigned char regs[7]; - - int temperature; - float Avg[3]; - float RMS[3]; - int i, j, tmp; - char tmpStr[200]; - - temperature = 0; - - /* sample rate = 8ms */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_SMPLRT_DIV, 0x07); - CHECK_TEST_ERROR(result); - - regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */ - switch (DEF_GYRO_FULLSCALE) { - case 2000: - regs[0] |= 0x18; - break; - case 1000: - regs[0] |= 0x10; - break; - case 500: - regs[0] |= 0x08; - break; - case 250: - default: - regs[0] |= 0x00; - break; - } - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_DLPF_FS_SYNC, regs[0]); - CHECK_TEST_ERROR(result); - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_INT_CFG, 0x00); - CHECK_TEST_ERROR(result); - - /* 1st, timing test */ - for (j = 0; j < 3; j++) { - - MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]); - - /* turn on all gyros, use gyro X for clocking - Set to Y and Z for 2nd and 3rd iteration */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGM, j + 1); - CHECK_TEST_ERROR(result); - - /* wait for 2 ms after switching clock source */ - usleep(2000); - - /* we will enable XYZ gyro in FIFO and nothing else */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_FIFO_EN2, 0x00); - CHECK_TEST_ERROR(result); - /* enable/reset FIFO */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST); - CHECK_TEST_ERROR(result); - - tmp = (int)(test_setup.test_time_per_axis / 600); - while (tmp-- > 0) { - /* enable XYZ gyro in FIFO and nothing else */ - result = inv_serial_single_write(mlsl_handle, - mputestCfgPtr->addr, MPUREG_FIFO_EN1, - BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT); - CHECK_TEST_ERROR(result); - - /* wait for 600 ms for data */ - usleep(600000); - - /* stop storing gyro in the FIFO */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_FIFO_EN1, 0x00); - CHECK_TEST_ERROR(result); - - /* Getting number of bytes in FIFO */ - result = inv_serial_read( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_FIFO_COUNTH, 2, dataout); - CHECK_TEST_ERROR(result); - /* number of 6 B packets in the FIFO */ - packet_count = CHARS_TO_SHORT(dataout) / 6; - sprintf(tmpStr, "Packet Count: %d - ", packet_count); - - if ( abs(packet_count - test_setup.packet_thresh) - <= /* Within +/- total_timing_tol % range */ - test_setup.total_timing_tol * test_setup.packet_thresh) { - for (i = 0; i < packet_count; i++) { - /* getting FIFO data */ - result = inv_serial_read_fifo(mlsl_handle, - mputestCfgPtr->addr, 6, dataout); - CHECK_TEST_ERROR(result); - x[total_count + i] = CHARS_TO_SHORT(&dataout[0]); - y[total_count + i] = CHARS_TO_SHORT(&dataout[2]); - z[total_count + i] = CHARS_TO_SHORT(&dataout[4]); - if (VERBOSE_OUT) { - MPL_LOGI("Gyros %-4d : %+13d %+13d %+13d\n", - total_count + i, x[total_count + i], - y[total_count + i], z[total_count + i]); - } - } - total_count += packet_count; - total_count_axis[j] += packet_count; - sprintf(tmpStr, "%sOK", tmpStr); - } else { - if (perform_full_test) - retVal |= 1 << j; - sprintf(tmpStr, "%sNOK - samples ignored", tmpStr); - } - MPL_LOGI("%s\n", tmpStr); - } - - /* remove gyros from FIFO */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_FIFO_EN1, 0x00); - CHECK_TEST_ERROR(result); - - /* Read Temperature */ - result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, - MPUREG_TEMP_OUT_H, 2, dataout); - CHECK_TEST_ERROR(result); - temperature += (short)CHARS_TO_SHORT(dataout); - } - - MPL_LOGI("\n"); - MPL_LOGI("Total %d samples\n", total_count); - MPL_LOGI("\n"); - - /* 2nd, check bias from X and Y PLL clock source */ - tmp = total_count != 0 ? total_count : 1; - for (i = 0, - Avg[X] = .0f, Avg[Y] = .0f, Avg[Z] = .0f; - i < total_count; i++) { - Avg[X] += 1.f * x[i] / tmp; - Avg[Y] += 1.f * y[i] / tmp; - Avg[Z] += 1.f * z[i] / tmp; - } - MPL_LOGI("bias : %+13.3f %+13.3f %+13.3f (LSB)\n", - Avg[X], Avg[Y], Avg[Z]); - if (VERBOSE_OUT) { - MPL_LOGI(" : %+13.3f %+13.3f %+13.3f (dps)\n", - Avg[X] / adjGyroSens, - Avg[Y] / adjGyroSens, - Avg[Z] / adjGyroSens); - } - if(perform_full_test) { - for (j = 0; j < 3; j++) { - if (fabs(Avg[j]) > test_setup.bias_thresh) { - MPL_LOGI("%s-Gyro bias (%.0f) exceeded threshold " - "(threshold = %d)\n", - a_name[j], Avg[j], test_setup.bias_thresh); - retVal |= 1 << (3+j); - } - } - } - - /* 3rd, check RMS */ - if (perform_full_test) { - for (i = 0, - RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f; - i < total_count; i++) { - RMS[X] += (x[i] - Avg[X]) * (x[i] - Avg[X]); - RMS[Y] += (y[i] - Avg[Y]) * (y[i] - Avg[Y]); - RMS[Z] += (z[i] - Avg[Z]) * (z[i] - Avg[Z]); - } - for (j = 0; j < 3; j++) { - if (RMS[j] > test_setup.rms_threshSq * total_count) { - MPL_LOGI("%s-Gyro RMS (%.2f) exceeded threshold " - "(threshold = %.2f)\n", - a_name[j], sqrt(RMS[j] / total_count), - sqrt(test_setup.rms_threshSq)); - retVal |= 1 << (6+j); - } - } - - MPL_LOGI("RMS : %+13.3f %+13.3f %+13.3f (LSB-rms)\n", - sqrt(RMS[X] / total_count), - sqrt(RMS[Y] / total_count), - sqrt(RMS[Z] / total_count)); - if (VERBOSE_OUT) { - MPL_LOGI("RMS ^ 2 : %+13.3f %+13.3f %+13.3f\n", - RMS[X] / total_count, - RMS[Y] / total_count, - RMS[Z] / total_count); - } - - if (RMS[X] == 0 || RMS[Y] == 0 || RMS[Z] == 0) { - /* If any of the RMS noise value returns zero, - then we might have dead gyro or FIFO/register failure, - the part is sleeping, or the part is not responsive */ - retVal |= 1 << 9; - } - } - - /* 4th, temperature average */ - temperature /= 3; - if (VERBOSE_OUT) - MPL_LOGI("Temperature : %+13.3f %13s %13s (deg. C)\n", - SHORT_TO_TEMP_C(temperature), "", ""); - - /* load into final storage */ - *temp_avg = (short)temperature; - gyro_biases[X] = FLOAT_TO_SHORT(Avg[X]); - gyro_biases[Y] = FLOAT_TO_SHORT(Avg[Y]); - gyro_biases[Z] = FLOAT_TO_SHORT(Avg[Z]); - - return retVal; -} - -#ifdef TRACK_IDS -/** - * @internal - * @brief Retrieve the unique MPU device identifier from the internal OTP - * bank 0 memory. - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @return 0 on success, a non-zero error code from the serial layer on error. - */ -static inv_error_t test_get_mpu_id(void *mlsl_handle) -{ - inv_error_t result; - unsigned char otp0[8]; - - - result = - inv_serial_read_mem(mlsl_handle, mputestCfgPtr->addr, - (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0) << 8 | - 0x00, 6, otp0); - if (result) - goto close; - - MPL_LOGI("\n"); - MPL_LOGI("DIE_ID : %06X\n", - ((int)otp0[1] << 8 | otp0[0]) & 0x1fff); - MPL_LOGI("WAFER_ID : %06X\n", - (((int)otp0[2] << 8 | otp0[1]) & 0x03ff ) >> 5); - MPL_LOGI("A_LOT_ID : %06X\n", - ( ((int)otp0[4] << 16 | (int)otp0[3] << 8 | - otp0[2]) & 0x3ffff) >> 2); - MPL_LOGI("W_LOT_ID : %06X\n", - ( ((int)otp0[5] << 8 | otp0[4]) & 0x3fff) >> 2); - MPL_LOGI("WP_ID : %06X\n", - ( ((int)otp0[6] << 8 | otp0[5]) & 0x03ff) >> 7); - MPL_LOGI("REV_ID : %06X\n", otp0[6] >> 2); - MPL_LOGI("\n"); - -close: - result = - inv_serial_single_write(mlsl_handle, mputestCfgPtr->addr, MPUREG_BANK_SEL, 0x00); - return result; -} -#endif /* TRACK_IDS */ - -/** - * @brief If requested via inv_test_setup_accel(), test the accelerometer biases - * and calculate the necessary bias correction. - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @param bias - * output pointer to store the initial bias calculation provided - * by the MPU Self Test. Requires 3 elements to store accel X, Y, - * and Z axis bias. - * @param perform_full_test - * If 1: - * calculates offsets and noise and compare it against set - * thresholds. The final exist status will reflect if any of the - * value is outside of the expected range. - * When 0; - * skip the noise calculation and pass/fail assessment; simply - * calculates the accel biases. - * - * @return 0 on success. A non-zero error code on error. - */ -int inv_test_accel(void *mlsl_handle, - short *bias, long gravity, - uint_fast8_t perform_full_test) -{ - int i; - - short *p_vals; - float x = 0.f, y = 0.f, z = 0.f, zg = 0.f; - float RMS[3]; - float accelRmsThresh = 1000000.f; /* enourmous so that the test always - passes - future deployment */ - unsigned short res; - unsigned long orig_requested_sensors; - struct mpu_platform_data *mputestPData = mputestCfgPtr->pdata; - - p_vals = (short*)inv_malloc(sizeof(short) * 3 * test_setup.accel_samples); - - /* load the slave descr from the getter */ - if (mputestPData->accel.get_slave_descr == NULL) { - MPL_LOGI("\n"); - MPL_LOGI("No accelerometer configured\n"); - return 0; - } - if (mputestCfgPtr->accel == NULL) { - MPL_LOGI("\n"); - MPL_LOGI("No accelerometer configured\n"); - return 0; - } - - /* resume the accel */ - orig_requested_sensors = mputestCfgPtr->requested_sensors; - mputestCfgPtr->requested_sensors = INV_THREE_AXIS_ACCEL | INV_THREE_AXIS_GYRO; - res = inv_mpu_resume(mputestCfgPtr, - mlsl_handle, NULL, NULL, NULL, - mputestCfgPtr->requested_sensors); - if(res != INV_SUCCESS) - goto accel_error; - - /* wait at least a sample cycle for the - accel data to be retrieved by MPU */ - inv_sleep(inv_mpu_get_sampling_period_us(mputestCfgPtr) / 1000); - - /* collect the samples */ - for(i = 0; i < test_setup.accel_samples; i++) { - short *vals = &p_vals[3 * i]; - if (test_get_data(mlsl_handle, mputestCfgPtr, vals)) { - goto accel_error; - } - x += 1.f * vals[X] / test_setup.accel_samples; - y += 1.f * vals[Y] / test_setup.accel_samples; - z += 1.f * vals[Z] / test_setup.accel_samples; - } - - mputestCfgPtr->requested_sensors = orig_requested_sensors; - res = inv_mpu_suspend(mputestCfgPtr, - mlsl_handle, NULL, NULL, NULL, - INV_ALL_SENSORS); - if (res != INV_SUCCESS) - goto accel_error; - - MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (LSB)\n", x, y, z); - if (VERBOSE_OUT) { - MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (gee)\n", - x / gravity, y / gravity, z / gravity); - } - - bias[0] = FLOAT_TO_SHORT(x); - bias[1] = FLOAT_TO_SHORT(y); - zg = z - g_z_sign * gravity; - bias[2] = FLOAT_TO_SHORT(zg); - - MPL_LOGI("Accel correct.: %+13d %+13d %+13d (LSB)\n", - bias[0], bias[1], bias[2]); - if (VERBOSE_OUT) { - MPL_LOGI("Accel correct.: " - "%+13.3f %+13.3f %+13.3f (gee)\n", - 1.f * bias[0] / gravity, - 1.f * bias[1] / gravity, - 1.f * bias[2] / gravity); - } - - if (perform_full_test) { - /* accel RMS - for now the threshold is only indicative */ - for (i = 0, - RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f; - i < test_setup.accel_samples; i++) { - short *vals = &p_vals[3 * i]; - RMS[X] += (vals[X] - x) * (vals[X] - x); - RMS[Y] += (vals[Y] - y) * (vals[Y] - y); - RMS[Z] += (vals[Z] - z) * (vals[Z] - z); - } - for (i = 0; i < 3; i++) { - if (RMS[i] > accelRmsThresh * accelRmsThresh - * test_setup.accel_samples) { - MPL_LOGI("%s-Accel RMS (%.2f) exceeded threshold " - "(threshold = %.2f)\n", - a_name[i], sqrt(RMS[i] / test_setup.accel_samples), - accelRmsThresh); - goto accel_error; - } - } - MPL_LOGI("RMS : %+13.3f %+13.3f %+13.3f (LSB-rms)\n", - sqrt(RMS[X] / DEF_N_ACCEL_SAMPLES), - sqrt(RMS[Y] / DEF_N_ACCEL_SAMPLES), - sqrt(RMS[Z] / DEF_N_ACCEL_SAMPLES)); - } - - return 0; /* success */ - -accel_error: /* error */ - bias[0] = bias[1] = bias[2] = 0; - return 1; -} - -/** - * @brief an user-space substitute of the power management function(s) - * in mldl_cfg.c for self test usage. - * Wake up and sleep the device, whether that is MPU3050, MPU6050A2, - * or MPU6050B1. - * @param mlsl_handle - * a file handle for the serial communication port used to - * communicate with the MPU device. - * @param power_level - * the power state to change the device into. Currently only 0 or - * 1 are supported, for sleep and full-power respectively. - * @return 0 on success; a non-zero error code on error. - */ -static inv_error_t inv_device_power_mgmt(void *mlsl_handle, - uint_fast8_t power_level) -{ - inv_error_t result; - static unsigned char pwr_mgm; - unsigned char b; - - if (power_level != 0 && power_level != 1) { - return INV_ERROR_INVALID_PARAMETER; - } - - if (power_level) { - result = inv_serial_read( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGM, 1, &pwr_mgm); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - /* reset */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGM, pwr_mgm | BIT_H_RESET); - - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - inv_sleep(5); - - /* re-read power mgmt reg - - it may have reset after H_RESET is applied */ - result = inv_serial_read( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGM, 1, &b); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - /* wake up */ - if (pwr_mgm & BIT_SLEEP) { - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGM, 0x00); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - - inv_sleep(60); - } else { - /* restore the power state the part was found in */ - if (pwr_mgm & BIT_SLEEP) { - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGM, pwr_mgm); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - } - - return INV_SUCCESS; -} - -/** - * @brief The main entry point of the MPU Self Test, triggering the run of - * the single tests, for gyros and accelerometers. - * Prepares the MPU for the test, taking the device out of low power - * state if necessary, switching the MPU secondary I2C interface into - * bypass mode and restoring the original power state at the end of - * the test. - * This function is also responsible for encoding the output of each - * test in the correct format as it is stored on the file/medium of - * choice (according to inv_serial_write_cal() function). - * The format needs to stay perfectly consistent with the one expected - * by the corresponding loader in ml_stored_data.c; currectly the - * loaded in use is inv_load_cal_V1 (record type 1 - initial - * calibration). - * - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @param provide_result - * If 1: - * perform and analyze the offset, drive frequency, and noise - * calculation and compare it against set threshouds. Report - * also the final result using a bit-mask like error code as - * described in the inv_test_gyro() function. - * When 0: - * skip the noise and drive frequency calculation and pass/fail - * assessment. It simply calculates the gyro and accel biases. - * NOTE: for MPU6050 devices, this parameter is currently - * ignored. - * - * @return 0 on success. A non-zero error code on error. - * Propagates the errors from the tests up to the caller. - */ -int inv_mpu_test(void *mlsl_handle, uint_fast8_t provide_result) -{ - int result = 0; - - short temp_avg; - short gyro_biases[3] = {0, 0, 0}; - short accel_biases[3] = {0, 0, 0}; - - unsigned long testStart = inv_get_tick_count(); - long accelSens[3] = {0}; - int ptr; - int tmp; - long long lltmp; - uint32_t chk; - - MPL_LOGI("Collecting %d groups of 600 ms samples for each axis\n", - DEF_TEST_TIME_PER_AXIS / 600); - MPL_LOGI("\n"); - - result = inv_device_power_mgmt(mlsl_handle, TRUE); - -#ifdef TRACK_IDS - result = test_get_mpu_id(mlsl_handle); - if (result != INV_SUCCESS) { - MPL_LOGI("Could not read the device's unique ID\n"); - MPL_LOGI("\n"); - return result; - } -#endif - - /* adjust the gyro sensitivity according to the gyro_sens_trim value */ - adjGyroSens = test_setup.gyro_sens * mputestCfgPtr->gyro_sens_trim / 131.f; - test_setup.gyro_fs = (int)(32768.f / adjGyroSens); - - /* collect gyro and temperature data */ - result = inv_test_gyro_3050(mlsl_handle, - gyro_biases, &temp_avg, provide_result); - - MPL_LOGI("\n"); - MPL_LOGI("Test time : %ld ms\n", inv_get_tick_count() - testStart); - if (result) - return result; - - /* collect accel data. if this step is skipped, - ensure the array still contains zeros. */ - if (mputestCfgPtr->accel != NULL) { - float fs; - RANGE_FIXEDPOINT_TO_FLOAT(mputestCfgPtr->accel->range, fs); - accelSens[0] = (long)(32768L / fs); - accelSens[1] = (long)(32768L / fs); - accelSens[2] = (long)(32768L / fs); - } else { - /* would be 0, but 1 to avoid divide-by-0 below */ - accelSens[0] = accelSens[1] = accelSens[2] = 1; - } - result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2], - provide_result); - if (result) - return result; - - result = inv_device_power_mgmt(mlsl_handle, FALSE); - if (result) - return result; - - ptr = 0; - dataStore[ptr++] = 0; /* total len of factory cal */ - dataStore[ptr++] = 0; - dataStore[ptr++] = 0; - dataStore[ptr++] = ML_INIT_CAL_LEN; - dataStore[ptr++] = 0; - dataStore[ptr++] = 5; /* record type 5 - initial calibration */ - - tmp = temp_avg; /* temperature */ - if (tmp < 0) tmp += 2 << 16; - USHORT_TO_CHARS(&dataStore[ptr], tmp); - ptr += 2; - - /* NOTE : 2 * test_setup.gyro_fs == 65536 / (32768 / test_setup.gyro_fs) */ - lltmp = (long)gyro_biases[0] * 2 * test_setup.gyro_fs; /* x gyro avg */ - if (lltmp < 0) lltmp += 1LL << 32; - UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); - ptr += 4; - lltmp = (long)gyro_biases[1] * 2 * test_setup.gyro_fs; /* y gyro avg */ - if (lltmp < 0) lltmp += 1LL << 32; - UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); - ptr += 4; - lltmp = (long)gyro_biases[2] * 2 * test_setup.gyro_fs; /* z gyro avg */ - if (lltmp < 0) lltmp += 1LL << 32; - UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); - ptr += 4; - - lltmp = (long)accel_biases[0] * 65536L / accelSens[0]; /* x accel avg */ - if (lltmp < 0) lltmp += 1LL << 32; - UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); - ptr += 4; - lltmp = (long)accel_biases[1] * 65536L / accelSens[1]; /* y accel avg */ - if (lltmp < 0) lltmp += 1LL << 32; - UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); - ptr += 4; - lltmp = (long)accel_biases[2] * 65536L / accelSens[2]; /* z accel avg */ - if (lltmp < 0) lltmp += 1LL << 32; - UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); - ptr += 4; - - /* add a checksum for data */ - chk = inv_checksum( - dataStore + INV_CAL_HDR_LEN, - ML_INIT_CAL_LEN - INV_CAL_HDR_LEN - INV_CAL_CHK_LEN); - UINT_TO_CHARS(&dataStore[ptr], chk); - ptr += 4; - - if (ptr != ML_INIT_CAL_LEN) { - MPL_LOGI("Invalid calibration data length: exp %d, got %d\n", - ML_INIT_CAL_LEN, ptr); - return -1; - } - - return result; -} - -/** - * @brief The main test API. Runs the MPU Self Test and, if successful, - * stores the encoded initial calibration data on the final storage - * medium of choice (cfr. inv_serial_write_cal() and the MLCAL_FILE - * define in your mlsl implementation). - * - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @param provide_result - * If 1: - * perform and analyze the offset, drive frequency, and noise - * calculation and compare it against set threshouds. Report - * also the final result using a bit-mask like error code as - * described in the inv_test_gyro() function. - * When 0: - * skip the noise and drive frequency calculation and pass/fail - * assessment. It simply calculates the gyro and accel biases. - * - * @return 0 on success or a non-zero error code from the callees on error. - */ -inv_error_t inv_factory_calibrate(void *mlsl_handle, - uint_fast8_t provide_result) -{ - int result; - - result = inv_mpu_test(mlsl_handle, provide_result); - if (provide_result) { - MPL_LOGI("\n"); - if (result == 0) { - MPL_LOGI("Test : PASSED\n"); - } else { - MPL_LOGI("Test : FAILED %d/%04X - Biases NOT stored\n", result, result); - return result; /* abort writing the calibration if the - test is not successful */ - } - MPL_LOGI("\n"); - } else { - MPL_LOGI("\n"); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - - result = inv_serial_write_cal(dataStore, ML_INIT_CAL_LEN); - if (result) { - MPL_LOGI("Error : cannot write calibration on file - error %d\n", - result); - return result; - } - - return INV_SUCCESS; -} - - - -/* ----------------------------------------------------------------------- - accel interface functions - -----------------------------------------------------------------------*/ - -/** - * @internal - * @brief Reads data for X, Y, and Z axis from the accelerometer device. - * Used only if an accelerometer has been setup using the - * inv_test_setup_accel() API. - * Takes care of the accelerometer endianess according to how the - * device has been described in the corresponding accelerometer driver - * file. - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @param slave - * a pointer to the descriptor of the slave accelerometer device - * in use. Contains the necessary information to operate, read, - * and communicate with the accelerometer device of choice. - * See the declaration of struct ext_slave_descr in mpu.h. - * @param pdata - * a pointer to the platform info of the slave accelerometer - * device in use. Describes how the device is oriented and - * mounted on host platform's PCB. - * @param vals - * output pointer to return the accelerometer's X, Y, and Z axis - * sensor data collected. - * @return 0 on success or a non-zero error code on error. - */ -static inv_error_t test_get_data( - void *mlsl_handle, - struct mldl_cfg *mputestCfgPtr, - short *vals) -{ - inv_error_t result; - unsigned char data[20]; - struct ext_slave_descr *slave = mputestCfgPtr->accel; - - result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, 0x23, - 6, data); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - if (VERBOSE_OUT) { - MPL_LOGI("Accel : 0x%02X%02X 0x%02X%02X 0x%02X%02X (raw)\n", - ACCEL_UNPACK(data)); - } - - if (CHECK_NACKS(data)) { - MPL_LOGI("Error fetching data from the accelerometer : " - "all bytes read 0xff\n"); - return INV_ERROR_SERIAL_READ; - } - - if (slave->endian == EXT_SLAVE_BIG_ENDIAN) { - vals[0] = CHARS_TO_SHORT(&data[0]); - vals[1] = CHARS_TO_SHORT(&data[2]); - vals[2] = CHARS_TO_SHORT(&data[4]); - } else { - vals[0] = CHARS_TO_SHORT_SWAPPED(&data[0]); - vals[1] = CHARS_TO_SHORT_SWAPPED(&data[2]); - vals[2] = CHARS_TO_SHORT_SWAPPED(&data[4]); - } - - if (VERBOSE_OUT) { - MPL_LOGI("Accel : %+13d %+13d %+13d (LSB)\n", - vals[0], vals[1], vals[2]); - } - return INV_SUCCESS; -} - -#ifdef __cplusplus -} -#endif - -/** - * @} - */ - |