diff options
Diffstat (limited to 'libsensors/mlsdk/mlutils/mputest.c')
| -rw-r--r-- | libsensors/mlsdk/mlutils/mputest.c | 1101 |
1 files changed, 1101 insertions, 0 deletions
diff --git a/libsensors/mlsdk/mlutils/mputest.c b/libsensors/mlsdk/mlutils/mputest.c new file mode 100644 index 0000000..4a598ff --- /dev/null +++ b/libsensors/mlsdk/mlutils/mputest.c @@ -0,0 +1,1101 @@ +/* + $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 + +/** + * @} + */ + |