| /* |
| $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) |
| |
| #ifdef CONFIG_MPU_SENSORS_MPU3050 |
| /** |
| * @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; |
| } |
| |
| #else /* CONFIG_MPU_SENSORS_MPU3050 */ |
| |
| /** |
| * @brief Test the gyroscope sensor. |
| * Implements the core logic of the MPU Self Test but does not provide |
| * a PASS/FAIL output as in the MPU-3050 implementation. |
| * @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. |
| * |
| * @return 0 on success. |
| * A non-zero error code on error. |
| */ |
| int inv_test_gyro_6050(void *mlsl_handle, |
| short gyro_biases[3], short *temp_avg) |
| { |
| 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 = 0; |
| float Avg[3]; |
| int i, j, tmp; |
| char tmpStr[200]; |
| |
| /* sample rate = 8ms */ |
| result = inv_serial_single_write( |
| mlsl_handle, mputestCfgPtr->addr, |
| MPUREG_SMPLRT_DIV, 0x07); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return 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_CONFIG, regs[0]); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| result = inv_serial_single_write( |
| mlsl_handle, mputestCfgPtr->addr, |
| MPUREG_INT_ENABLE, 0x00); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return 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 */ |
| #ifdef CONFIG_MPU_SENSORS_MPU6050A2 |
| result = inv_serial_single_write( |
| mlsl_handle, mputestCfgPtr->addr, |
| MPUREG_PWR_MGMT_1, BITS_PWRSEL | (j + 1)); |
| #else |
| result = inv_serial_single_write( |
| mlsl_handle, mputestCfgPtr->addr, |
| MPUREG_PWR_MGMT_1, j + 1); |
| #endif |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| |
| /* wait for 2 ms after switching clock source */ |
| usleep(2000); |
| |
| /* enable/reset FIFO */ |
| result = inv_serial_single_write( |
| mlsl_handle, mputestCfgPtr->addr, |
| MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return 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_EN, |
| BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return 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_EN, 0x00); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| /* Getting number of bytes in FIFO */ |
| result = inv_serial_read( |
| mlsl_handle, mputestCfgPtr->addr, |
| MPUREG_FIFO_COUNTH, 2, dataout); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return 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); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return 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 { |
| 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_EN, 0x00); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| |
| /* Read Temperature */ |
| result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, |
| MPUREG_TEMP_OUT_H, 2, dataout); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return 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); |
| } |
| |
| 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 INV_SUCCESS; |
| } |
| |
| #endif /* CONFIG_MPU_SENSORS_MPU3050 */ |
| |
| #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); |
| #ifndef CONFIG_MPU_SENSORS_MPU6050A2 |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| #endif |
| 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 */ |
| #ifdef CONFIG_MPU_SENSORS_MPU6050A2 |
| if ((b & BITS_PWRSEL) != BITS_PWRSEL) { |
| result = inv_serial_single_write( |
| mlsl_handle, mputestCfgPtr->addr, |
| MPUREG_PWR_MGM, BITS_PWRSEL); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| } |
| #else |
| 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; |
| } |
| } |
| #endif |
| inv_sleep(60); |
| |
| #if defined(CONFIG_MPU_SENSORS_MPU6050A2) || \ |
| defined(CONFIG_MPU_SENSORS_MPU6050B1) |
| result = inv_serial_single_write( |
| mlsl_handle, mputestCfgPtr->addr, |
| MPUREG_INT_PIN_CFG, BIT_BYPASS_EN); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| #endif |
| } else { |
| /* restore the power state the part was found in */ |
| #ifdef CONFIG_MPU_SENSORS_MPU6050A2 |
| if ((pwr_mgm & BITS_PWRSEL) != BITS_PWRSEL) { |
| result = inv_serial_single_write( |
| mlsl_handle, mputestCfgPtr->addr, |
| MPUREG_PWR_MGM, pwr_mgm); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| } |
| #else |
| 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; |
| } |
| } |
| #endif |
| } |
| |
| 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 */ |
| #ifdef CONFIG_MPU_SENSORS_MPU3050 |
| result = inv_test_gyro_3050(mlsl_handle, |
| gyro_biases, &temp_avg, provide_result); |
| #else |
| MPL_LOGW_IF(provide_result, |
| "Self Test for MPU-6050 devices is for bias correction only: " |
| "no test PASS/FAIL result will be provided\n"); |
| result = inv_test_gyro_6050(mlsl_handle, gyro_biases, &temp_avg); |
| #endif |
| |
| 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); |
| #if defined CONFIG_MPU_SENSORS_MPU6050B1 |
| if (MPL_PROD_KEY(mputestCfgPtr->product_id, |
| mputestCfgPtr->product_revision) == MPU_PRODUCT_KEY_B1_E1_5) { |
| accelSens[2] /= 2; |
| } else { |
| unsigned short trim_corr = 16384 / mputestCfgPtr->accel_sens_trim; |
| accelSens[0] /= trim_corr; |
| accelSens[1] /= trim_corr; |
| accelSens[2] /= trim_corr; |
| } |
| #endif |
| } else { |
| /* would be 0, but 1 to avoid divide-by-0 below */ |
| accelSens[0] = accelSens[1] = accelSens[2] = 1; |
| } |
| #ifdef CONFIG_MPU_SENSORS_MPU3050 |
| result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2], |
| provide_result); |
| #else |
| result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2], |
| FALSE); |
| #endif |
| 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; |
| #ifndef CONFIG_MPU_SENSORS_MPU3050 |
| struct ext_slave_platform_data *pdata = &mputestCfgPtr->pdata->accel; |
| #endif |
| |
| #ifdef CONFIG_MPU_SENSORS_MPU3050 |
| result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, 0x23, |
| 6, data); |
| #else |
| result = inv_serial_read(mlsl_handle, pdata->address, slave->read_reg, |
| slave->read_len, data); |
| #endif |
| 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 |
| |
| /** |
| * @} |
| */ |
| |
