1396 lines
49 KiB
C
1396 lines
49 KiB
C
/*
|
|
$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
|
|
|
|
/**
|
|
* @}
|
|
*/
|
|
|