/*
$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
/**
* @}
*/