/**
* @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)
{
umplStartMPU();
}
if (umplState == UMPL_RUN)
{
fifo_rate = inv_get_fifo_rate();
}
if (umplState == UMPL_RUN || umplState == UMPL_ACCEL_ONLY)
{
if( freq <= 1.25 )
inv_set_fifo_rate(159); //fifo rate = 1.25 hz
else if( freq <= 2.5 && freq > 1.25 )
inv_set_fifo_rate(79); //fifo rate = 2.5 hz
else if( freq <= 10.0 && freq > 2.5 )
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)
{
umplStartMPU();
}
if (umplState == UMPL_RUN)
{
fifo_rate = inv_get_fifo_rate();
}
if (umplState == UMPL_RUN || umplState == UMPL_LPACCEL_ONLY)
{
if( freq <= 18.0 )
inv_set_fifo_rate(10); //fifo rate = 18 hz
else if( freq <= 50.0 && freq > 18.0 )
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;
}
/**
* @brief umplStop stops the uMPL state machine. This function
* in turn calls the necessary MPL functions like inv_dmp_stop()
* and inv_dmp_close() to stop the motion processing algorithms.
*
*
* @pre umplInit() and umplStartMPU() must have been called.
*
* @return INV_SUCCESS if successful, a non-zero error code otherwise.
*/
inv_error_t umplStop(void)
{
inv_error_t result;
if (umplState == UMPL_RUN)
{
#ifndef UMPL_DISABLE_STORE_CAL
result = inv_ustore_calibration();
if (result != INV_SUCCESS)
MPL_LOGE("inv_ustore_calibration failed with %d in umplStop\n",result);
#endif
}
if (umplState == UMPL_RUN || umplState == UMPL_ACCEL_ONLY ||
umplState == UMPL_LPACCEL_ONLY)
{
result = inv_dmp_stop();
if (result != INV_SUCCESS) return result;
result = inv_dmp_close();
if (result != INV_SUCCESS) return result;
umplSetState(UMPL_STOP);
return INV_SUCCESS;
}
return INV_ERROR_SM_IMPROPER_STATE;
}
/**
* @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;
}
/**
* @brief umplInit is the entry point to uMPL and must be called before
* all other functions. It initializes the platform specific data.
* It also initializes the MPL state.
*
*
* @param data
* A dummy port number. Could be NULL.
*
*/
inv_error_t umplInit(unsigned char *port)
{
if (umplState == UMPL_UNINIT)
{
inv_error_t result;
result = inv_ustore_enable();
if (result != INV_SUCCESS) return result;
result = inv_init_ustore_lite_fusion();
if (result != INV_SUCCESS) return result;
result = umplPlatformSetup();
if (result != INV_SUCCESS) return result;
result = inv_serial_start((char const *)port);
if (result != INV_SUCCESS) return result;
umplSetState(UMPL_STOP);
return INV_SUCCESS;
}
return INV_ERROR_SM_IMPROPER_STATE;
}
