in drivers/sensors/lsm303agr.c [437:850]
static int lsm303agr_selftest(FAR struct lsm303agr_dev_s *priv,
uint32_t mode)
{
int samples = 5;
int i;
int i2;
int i3;
uint8_t value;
int8_t lox = 0;
int8_t loxst = 0;
int8_t hix = 0;
int8_t hixst = 0;
int8_t loy = 0;
int8_t loyst = 0;
int8_t hiy = 0;
int8_t hiyst = 0;
int8_t loz = 0;
int8_t lozst = 0;
int8_t hiz = 0;
int8_t hizst = 0;
int16_t OUTX_NOST[samples];
int16_t OUTY_NOST[samples];
int16_t OUTZ_NOST[samples];
int16_t OUTX_ST[samples];
int16_t OUTY_ST[samples];
int16_t OUTZ_ST[samples];
int16_t avr_x = 0;
int16_t avr_y = 0;
int16_t avr_z = 0;
int16_t avr_xst = 0;
int16_t avr_yst = 0;
int16_t avr_zst = 0;
int16_t min_x = 0;
int16_t min_y = 0;
int16_t min_z = 0;
int16_t max_x = 0;
int16_t max_y = 0;
int16_t max_z = 0;
int16_t min_xst = 0;
int16_t min_yst = 0;
int16_t min_zst = 0;
int16_t max_xst = 0;
int16_t max_yst = 0;
int16_t max_zst = 0;
int16_t raw_xst = 0;
int16_t raw_yst = 0;
int16_t raw_zst = 0;
int16_t raw_x = 0;
int16_t raw_y = 0;
int16_t raw_z = 0;
/* mode = 0 then add hex 0x06 to OUT registers */
int8_t registershift;
/* Keep the device still during the self-test procedure. Setting registers
* Power up, wait for 100ms for stable output.
*/
if (mode == 0)
{
registershift = 0x00;
lsm303agr_writereg8(priv, LSM303AGR_CTRL_REG2_A, 0x00);
lsm303agr_writereg8(priv, LSM303AGR_CTRL_REG3_A, 0x00);
lsm303agr_writereg8(priv, LSM303AGR_CTRL_REG4_A, 0x81);
lsm303agr_writereg8(priv, LSM303AGR_CTRL_REG1_A, 0x57);
g_accelerofactor = 1;
}
else
{
registershift = 0x40;
lsm303agr_writereg8(priv, LSM303AGR_CFG_REG_A_M, 0x8c);
lsm303agr_writereg8(priv, LSM303AGR_CFG_REG_B_M, 0x02);
lsm303agr_writereg8(priv, LSM303AGR_CFG_REG_C_M, 0x10);
g_magnetofactor = 1;
}
nxsig_usleep(100000); /* 100ms */
/* Read the output registers after checking XLDA bit 5 times */
bool checkbit = false;
/* Wait until first sample and data is available */
while (checkbit)
{
if (mode == 0)
{
lsm303agr_readreg8(priv, LSM303AGR_STATUS_REG_A, &value);
checkbit = lsm303agr_isbitset(value, LSM303AGR_STATUS_REG_A_ZYXDA);
}
else
{
lsm303agr_readreg8(priv, LSM303AGR_STATUS_REG_M, &value);
checkbit = lsm303agr_isbitset(value, LSM303AGR_STATUS_REG_M_ZYXDA);
}
}
nxsig_usleep(100000); /* 100ms */
/* Read OUT registers Gyro is starting at 22h and Accelero at 28h */
lsm303agr_readreg8(priv,
LSM303AGR_OUT_X_L_A + registershift,
(FAR uint8_t *)&lox);
lsm303agr_readreg8(priv,
LSM303AGR_OUT_X_H_A + registershift,
(FAR uint8_t *)&hix);
lsm303agr_readreg8(priv,
LSM303AGR_OUT_Y_L_A + registershift,
(FAR uint8_t *)&loy);
lsm303agr_readreg8(priv,
LSM303AGR_OUT_Y_H_A + registershift,
(FAR uint8_t *)&hiy);
lsm303agr_readreg8(priv,
LSM303AGR_OUT_Z_L_A + registershift,
(FAR uint8_t *)&loz);
lsm303agr_readreg8(priv,
LSM303AGR_OUT_Z_H_A + registershift,
(FAR uint8_t *)&hiz);
/* check XLDA 5 times */
for (i = 0; i < samples; i++)
{
if (mode == 0)
{
lsm303agr_readreg8(priv, LSM303AGR_STATUS_REG_A, &value);
checkbit = lsm303agr_isbitset(value, LSM303AGR_STATUS_REG_A_ZYXDA);
}
else
{
lsm303agr_readreg8(priv, LSM303AGR_STATUS_REG_M, &value);
checkbit = lsm303agr_isbitset(value, LSM303AGR_STATUS_REG_M_ZYXDA);
}
/* Average the stored data on each axis *
* http://ozzmaker.com/accelerometer-to-g/
*/
lsm303agr_readreg8(priv,
LSM303AGR_OUT_X_L_A + registershift,
(FAR uint8_t *)&lox);
lsm303agr_readreg8(priv,
LSM303AGR_OUT_X_H_A + registershift,
(FAR uint8_t *)&hix);
raw_x = (int16_t) (((uint16_t) hix << 8U) | (uint16_t) lox);
lsm303agr_readreg8(priv,
LSM303AGR_OUT_Y_L_A + registershift,
(FAR uint8_t *)&loy);
lsm303agr_readreg8(priv,
LSM303AGR_OUT_Y_H_A + registershift,
(FAR uint8_t *)&hiy);
raw_y = (int16_t) (((uint16_t) hiy << 8U) | (uint16_t) loy);
lsm303agr_readreg8(priv,
LSM303AGR_OUT_Z_L_A + registershift,
(FAR uint8_t *)&loz);
lsm303agr_readreg8(priv,
LSM303AGR_OUT_Z_H_A + registershift,
(FAR uint8_t *)&hiz);
raw_z = (int16_t) (((uint16_t) hiz << 8U) | (uint16_t) loz);
/* selftest only uses raw values */
OUTX_NOST[i] = raw_x;
OUTY_NOST[i] = raw_y;
OUTZ_NOST[i] = raw_z;
}
/* Enable Selftest */
if (mode == 0)
{
lsm303agr_writereg8(priv, LSM303AGR_CTRL_REG4_A, 0x85);
}
else
{
lsm303agr_writereg8(priv, LSM303AGR_CFG_REG_C_M, 0x12);
}
nxsig_usleep(100000); /* 100ms */
checkbit = false;
while (checkbit)
{
if (mode == 0)
{
lsm303agr_readreg8(priv, LSM303AGR_STATUS_REG_A, &value);
checkbit = lsm303agr_isbitset(value, LSM303AGR_STATUS_REG_A_ZYXDA);
}
else
{
lsm303agr_readreg8(priv, LSM303AGR_STATUS_REG_M, &value);
checkbit = lsm303agr_isbitset(value, LSM303AGR_STATUS_REG_M_ZYXDA);
}
}
nxsig_usleep(100000); /* 100ms */
/* Now do all the ST values */
lsm303agr_readreg8(priv,
LSM303AGR_OUT_X_L_A + registershift,
(FAR uint8_t *)&loxst);
lsm303agr_readreg8(priv,
LSM303AGR_OUT_X_H_A + registershift,
(FAR uint8_t *)&hixst);
lsm303agr_readreg8(priv,
LSM303AGR_OUT_Y_L_A + registershift,
(FAR uint8_t *)&loyst);
lsm303agr_readreg8(priv,
LSM303AGR_OUT_Y_H_A + registershift,
(FAR uint8_t *)&hiyst);
lsm303agr_readreg8(priv,
LSM303AGR_OUT_Z_L_A + registershift,
(FAR uint8_t *)&lozst);
lsm303agr_readreg8(priv,
LSM303AGR_OUT_Z_H_A + registershift,
(FAR uint8_t *)&hizst);
for (i2 = 0; i2 < samples; i2++)
{
if (mode == 0)
{
lsm303agr_readreg8(priv, LSM303AGR_STATUS_REG_A, &value);
checkbit = lsm303agr_isbitset(value, LSM303AGR_STATUS_REG_A_ZYXDA);
}
else
{
lsm303agr_readreg8(priv, LSM303AGR_STATUS_REG_M, &value);
checkbit = lsm303agr_isbitset(value, LSM303AGR_STATUS_REG_M_ZYXDA);
}
nxsig_usleep(100000); /* 100ms */
lsm303agr_readreg8(priv,
LSM303AGR_OUT_X_L_A + registershift,
(FAR uint8_t *)&loxst);
lsm303agr_readreg8(priv,
LSM303AGR_OUT_X_H_A + registershift,
(FAR uint8_t *)&hixst);
raw_xst = (int16_t) (((uint16_t) hixst << 8U) | (uint16_t) loxst);
lsm303agr_readreg8(priv,
LSM303AGR_OUT_Y_L_A + registershift,
(FAR uint8_t *)&loyst);
lsm303agr_readreg8(priv,
LSM303AGR_OUT_Y_H_A + registershift,
(FAR uint8_t *)&hiyst);
raw_yst = (int16_t) (((uint16_t) hiyst << 8U) | (uint16_t) loyst);
lsm303agr_readreg8(priv,
LSM303AGR_OUT_Z_L_A + registershift,
(FAR uint8_t *)&lozst);
lsm303agr_readreg8(priv,
LSM303AGR_OUT_Z_H_A + registershift,
(FAR uint8_t *)&hizst);
raw_zst = (int16_t) (((uint16_t) hizst << 8U) | (uint16_t) lozst);
/* Selftest only uses raw values */
OUTX_ST[i2] = raw_xst;
OUTY_ST[i2] = raw_yst;
OUTZ_ST[i2] = raw_zst;
}
/* Average stored data on each axis */
for (i3 = 0; i3 < samples; i3++)
{
avr_x = avr_x + (int16_t) OUTX_NOST[i3];
avr_y = avr_y + (int16_t) OUTY_NOST[i3];
avr_z = avr_z + (int16_t) OUTZ_NOST[i3];
avr_xst = avr_xst + (int16_t) OUTX_ST[i3];
avr_yst = avr_yst + (int16_t) OUTY_ST[i3];
avr_zst = avr_zst + (int16_t) OUTZ_ST[i3];
}
avr_x = (int16_t) avr_x / samples;
avr_y = (int16_t) avr_y / samples;
avr_z = (int16_t) avr_z / samples;
avr_xst = (int16_t) avr_xst / samples;
avr_yst = (int16_t) avr_yst / samples;
avr_zst = (int16_t) avr_zst / samples;
min_x = OUTX_NOST[lsm303agr_find_minimum(OUTX_NOST, samples)];
min_y = OUTY_NOST[lsm303agr_find_minimum(OUTY_NOST, samples)];
min_z = OUTZ_NOST[lsm303agr_find_minimum(OUTZ_NOST, samples)];
max_x = OUTX_NOST[lsm303agr_find_maximum(OUTX_NOST, samples)];
max_y = OUTY_NOST[lsm303agr_find_maximum(OUTY_NOST, samples)];
max_z = OUTZ_NOST[lsm303agr_find_maximum(OUTZ_NOST, samples)];
min_xst = OUTX_ST[lsm303agr_find_minimum(OUTX_ST, samples)];
min_yst = OUTY_ST[lsm303agr_find_minimum(OUTY_ST, samples)];
min_zst = OUTZ_ST[lsm303agr_find_minimum(OUTZ_ST, samples)];
max_xst = OUTX_ST[lsm303agr_find_maximum(OUTX_ST, samples)];
max_yst = OUTY_ST[lsm303agr_find_maximum(OUTY_ST, samples)];
max_zst = OUTZ_ST[lsm303agr_find_maximum(OUTZ_ST, samples)];
sninfo("stdev_x: -%d %d +%d\n", avr_x - min_x, avr_x, max_x - avr_x);
sninfo("stdev_y: -%d %d +%d\n", avr_y - min_y, avr_y, max_y - avr_y);
sninfo("stdev_z: -%d %d +%d\n", avr_z - min_z, avr_z, max_z - avr_z);
sninfo("stdev_xst: -%d %d +%d\n", avr_xst - min_xst, avr_xst,
max_xst - avr_xst);
sninfo("stdev_yst: -%d %d +%d\n", avr_yst - min_yst, avr_yst,
max_yst - avr_yst);
sninfo("stdev_zst: -%d %d +%d\n", avr_zst - min_zst, avr_zst,
max_zst - avr_zst);
sninfo("avr_x: %d\n", avr_x);
sninfo("avr_y: %d\n", avr_y);
sninfo("avr_z: %d\n", avr_z);
sninfo("min_x: %d\n", min_x);
sninfo("max_x: %d\n", max_x);
sninfo("min_xst: %d\n", min_xst);
sninfo("max_xst: %d\n", max_xst);
/* Validation */
if ((avr_x >= min_x && avr_x <= max_x) &&
(avr_xst >= min_xst && avr_xst <= max_xst))
{
sninfo("PASSED NOST AND ST FOR X!\n");
}
else
{
sninfo("FAILED NOST AND ST FOR X!\n");
sninfo("[ %d - %d ]", min_x, min_xst);
sninfo(" <=\n ");
sninfo("[ %d - %d ]", avr_x, avr_xst);
sninfo(" <=\n ");
sninfo("[ %d - %d ]", max_x, max_xst);
sninfo("\n");
}
if ((avr_y >= min_y && avr_y <= max_y) &&
(avr_yst >= min_yst && avr_yst <= max_yst))
{
sninfo("PASSED NOST AND ST FOR Y!\n");
}
else
{
sninfo("FAILED NOST AND ST FOR Y!\n");
sninfo("[ %d - %d ]", min_y, min_yst);
sninfo(" <=\n ");
sninfo("[ %d - %d ]", avr_y, avr_yst);
sninfo(" <=\n ");
sninfo("[ %d - %d ]", max_y, max_yst);
sninfo("\n");
}
if ((avr_z >= min_z && avr_z <= max_z) &&
(avr_zst >= min_zst && avr_zst <= max_zst))
{
sninfo("PASSED NOST AND ST FOR Z!\n");
}
else
{
sninfo("FAILED NOST AND ST FOR Z!\n");
sninfo("[ %d - %d ]", min_z, min_zst);
sninfo(" <=\n ");
sninfo("[ %d - %d ]", avr_z, avr_zst);
sninfo(" <=\n ");
sninfo("[ %d - %d ]", max_z, max_zst);
sninfo("\n");
}
nxsig_sleep(2);
/* Disable test */
switch (mode)
{
case 0:
{
sninfo("SELFTEST ACCELERO DISABLED\n");
lsm303agr_writereg8(priv, LSM303AGR_CTRL_REG1_A, 0x00);
lsm303agr_writereg8(priv, LSM303AGR_CTRL_REG4_A, 0x01);
}
break;
case 1:
{
sninfo("SELFTEST MAGNETO DISABLED\n");
lsm303agr_writereg8(priv, LSM303AGR_CFG_REG_C_M, 0x10);
lsm303agr_writereg8(priv, LSM303AGR_CFG_REG_A_M, 0x83);
}
break;
default:
break;
}
return OK;
}