int mantis_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = ML_SUCCESS;
unsigned char reg;
struct mantis_private_data *private_data;
private_data = (struct mantis_private_data *) pdata->private_data;
MLSLSerialRead(mlsl_handle, pdata->address,
MPUREG_PWR_MGMT_2, 1, ®);
reg &= ~(BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
MPUREG_PWR_MGMT_2, reg);
ERROR_CHECK(result);
if (slave->range.mantissa == 2)
reg = 0;
else if (slave->range.mantissa == 4)
reg = 1 << 3;
else if (slave->range.mantissa == 8)
reg = 2 << 3;
else if (slave->range.mantissa == 16)
reg = 3 << 3;
else
return ML_ERROR;
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
MPUREG_ACCEL_CONFIG, reg);
ERROR_CHECK(result);
reg = (unsigned char) private_data->suspend.ths / ACCEL_MOT_THR_LSB;
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
MPUREG_ACCEL_MOT_THR, reg);
ERROR_CHECK(result);
reg = (unsigned char)
ACCEL_ZRMOT_THR_LSB_CONVERSION(private_data->resume.ths);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
MPUREG_ACCEL_ZRMOT_THR, reg);
ERROR_CHECK(result);
reg = (unsigned char) private_data->suspend.ths / ACCEL_MOT_DUR_LSB;
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
MPUREG_ACCEL_MOT_DUR, reg);
ERROR_CHECK(result);
reg = (unsigned char) private_data->resume.ths / ACCEL_ZRMOT_DUR_LSB;
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
MPUREG_ACCEL_ZRMOT_DUR, reg);
ERROR_CHECK(result);
return result;
}
int mantis_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = INV_SUCCESS;
unsigned char reg;
struct mantis_private_data *private_data =
(struct mantis_private_data *)pdata->private_data;
result = inv_serial_read(mlsl_handle, pdata->address,
MPUREG_PWR_MGMT_1, 1, ®);
ERROR_CHECK(result);
#if defined(CONFIG_MPU_SENSORS_MPU6050B1)
if (reg & BIT_SLEEP) {
result = inv_serial_single_write(mlsl_handle, pdata->address,
MPUREG_PWR_MGMT_1, reg & ~BIT_SLEEP);
ERROR_CHECK(result);
}
#else
if ((reg & BITS_PWRSEL) != BITS_PWRSEL) {
result = inv_serial_single_write(mlsl_handle, pdata->address,
MPUREG_PWR_MGMT_1, reg | BITS_PWRSEL);
ERROR_CHECK(result);
}
#endif
inv_sleep(2);
result = inv_serial_read(mlsl_handle, pdata->address,
MPUREG_PWR_MGMT_2, 1, ®);
ERROR_CHECK(result);
reg &= ~(BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA);
result = inv_serial_single_write(mlsl_handle, pdata->address,
MPUREG_PWR_MGMT_2, reg);
ERROR_CHECK(result);
/* settings */
result = mantis_set_fsr(mlsl_handle, pdata, &private_data->resume,
TRUE, private_data->resume.fsr);
ERROR_CHECK(result);
result = mantis_set_odr(mlsl_handle, pdata, &private_data->resume,
TRUE, private_data->resume.odr);
ERROR_CHECK(result);
result = mantis_set_irq(mlsl_handle, pdata, &private_data->resume,
TRUE, private_data->resume.irq_type);
ERROR_CHECK(result);
/* motion, no_motion */
reg = (unsigned char)private_data->suspend.ths / ACCEL_MOT_THR_LSB;
result = inv_serial_single_write(mlsl_handle, pdata->address,
MPUREG_ACCEL_MOT_THR, reg);
ERROR_CHECK(result);
reg = (unsigned char)
ACCEL_ZRMOT_THR_LSB_CONVERSION(private_data->resume.ths);
result = inv_serial_single_write(mlsl_handle, pdata->address,
MPUREG_ACCEL_ZRMOT_THR, reg);
ERROR_CHECK(result);
reg = (unsigned char)private_data->suspend.ths / ACCEL_MOT_DUR_LSB;
result = inv_serial_single_write(mlsl_handle, pdata->address,
MPUREG_ACCEL_MOT_DUR, reg);
ERROR_CHECK(result);
reg = (unsigned char)private_data->resume.ths / ACCEL_ZRMOT_DUR_LSB;
result = inv_serial_single_write(mlsl_handle, pdata->address,
MPUREG_ACCEL_ZRMOT_DUR, reg);
ERROR_CHECK(result);
return result;
}
static int mpu6050_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
unsigned char reg;
struct mpu6050_private_data *private_data =
(struct mpu6050_private_data *)pdata->private_data;
result = inv_serial_read(mlsl_handle, pdata->address,
MPUREG_PWR_MGMT_1, 1, ®);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
if (reg & BIT_SLEEP) {
result = inv_serial_single_write(mlsl_handle, pdata->address,
MPUREG_PWR_MGMT_1, reg & ~BIT_SLEEP);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
}
msleep(2);
result = inv_serial_read(mlsl_handle, pdata->address,
MPUREG_PWR_MGMT_2, 1, ®);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
reg &= ~(BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA);
result = inv_serial_single_write(mlsl_handle, pdata->address,
MPUREG_PWR_MGMT_2, reg);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* settings */
result = mpu6050_set_fsr(mlsl_handle, pdata, &private_data->resume,
true, private_data->resume.fsr);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = mpu6050_set_odr(mlsl_handle, pdata, &private_data->resume,
true, private_data->resume.odr);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = mpu6050_set_irq(mlsl_handle, pdata, &private_data->resume,
true, private_data->resume.irq_type);
/* motion, no_motion */
/* TODO : port these in their respective _set_thrs and _set_dur
functions and use the APPLY paremeter to apply just like
_set_odr, _set_irq, and _set_fsr. */
reg = (unsigned char)private_data->suspend.ths / ACCEL_MOT_THR_LSB;
result = inv_serial_single_write(mlsl_handle, pdata->address,
MPUREG_ACCEL_MOT_THR, reg);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
reg = (unsigned char)
ACCEL_ZRMOT_THR_LSB_CONVERSION(private_data->resume.ths);
result = inv_serial_single_write(mlsl_handle, pdata->address,
MPUREG_ACCEL_ZRMOT_THR, reg);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
reg = (unsigned char)private_data->suspend.ths / ACCEL_MOT_DUR_LSB;
result = inv_serial_single_write(mlsl_handle, pdata->address,
MPUREG_ACCEL_MOT_DUR, reg);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
reg = (unsigned char)private_data->resume.ths / ACCEL_ZRMOT_DUR_LSB;
result = inv_serial_single_write(mlsl_handle, pdata->address,
MPUREG_ACCEL_ZRMOT_DUR, reg);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
return 0;
}
