/**
* @brief one-time device driver initialization function.
* If the driver is built as a kernel module, this function will be
* called when the module is loaded in the kernel.
* If the driver is built-in in the kernel, this function will be
* called at boot time.
*
* @param mlsl_handle
* the handle to the serial channel the device is connected to.
* @param slave
* a pointer to the slave descriptor data structure.
* @param pdata
* a pointer to the slave platform data.
*
* @return INV_SUCCESS if successful or a non-zero error code.
*/
static int mma8450_init(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
struct mma8450_private_data *private_data;
private_data = (struct mma8450_private_data *)
inv_malloc(sizeof(struct mma8450_private_data));
if (!private_data)
return INV_ERROR_MEMORY_EXAUSTED;
pdata->private_data = private_data;
mma8450_set_odr(mlsl_handle, pdata, &private_data->suspend,
FALSE, 0);
mma8450_set_odr(mlsl_handle, pdata, &private_data->resume,
FALSE, 200000);
mma8450_set_fsr(mlsl_handle, pdata, &private_data->suspend,
FALSE, 2000);
mma8450_set_fsr(mlsl_handle, pdata, &private_data->resume,
FALSE, 2000);
mma8450_set_irq(mlsl_handle, pdata, &private_data->suspend,
FALSE,
MPU_SLAVE_IRQ_TYPE_NONE);
mma8450_set_irq(mlsl_handle, pdata, &private_data->resume,
FALSE,
MPU_SLAVE_IRQ_TYPE_NONE);
return INV_SUCCESS;
}
static int lis3dh_init(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
inv_error_t result;
long range;
struct lis3dh_private_data *private_data;
private_data = (struct lis3dh_private_data *)
inv_malloc(sizeof(struct lis3dh_private_data));
if (!private_data)
return INV_ERROR_MEMORY_EXAUSTED;
pdata->private_data = private_data;
private_data->resume.ctrl_reg1 = 0x67;
private_data->suspend.ctrl_reg1 = 0x18;
private_data->resume.mot_int1_cfg = 0x95;
private_data->suspend.mot_int1_cfg = 0x2a;
lis3dh_set_odr(mlsl_handle, pdata, &private_data->suspend, FALSE, 0);
lis3dh_set_odr(mlsl_handle, pdata, &private_data->resume,
FALSE, 200000);
range = RANGE_FIXEDPOINT_TO_LONG_MG(slave->range);
lis3dh_set_fsr(mlsl_handle, pdata, &private_data->suspend,
FALSE, range);
lis3dh_set_fsr(mlsl_handle, pdata, &private_data->resume,
FALSE, range);
lis3dh_set_ths(mlsl_handle, pdata, &private_data->suspend,
FALSE, 80);
lis3dh_set_ths(mlsl_handle, pdata, &private_data->resume,
FALSE, 40);
lis3dh_set_dur(mlsl_handle, pdata, &private_data->suspend,
FALSE, 1000);
lis3dh_set_dur(mlsl_handle, pdata, &private_data->resume,
FALSE, 2540);
lis3dh_set_irq(mlsl_handle, pdata, &private_data->suspend,
FALSE, MPU_SLAVE_IRQ_TYPE_NONE);
lis3dh_set_irq(mlsl_handle, pdata, &private_data->resume,
FALSE, MPU_SLAVE_IRQ_TYPE_NONE);
result = inv_serial_single_write(mlsl_handle, pdata->address,
LIS3DH_CTRL_REG1, 0x07);
inv_sleep(6);
return INV_SUCCESS;
}
static int mantis_init(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
struct mantis_private_data *private_data;
long range;
#ifdef CONFIG_MPU_SENSORS_MPU3050
(void *)private_data;
return INV_ERROR_INVALID_MODULE;
#endif
private_data = (struct mantis_private_data *)
inv_malloc(sizeof(struct mantis_private_data));
if (!private_data)
return INV_ERROR_MEMORY_EXAUSTED;
pdata->private_data = private_data;
mantis_set_odr(mlsl_handle, pdata, &private_data->suspend,
FALSE, 0);
mantis_set_odr(mlsl_handle, pdata, &private_data->resume,
FALSE, 200000);
range = RANGE_FIXEDPOINT_TO_LONG_MG(slave->range);
mantis_set_fsr(mlsl_handle, pdata, &private_data->suspend,
FALSE, range);
mantis_set_fsr(mlsl_handle, pdata, &private_data->resume,
FALSE, range);
mantis_set_irq(mlsl_handle, pdata, &private_data->suspend, FALSE,
MPU_SLAVE_IRQ_TYPE_NONE);
mantis_set_irq(mlsl_handle, pdata, &private_data->resume, FALSE,
MPU_SLAVE_IRQ_TYPE_NONE);
mantis_set_ths(mlsl_handle, pdata, &private_data->suspend, FALSE, 80);
mantis_set_ths(mlsl_handle, pdata, &private_data->resume, FALSE, 40);
mantis_set_dur(mlsl_handle, pdata, &private_data->suspend, FALSE, 1000);
mantis_set_dur(mlsl_handle, pdata, &private_data->resume, FALSE, 2540);
return INV_SUCCESS;
}
inv_error_t inv_read_cal(unsigned char **calData, size_t *bytesRead)
{
FILE *fp;
inv_error_t result = INV_SUCCESS;
size_t fsize;
fp = fopen(MLCAL_FILE,"rb");
if (fp == NULL) {
MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE);
return INV_ERROR_FILE_OPEN;
}
// obtain file size
fseek (fp, 0 , SEEK_END);
fsize = ftell (fp);
rewind (fp);
*calData = (unsigned char *)inv_malloc(fsize);
if (*calData==NULL) {
MPL_LOGE("Could not allocate buffer of %d bytes - "
"aborting\n", fsize);
fclose(fp);
return INV_ERROR_MEMORY_EXAUSTED;
}
*bytesRead = fread(*calData, 1, fsize, fp);
if (*bytesRead != fsize) {
MPL_LOGE("bytes read (%d) don't match file size (%d)\n",
*bytesRead, fsize);
result = INV_ERROR_FILE_READ;
goto read_cal_end;
}
else {
MPL_LOGI("Bytes read = %d", *bytesRead);
}
read_cal_end:
fclose(fp);
return result;
}
/**
* @brief Store runtime calibration data to a file
*
* @pre Must be in INV_STATE_DMP_OPENED state.
* inv_dmp_open() or inv_dmp_stop() must have been called.
* inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
* been called.
*
* @return 0 or error code.
*/
inv_error_t inv_store_calibration(void)
{
unsigned char *calData;
inv_error_t result;
size_t length;
result = inv_get_mpl_state_size(&length);
calData = (unsigned char *)inv_malloc(length);
if (!calData) {
MPL_LOGE("Could not allocate buffer of %d bytes - "
"aborting\n", length);
return INV_ERROR_MEMORY_EXAUSTED;
}
else {
MPL_LOGI("mpl state size = %d", length);
}
result = inv_save_mpl_states(calData, length);
if (result != INV_SUCCESS) {
MPL_LOGE("Could not save mpl states - "
"error %d - aborting\n", result);
goto free_mem_n_exit;
}
else {
MPL_LOGE("calData from inv_save_mpl_states, size=%d",
strlen((char *)calData));
}
result = inv_write_cal(calData, length);
if (result != INV_SUCCESS) {
MPL_LOGE("Could not store calibrated data on file - "
"error %d - aborting\n", result);
goto free_mem_n_exit;
}
free_mem_n_exit:
inv_free(calData);
return result;
}
/**
* @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 enable_axis
* specify which axis has to be checked and corrected: provides
* a switch mode between 3 axis calibration and Z axis only
* calibration.
* @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 gravity
* The gravity value given the parts' sensitivity: for example
* if the accelerometer is set to +/- 2 gee ==> the gravity
* value will be 2^14 = 16384.
* @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 test_accel(void *mlsl_handle, int enable_axes,
short *bias, long gravity,
uint_fast8_t perform_full_test)
{
short *p_vals;
float avg[3] = {0.f, 0.f, 0.f}, zg = 0.f;
float rms[3];
float accel_rms_thresh = 1000000.f; /* enourmous to make the test always
passes - future deployment */
int accel_error = false;
const long sample_period = inv_get_sample_step_size_ms() * 1000;
int ii;
p_vals = (short*)inv_malloc(sizeof(short) * 3 * test_setup.accel_samples);
/* collect the samples */
for(ii = 0; ii < test_setup.accel_samples; ii++) {
unsigned result = INV_ERROR_ACCEL_DATA_NOT_READY;
int tries = 0;
long accel_data[3];
short *vals = &p_vals[3 * ii];
/* ignore data not ready errors but don't try more than 5 times */
while (result == INV_ERROR_ACCEL_DATA_NOT_READY && tries++ < 5) {
result = inv_get_accel_data(accel_data);
usleep(sample_period);
}
if (result || tries >= 5) {
MPL_LOGV("cannot reliably fetch data from the accelerometer");
accel_error = true;
goto accel_early_exit;
}
vals[X] = (short)accel_data[X];
vals[Y] = (short)accel_data[Y];
vals[Z] = (short)accel_data[Z];
avg[X] += 1.f * vals[X] / test_setup.accel_samples;
avg[Y] += 1.f * vals[Y] / test_setup.accel_samples;
avg[Z] += 1.f * vals[Z] / test_setup.accel_samples;
if (VERBOSE_OUT)
MPL_LOGI("Accel : %+13d %+13d %+13d (LSB)\n",
vals[X], vals[Y], vals[Z]);
}
if (((enable_axes << 4) & INV_THREE_AXIS_ACCEL) == INV_THREE_AXIS_ACCEL) {
MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (LSB)\n",
avg[X], avg[Y], avg[Z]);
if (VERBOSE_OUT)
MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (gee)\n",
avg[X] / gravity, avg[Y] / gravity, avg[Z] / gravity);
bias[X] = FLOAT_TO_SHORT(avg[X]);
bias[Y] = FLOAT_TO_SHORT(avg[Y]);
zg = avg[Z] - g_z_sign * gravity;
bias[Z] = FLOAT_TO_SHORT(zg);
MPL_LOGI("Accel correct.: %+13d %+13d %+13d (LSB)\n",
bias[X], bias[Y], bias[Z]);
if (VERBOSE_OUT)
MPL_LOGI("Accel correct.: "
"%+13.3f %+13.3f %+13.3f (gee)\n",
1.f * bias[X] / gravity,
1.f * bias[Y] / gravity,
1.f * bias[Z] / gravity);
if (perform_full_test) {
/* accel RMS - for now the threshold is only indicative */
for (ii = 0,
rms[X] = 0.f, rms[Y] = 0.f, rms[Z] = 0.f;
ii < test_setup.accel_samples; ii++) {
short *vals = &p_vals[3 * ii];
rms[X] += (vals[X] - avg[X]) * (vals[X] - avg[X]);
rms[Y] += (vals[Y] - avg[Y]) * (vals[Y] - avg[Y]);
rms[Z] += (vals[Z] - avg[Z]) * (vals[Z] - avg[Z]);
}
for (ii = 0; ii < 3; ii++) {
if (rms[ii] > accel_rms_thresh * accel_rms_thresh
* test_setup.accel_samples) {
MPL_LOGI("%s-Accel RMS (%.2f) exceeded threshold "
"(threshold = %.2f)\n", a_name[ii],
sqrt(rms[ii] / test_setup.accel_samples),
accel_rms_thresh);
accel_error = true;
goto accel_early_exit;
}
}
MPL_LOGI("Accel 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));
}
} else {
MPL_LOGI("Accel Z bias : %+13.3f (LSB)\n", avg[Z]);
if (VERBOSE_OUT)
MPL_LOGI("Accel Z bias : %+13.3f (gee)\n", avg[Z] / gravity);
zg = avg[Z] - g_z_sign * gravity;
bias[Z] = FLOAT_TO_SHORT(zg);
MPL_LOGI("Accel Z correct.: %+13d (LSB)\n", bias[Z]);
if (VERBOSE_OUT)
MPL_LOGI("Accel Z correct.: "
"%+13.3f (gee)\n", 1.f * bias[Z] / gravity);
}
accel_early_exit:
if (accel_error) {
bias[0] = bias[1] = bias[2] = 0;
return (1); /* error */
}
inv_free(p_vals);
return (0); /* success */
}
static int ak8975_init(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
unsigned char serial_data[COMPASS_NUM_AXES];
struct ak8975_private_data *private_data;
private_data = (struct ak8975_private_data *)
inv_malloc(sizeof(struct ak8975_private_data));
if (!private_data)
return INV_ERROR_MEMORY_EXAUSTED;
result = inv_serial_single_write(mlsl_handle, pdata->address,
AK8975_REG_CNTL,
AK8975_CNTL_MODE_POWER_DOWN);
ERROR_CHECK(result);
/* Wait at least 100us */
#ifdef __KERNEL__
udelay(100);
#else
inv_sleep(1);
#endif
result = inv_serial_single_write(mlsl_handle, pdata->address,
AK8975_REG_CNTL,
AK8975_CNTL_MODE_FUSE_ROM_ACCESS);
ERROR_CHECK(result);
/* Wait at least 200us */
#ifdef __KERNEL__
udelay(200);
#else
inv_sleep(1);
#endif
result = inv_serial_read(mlsl_handle, pdata->address,
AK8975_REG_ASAX,
COMPASS_NUM_AXES,
serial_data);
ERROR_CHECK(result);
pdata->private_data = private_data;
private_data->init.asa[0] = serial_data[0];
private_data->init.asa[1] = serial_data[1];
private_data->init.asa[2] = serial_data[2];
result = inv_serial_single_write(mlsl_handle, pdata->address,
AK8975_REG_CNTL,
AK8975_CNTL_MODE_POWER_DOWN);
ERROR_CHECK(result);
#ifdef __KERNEL__
udelay(100);
#else
inv_sleep(1);
#endif
return INV_SUCCESS;
}
