/**
* @brief umplStartAccelOnlyLowPower starts the accelerometer
* in low power mode.
*
* @pre umplStartMPU() must have been called.
* @param freq
* The update rate can be 40 Hz, 10 Hz, 2.5 Hz, 1.25 Hz.
*
* @return INV_SUCCESS if successful, a non-zero error code otherwise.
*/
inv_error_t umplStartAccelOnlyLowPower(float freq)
{
//int result;
if(umplState == UMPL_STOP)
{
MPL_LOGD("UMPL_STOP to UMPL_LPACCEL_ONLY\n");
umplStartMPU();
}
if (umplState == UMPL_RUN)
{
fifo_rate = inv_get_fifo_rate();
}
if (umplState == UMPL_RUN || umplState == UMPL_ACCEL_ONLY)
{
if( freq <= 1.25F )
inv_set_fifo_rate(159); //fifo rate = 1.25 hz
else if( freq <= 2.5F && freq > 1.25F )
inv_set_fifo_rate(79); //fifo rate = 2.5 hz
else if( freq <= 10.0F && freq > 2.5F )
inv_set_fifo_rate(19); //fifo rate = 10 hz
else
inv_set_fifo_rate(4); //fifo rate = 40 hz
inv_set_mpu_sensors(INV_THREE_AXIS_ACCEL);
umplSetState(UMPL_LPACCEL_ONLY);
return INV_SUCCESS;
}
return INV_ERROR_SM_IMPROPER_STATE;
}
/**
* @brief umplStartAccelOnly starts the accelerometer.
*
* @pre umplStartMPU() must have been called.
* @param freq
* The update rate can be 18 Hz, 50 Hz and 100 Hz.
*
* @return INV_SUCCESS if successful, a non-zero error code otherwise.
*/
inv_error_t umplStartAccelOnly(float freq)
{
//int result;
if(umplState == UMPL_STOP)
{
MPL_LOGD("UMPL_STOP to UMPL_ACCEL_ONLY\n");
umplStartMPU();
}
if (umplState == UMPL_RUN)
{
fifo_rate = inv_get_fifo_rate();
}
if (umplState == UMPL_RUN || umplState == UMPL_LPACCEL_ONLY)
{
if( freq <= 18.0F )
inv_set_fifo_rate(10); //fifo rate = 18 hz
else if( freq <= 50.0F && freq > 18.0F )
inv_set_fifo_rate(3); //fifo rate = 50 hz
else
inv_set_fifo_rate(1); //fifo rate = 100 hz
inv_set_mpu_sensors(INV_THREE_AXIS_ACCEL);
umplSetState(UMPL_ACCEL_ONLY);
return INV_SUCCESS;
}
return INV_ERROR_SM_IMPROPER_STATE;
}
void lis331dlh_set_dur(struct lis331dlh_config *config,
long dur)
{
long reg_dur = (dur * config->odr) / 1000000L;
config->dur = dur;
if (reg_dur > LIS331_MAX_DUR)
reg_dur = LIS331_MAX_DUR;
config->reg_dur = (unsigned char) reg_dur;
MPL_LOGD("DUR: %d, 0x%02x\n", config->dur, (int)config->reg_dur);
}
void lis331dlh_set_ths(struct lis331dlh_config *config,
long ths)
{
if ((unsigned int) ths > 1000 * config->fsr)
ths = (long) 1000 * config->fsr;
if (ths < 0)
ths = 0;
config->ths = ths;
config->reg_ths = (unsigned char)(long)((ths * 128L) / (config->fsr));
MPL_LOGD("THS: %d, 0x%02x\n", config->ths, (int)config->reg_ths);
}
static void lis331dlh_set_fsr(
struct lis331dlh_config *config,
long fsr)
{
if (fsr <= 2048)
config->fsr = 2048;
else if (fsr <= 4096)
config->fsr = 4096;
else
config->fsr = 8192;
lis331dlh_set_ths(config, config->ths);
MPL_LOGD("FSR: %d\n", config->fsr);
}
/**
* @brief umplStartMPU kicks starts the uMPL state machine. This function
* in turn calls the MPL functions like inv_dmp_open() and inv_dmp_start() which starts
* the motion processing algorithms. This function also enables the required features
* such as turning on the bias trackers and temperature compensation.
* This function enables the required type of data to be put in FIFO.
*
*
*
* @pre umplInit() must have been called.
*
* @return INV_SUCCESS if successful, a non-zero error code otherwise.
*/
inv_error_t umplStartMPU(void)
{
inv_error_t result;
if (umplState == UMPL_STOP)
{
MPL_LOGV("UMPL_STOP to UMPL_RUN\n");
result = inv_dmp_open();
if (result != INV_SUCCESS) return result;
result = umplDmpSetup();
if (result != INV_SUCCESS) return result;
result = inv_dmp_start();
if (result != INV_SUCCESS) return result;
#ifndef UMPL_DISABLE_LOAD_CAL
result = inv_uload_calibration();
if (result != INV_SUCCESS)
MPL_LOGE("inv_uload_calibration failed with %d in umplStartMPU\n",result);
#endif
umplSetState(UMPL_RUN);
return INV_SUCCESS;
}
else if( umplState == UMPL_ACCEL_ONLY )
{
struct mldl_cfg * mldl_cfg = inv_get_dl_config();
MPL_LOGD("UMPL_ACCEL_ONLY (or UMPL_LPACCEL_ONLY) to UMPL_RUN\n");
if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]) {
inv_set_mpu_sensors( INV_NINE_AXIS );
} else {
inv_set_mpu_sensors( INV_SIX_AXIS_GYRO_ACCEL );
}
inv_set_fifo_rate(fifo_rate);
umplSetState(UMPL_RUN);
return INV_SUCCESS;
}
else if( umplState == UMPL_LPACCEL_ONLY )
{
umplStartAccelOnly(0.0);
umplStartMPU();
}
return INV_ERROR_SM_IMPROPER_STATE;
}
/**
* Set the Output data rate for the particular configuration
*
* @param config Config to modify with new ODR
* @param odr Output data rate in units of 1/1000Hz
*/
static void lis331dlh_set_odr(
struct lis331dlh_config *config,
long odr)
{
unsigned char bits;
if (odr > 400000) {
config->odr = 1000000;
bits = 0x38;
} else if (odr > 100000) {
config->odr = 400000;
bits = 0x30;
} else if (odr > 50000) {
config->odr = 100000;
bits = 0x28;
} else if (odr > 10000) {
config->odr = 50000;
bits = 0x20;
} else if (odr > 5000) {
config->odr = 10000;
bits = 0xC0;
} else if (odr > 2000) {
config->odr = 5000;
bits = 0xB0;
} else if (odr > 1000) {
config->odr = 2000;
bits = 0x80;
} else if (odr > 500) {
config->odr = 1000;
bits = 0x60;
} else if (odr > 0) {
config->odr = 500;
bits = 0x40;
} else {
config->odr = 0;
bits = 0;
}
config->ctrl_reg1 = bits | (config->ctrl_reg1 & 0x7);
lis331dlh_set_dur(config, config->dur);
MPL_LOGD("ODR: %d, 0x%02x\n", config->odr, (int)config->ctrl_reg1);
}
/* ---------------------- */
void mpu_print_cfg(struct mldl_cfg * mldl_cfg)
{
struct mpu3050_platform_data *pdata = mldl_cfg->pdata;
struct ext_slave_platform_data *accel = &mldl_cfg->pdata->accel;
struct ext_slave_platform_data *compass = &mldl_cfg->pdata->compass;
struct ext_slave_platform_data *pressure = &mldl_cfg->pdata->pressure;
MPL_LOGD("mldl_cfg.addr = %02x\n", mldl_cfg->addr);
MPL_LOGD("mldl_cfg.int_config = %02x\n", mldl_cfg->int_config);
MPL_LOGD("mldl_cfg.ext_sync = %02x\n", mldl_cfg->ext_sync);
MPL_LOGD("mldl_cfg.full_scale = %02x\n", mldl_cfg->full_scale);
MPL_LOGD("mldl_cfg.lpf = %02x\n", mldl_cfg->lpf);
MPL_LOGD("mldl_cfg.clk_src = %02x\n", mldl_cfg->clk_src);
MPL_LOGD("mldl_cfg.divider = %02x\n", mldl_cfg->divider);
MPL_LOGD("mldl_cfg.dmp_enable = %02x\n", mldl_cfg->dmp_enable);
MPL_LOGD("mldl_cfg.fifo_enable = %02x\n", mldl_cfg->fifo_enable);
MPL_LOGD("mldl_cfg.dmp_cfg1 = %02x\n", mldl_cfg->dmp_cfg1);
MPL_LOGD("mldl_cfg.dmp_cfg2 = %02x\n", mldl_cfg->dmp_cfg2);
MPL_LOGD("mldl_cfg.offset_tc[0] = %02x\n", mldl_cfg->offset_tc[0]);
MPL_LOGD("mldl_cfg.offset_tc[1] = %02x\n", mldl_cfg->offset_tc[1]);
MPL_LOGD("mldl_cfg.offset_tc[2] = %02x\n", mldl_cfg->offset_tc[2]);
MPL_LOGD("mldl_cfg.silicon_revision = %02x\n", mldl_cfg->silicon_revision);
MPL_LOGD("mldl_cfg.product_id = %02x\n", mldl_cfg->product_id);
MPL_LOGD("mldl_cfg.trim = %02x\n", mldl_cfg->trim);
if (mldl_cfg->accel) {
MPL_LOGD("slave_accel->suspend = %02x\n", (int)mldl_cfg->accel->suspend);
MPL_LOGD("slave_accel->resume = %02x\n", (int)mldl_cfg->accel->resume);
MPL_LOGD("slave_accel->read = %02x\n", (int)mldl_cfg->accel->read);
MPL_LOGD("slave_accel->type = %02x\n", mldl_cfg->accel->type);
MPL_LOGD("slave_accel->reg = %02x\n", mldl_cfg->accel->reg);
MPL_LOGD("slave_accel->len = %02x\n", mldl_cfg->accel->len);
MPL_LOGD("slave_accel->endian = %02x\n", mldl_cfg->accel->endian);
MPL_LOGD("slave_accel->range.mantissa= %02x\n", (int)mldl_cfg->accel->range.mantissa);
MPL_LOGD("slave_accel->range.fraction= %02x\n", (int)mldl_cfg->accel->range.fraction);
} else {
MPL_LOGD("slave_accel = NULL\n");
}
if (mldl_cfg->compass) {
MPL_LOGD("slave_compass->suspend = %02x\n", (int)mldl_cfg->compass->suspend);
MPL_LOGD("slave_compass->resume = %02x\n", (int)mldl_cfg->compass->resume);
MPL_LOGD("slave_compass->read = %02x\n", (int)mldl_cfg->compass->read);
MPL_LOGD("slave_compass->type = %02x\n", mldl_cfg->compass->type);
MPL_LOGD("slave_compass->reg = %02x\n", mldl_cfg->compass->reg);
MPL_LOGD("slave_compass->len = %02x\n", mldl_cfg->compass->len);
MPL_LOGD("slave_compass->endian = %02x\n", mldl_cfg->compass->endian);
MPL_LOGD("slave_compass->range.mantissa= %02x\n", (int)mldl_cfg->compass->range.mantissa);
MPL_LOGD("slave_compass->range.fraction= %02x\n", (int)mldl_cfg->compass->range.fraction);
} else {
MPL_LOGD("slave_compass = NULL\n");
}
if (mldl_cfg->pressure) {
MPL_LOGD("slave_pressure->suspend = %02x\n", (int)mldl_cfg->pressure->suspend);
MPL_LOGD("slave_pressure->resume = %02x\n", (int)mldl_cfg->pressure->resume);
MPL_LOGD("slave_pressure->read = %02x\n", (int)mldl_cfg->pressure->read);
MPL_LOGD("slave_pressure->type = %02x\n", mldl_cfg->pressure->type);
MPL_LOGD("slave_pressure->reg = %02x\n", mldl_cfg->pressure->reg);
MPL_LOGD("slave_pressure->len = %02x\n", mldl_cfg->pressure->len);
MPL_LOGD("slave_pressure->endian = %02x\n", mldl_cfg->pressure->endian);
MPL_LOGD("slave_pressure->range.mantissa= %02x\n", (int)mldl_cfg->pressure->range.mantissa);
MPL_LOGD("slave_pressure->range.fraction= %02x\n", (int)mldl_cfg->pressure->range.fraction);
} else {
MPL_LOGD("slave_pressure = NULL\n");
}
MPL_LOGD("accel->get_slave_descr = %x\n",(unsigned int) accel->get_slave_descr);
MPL_LOGD("accel->adapt_num = %02x\n", accel->adapt_num);
MPL_LOGD("accel->bus = %02x\n", accel->bus);
MPL_LOGD("accel->address = %02x\n", accel->address);
MPL_LOGD("accel->orientation = \n"
" %2d %2d %2d\n"
" %2d %2d %2d\n"
" %2d %2d %2d\n",
accel->orientation[0],accel->orientation[1],accel->orientation[2],
accel->orientation[3],accel->orientation[4],accel->orientation[5],
accel->orientation[6],accel->orientation[7],accel->orientation[8]);
MPL_LOGD("compass->get_slave_descr = %x\n",(unsigned int) compass->get_slave_descr);
MPL_LOGD("compass->adapt_num = %02x\n", compass->adapt_num);
MPL_LOGD("compass->bus = %02x\n", compass->bus);
MPL_LOGD("compass->address = %02x\n", compass->address);
MPL_LOGD("compass->orientation = \n"
" %2d %2d %2d\n"
" %2d %2d %2d\n"
" %2d %2d %2d\n",
compass->orientation[0],compass->orientation[1],compass->orientation[2],
compass->orientation[3],compass->orientation[4],compass->orientation[5],
compass->orientation[6],compass->orientation[7],compass->orientation[8]);
MPL_LOGD("pressure->get_slave_descr = %x\n",(unsigned int) pressure->get_slave_descr);
MPL_LOGD("pressure->adapt_num = %02x\n", pressure->adapt_num);
MPL_LOGD("pressure->bus = %02x\n", pressure->bus);
MPL_LOGD("pressure->address = %02x\n", pressure->address);
MPL_LOGD("pressure->orientation = \n"
" %2d %2d %2d\n"
" %2d %2d %2d\n"
" %2d %2d %2d\n",
pressure->orientation[0],pressure->orientation[1],pressure->orientation[2],
pressure->orientation[3],pressure->orientation[4],pressure->orientation[5],
pressure->orientation[6],pressure->orientation[7],pressure->orientation[8]);
MPL_LOGD("pdata->int_config = %02x\n", pdata->int_config);
MPL_LOGD("pdata->level_shifter = %02x\n", pdata->level_shifter);
MPL_LOGD("pdata->orientation = \n"
" %2d %2d %2d\n"
" %2d %2d %2d\n"
" %2d %2d %2d\n",
pdata->orientation[0],pdata->orientation[1],pdata->orientation[2],
pdata->orientation[3],pdata->orientation[4],pdata->orientation[5],
pdata->orientation[6],pdata->orientation[7],pdata->orientation[8]);
MPL_LOGD("Struct sizes: mldl_cfg: %d, "
"ext_slave_descr:%d, mpu3050_platform_data:%d: RamOffset: %d\n",
sizeof(struct mldl_cfg), sizeof(struct ext_slave_descr),
sizeof(struct mpu3050_platform_data),
offsetof(struct mldl_cfg, ram));
}
