diff options
Diffstat (limited to 'libsensors/mlsdk/mllite/mlcontrol.c')
| -rw-r--r-- | libsensors/mlsdk/mllite/mlcontrol.c | 797 |
1 files changed, 797 insertions, 0 deletions
diff --git a/libsensors/mlsdk/mllite/mlcontrol.c b/libsensors/mlsdk/mllite/mlcontrol.c new file mode 100644 index 0000000..278438a --- /dev/null +++ b/libsensors/mlsdk/mllite/mlcontrol.c @@ -0,0 +1,797 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/******************************************************************************* + * + * $Id: mlcontrol.c 5641 2011-06-14 02:10:02Z mcaramello $ + * + *******************************************************************************/ + +/** + * @defgroup CONTROL + * @brief Motion Library - Control Engine. + * The Control Library processes gyroscopes, accelerometers, and + * compasses to provide control signals that can be used in user + * interfaces. + * These signals can be used to manipulate objects such as documents, + * images, cursors, menus, etc. + * + * @{ + * @file mlcontrol.c + * @brief The Control Library. + * + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#include "mltypes.h" +#include "mlinclude.h" +#include "mltypes.h" +#include "ml.h" +#include "mlos.h" +#include "mlsl.h" +#include "mldl.h" +#include "mlcontrol.h" +#include "dmpKey.h" +#include "mlstates.h" +#include "mlFIFO.h" +#include "string.h" + +/* - Global Vars. - */ +struct control_params cntrl_params = { + { + MLCTRL_SENSITIVITY_0_DEFAULT, + MLCTRL_SENSITIVITY_1_DEFAULT, + MLCTRL_SENSITIVITY_2_DEFAULT, + MLCTRL_SENSITIVITY_3_DEFAULT}, // sensitivity + MLCTRL_FUNCTIONS_DEFAULT, // functions + { + MLCTRL_PARAMETER_ARRAY_0_DEFAULT, + MLCTRL_PARAMETER_ARRAY_1_DEFAULT, + MLCTRL_PARAMETER_ARRAY_2_DEFAULT, + MLCTRL_PARAMETER_ARRAY_3_DEFAULT}, // parameterArray + { + MLCTRL_PARAMETER_AXIS_0_DEFAULT, + MLCTRL_PARAMETER_AXIS_1_DEFAULT, + MLCTRL_PARAMETER_AXIS_2_DEFAULT, + MLCTRL_PARAMETER_AXIS_3_DEFAULT}, // parameterAxis + { + MLCTRL_GRID_THRESHOLD_0_DEFAULT, + MLCTRL_GRID_THRESHOLD_1_DEFAULT, + MLCTRL_GRID_THRESHOLD_2_DEFAULT, + MLCTRL_GRID_THRESHOLD_3_DEFAULT}, // gridThreshold + { + MLCTRL_GRID_MAXIMUM_0_DEFAULT, + MLCTRL_GRID_MAXIMUM_1_DEFAULT, + MLCTRL_GRID_MAXIMUM_2_DEFAULT, + MLCTRL_GRID_MAXIMUM_3_DEFAULT}, // gridMaximum + MLCTRL_GRID_CALLBACK_DEFAULT // gridCallback +}; + +/* - Extern Vars. - */ +struct control_obj cntrl_obj; +extern const unsigned char *dmpConfig1; + +/* -------------- */ +/* - Functions. - */ +/* -------------- */ + +/** + * @brief inv_set_control_sensitivity is used to set the sensitivity for a control + * signal. + * + * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or + * inv_open_low_power_pedometer(). + * + * @param controlSignal Indicates which control signal is being modified. + * Must be one of: + * - INV_CONTROL_1, + * - INV_CONTROL_2, + * - INV_CONTROL_3 or + * - INV_CONTROL_4. + * + * @param sensitivity The sensitivity of the control signal. + * + * @return error code + */ +inv_error_t inv_set_control_sensitivity(unsigned short controlSignal, + long sensitivity) +{ + INVENSENSE_FUNC_START; + unsigned char regs[2]; + long finalSens = 0; + inv_error_t result; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + finalSens = sensitivity * 100; + if (finalSens > 16384) { + finalSens = 16384; + } + regs[0] = (unsigned char)(finalSens / 256); + regs[1] = (unsigned char)(finalSens % 256); + switch (controlSignal) { + case INV_CONTROL_1: + result = inv_set_mpu_memory(KEY_D_0_224, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + cntrl_params.sensitivity[0] = (unsigned short)sensitivity; + break; + case INV_CONTROL_2: + result = inv_set_mpu_memory(KEY_D_0_228, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + cntrl_params.sensitivity[1] = (unsigned short)sensitivity; + break; + case INV_CONTROL_3: + result = inv_set_mpu_memory(KEY_D_0_232, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + cntrl_params.sensitivity[2] = (unsigned short)sensitivity; + break; + case INV_CONTROL_4: + result = inv_set_mpu_memory(KEY_D_0_236, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + cntrl_params.sensitivity[3] = (unsigned short)sensitivity; + break; + default: + break; + } + if (finalSens != sensitivity * 100) { + return INV_ERROR_INVALID_PARAMETER; + } else { + return INV_SUCCESS; + } +} + +/** + * @brief inv_set_control_func allows the user to choose how the sensor data will + * be processed in order to provide a control parameter. + * inv_set_control_func allows the user to choose which control functions + * will be incorporated in the sensor data processing. + * The control functions are: + * - INV_GRID + * Indicates that the user will be controlling a system that + * has discrete steps, such as icons, menu entries, pixels, etc. + * - INV_SMOOTH + * Indicates that noise from unintentional motion should be filtered out. + * - INV_DEAD_ZONE + * Indicates that a dead zone should be used, below which sensor + * data is set to zero. + * - INV_HYSTERESIS + * Indicates that, when INV_GRID is selected, hysteresis should + * be used to prevent the control signal from switching rapidly across + * elements of the grid. + * + * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or + * inv_open_low_power_pedometer(). + * + * @param function Indicates what functions will be used. + * Can be a bitwise OR of several values. + * + * @return Zero if the command is successful; an ML error code otherwise. + */ +inv_error_t inv_set_control_func(unsigned short function) +{ + INVENSENSE_FUNC_START; + unsigned char regs[8] = { DINA06, DINA26, + DINA46, DINA66, + DINA0E, DINA2E, + DINA4E, DINA6E + }; + unsigned char i; + inv_error_t result; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if ((function & INV_SMOOTH) == 0) { + for (i = 0; i < 8; i++) { + regs[i] = DINA80 + 3; + } + } + result = inv_set_mpu_memory(KEY_CFG_4, 8, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + cntrl_params.functions = function; + result = inv_set_dead_zone(); + + return result; +} + +/** + * @brief inv_get_control_signal is used to get the current control signal with + * high precision. + * inv_get_control_signal is used to acquire the current data of a control signal. + * If INV_GRID is being used, inv_get_grid_number will probably be preferrable. + * + * @param controlSignal Indicates which control signal is being queried. + * Must be one of: + * - INV_CONTROL_1, + * - INV_CONTROL_2, + * - INV_CONTROL_3 or + * - INV_CONTROL_4. + * + * @param reset Indicates whether the control signal should be reset to zero. + * Options are INV_RESET or INV_NO_RESET + * @param data A pointer to the current control signal data. + * + * @return Zero if the command is successful; an ML error code otherwise. + */ +inv_error_t inv_get_control_signal(unsigned short controlSignal, + unsigned short reset, long *data) +{ + INVENSENSE_FUNC_START; + + if (inv_get_state() != INV_STATE_DMP_STARTED) + return INV_ERROR_SM_IMPROPER_STATE; + + switch (controlSignal) { + case INV_CONTROL_1: + *data = cntrl_obj.controlInt[0]; + if (reset == INV_RESET) { + cntrl_obj.controlInt[0] = 0; + } + break; + case INV_CONTROL_2: + *data = cntrl_obj.controlInt[1]; + if (reset == INV_RESET) { + cntrl_obj.controlInt[1] = 0; + } + break; + case INV_CONTROL_3: + *data = cntrl_obj.controlInt[2]; + if (reset == INV_RESET) { + cntrl_obj.controlInt[2] = 0; + } + break; + case INV_CONTROL_4: + *data = cntrl_obj.controlInt[3]; + if (reset == INV_RESET) { + cntrl_obj.controlInt[3] = 0; + } + break; + default: + break; + } + return INV_SUCCESS; +} + +/** + * @brief inv_get_grid_num is used to get the current grid location for a certain + * control signal. + * inv_get_grid_num is used to acquire the current grid location. + * + * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or + * inv_open_low_power_pedometer(). + * + * @param controlSignal Indicates which control signal is being queried. + * Must be one of: + * - INV_CONTROL_1, + * - INV_CONTROL_2, + * - INV_CONTROL_3 or + * - INV_CONTROL_4. + * + * @param reset Indicates whether the control signal should be reset to zero. + * Options are INV_RESET or INV_NO_RESET + * @param data A pointer to the current grid number. + * + * @return Zero if the command is successful; an ML error code otherwise. + */ + +inv_error_t inv_get_grid_num(unsigned short controlSignal, unsigned short reset, + long *data) +{ + INVENSENSE_FUNC_START; + + if (inv_get_state() != INV_STATE_DMP_STARTED) + return INV_ERROR_SM_IMPROPER_STATE; + + switch (controlSignal) { + case INV_CONTROL_1: + *data = cntrl_obj.gridNum[0]; + if (reset == INV_RESET) { + cntrl_obj.gridNum[0] = 0; + } + break; + case INV_CONTROL_2: + *data = cntrl_obj.gridNum[1]; + if (reset == INV_RESET) { + cntrl_obj.gridNum[1] = 0; + } + break; + case INV_CONTROL_3: + *data = cntrl_obj.gridNum[2]; + if (reset == INV_RESET) { + cntrl_obj.gridNum[2] = 0; + } + break; + case INV_CONTROL_4: + *data = cntrl_obj.gridNum[3]; + if (reset == INV_RESET) { + cntrl_obj.gridNum[3] = 0; + } + break; + default: + break; + } + + return INV_SUCCESS; +} + +/** + * @brief inv_set_grid_thresh is used to set the grid size for a control signal. + * inv_set_grid_thresh is used to adjust the size of the grid being controlled. + * @param controlSignal Indicates which control signal is being modified. + * Must be one of: + * - INV_CONTROL_1, + * - INV_CONTROL_2, + * - INV_CONTROL_3 and + * - INV_CONTROL_4. + * @param threshold The threshold of the control signal at which the grid + * number will be incremented or decremented. + * @return Zero if the command is successful; an ML error code otherwise. + */ + +inv_error_t inv_set_grid_thresh(unsigned short controlSignal, long threshold) +{ + INVENSENSE_FUNC_START; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + switch (controlSignal) { + case INV_CONTROL_1: + cntrl_params.gridThreshold[0] = threshold; + break; + case INV_CONTROL_2: + cntrl_params.gridThreshold[1] = threshold; + break; + case INV_CONTROL_3: + cntrl_params.gridThreshold[2] = threshold; + break; + case INV_CONTROL_4: + cntrl_params.gridThreshold[3] = threshold; + break; + default: + return INV_ERROR_INVALID_PARAMETER; + break; + } + + return INV_SUCCESS; +} + +/** + * @brief inv_set_grid_max is used to set the maximum grid number for a control signal. + * inv_set_grid_max is used to adjust the maximum allowed grid number, above + * which the grid number will not be incremented. + * The minimum grid number is always zero. + * + * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or + * inv_open_low_power_pedometer(). + * + * @param controlSignal Indicates which control signal is being modified. + * Must be one of: + * - INV_CONTROL_1, + * - INV_CONTROL_2, + * - INV_CONTROL_3 and + * - INV_CONTROL_4. + * + * @param maximum The maximum grid number for a control signal. + * @return Zero if the command is successful; an ML error code otherwise. + */ + +inv_error_t inv_set_grid_max(unsigned short controlSignal, long maximum) +{ + INVENSENSE_FUNC_START; + + if (inv_get_state() != INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + switch (controlSignal) { + case INV_CONTROL_1: + cntrl_params.gridMaximum[0] = maximum; + break; + case INV_CONTROL_2: + cntrl_params.gridMaximum[1] = maximum; + break; + case INV_CONTROL_3: + cntrl_params.gridMaximum[2] = maximum; + break; + case INV_CONTROL_4: + cntrl_params.gridMaximum[3] = maximum; + break; + default: + return INV_ERROR_INVALID_PARAMETER; + break; + } + + return INV_SUCCESS; +} + +/** + * @brief GridCallback function pointer type, to be passed as argument of + * inv_set_grid_callback. + * + * @param controlSignal Indicates which control signal crossed a grid threshold. + * Must be one of: + * - INV_CONTROL_1, + * - INV_CONTROL_2, + * - INV_CONTROL_3 and + * - INV_CONTROL_4. + * + * @param gridNumber An array of four numbers representing the grid number for each + * control signal. + * @param gridChange An array of four numbers representing the change in grid number + * for each control signal. +**/ +typedef void (*fpGridCb) (unsigned short controlSignal, long *gridNum, + long *gridChange); + +/** + * @brief inv_set_grid_callback is used to register a callback function that + * will trigger when the grid location changes. + * inv_set_grid_callback allows a user to define a callback function that will + * run when a control signal crosses a grid threshold. + + * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or + * inv_open_low_power_pedometer(). inv_dmp_start must <b>NOT</b> have + * been called. + * + * @param func A user defined callback function + * @return Zero if the command is successful; an ML error code otherwise. +**/ +inv_error_t inv_set_grid_callback(fpGridCb func) +{ + INVENSENSE_FUNC_START; + + if (inv_get_state() != INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + cntrl_params.gridCallback = func; + return INV_SUCCESS; +} + +/** + * @brief inv_set_control_data is used to assign physical parameters to control signals. + * inv_set_control_data allows flexibility in assigning physical parameters to + * control signals. For example, the user is allowed to use raw gyroscope data + * as an input to the control algorithm. + * Alternatively, angular velocity can be used, which combines gyroscopes and + * accelerometers to provide a more robust physical parameter. Finally, angular + * velocity in world coordinates can be used, providing a control signal in + * which pitch and yaw are provided relative to gravity. + * + * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or + * inv_open_low_power_pedometer(). + * + * @param controlSignal Indicates which control signal is being modified. + * Must be one of: + * - INV_CONTROL_1, + * - INV_CONTROL_2, + * - INV_CONTROL_3 or + * - INV_CONTROL_4. + * + * @param parameterArray Indicates which parameter array is being assigned to a + * control signal. Must be one of: + * - INV_GYROS, + * - INV_ANGULAR_VELOCITY, or + * + * @param parameterAxis Indicates which axis of the parameter array will be used. + * Must be: + * - INV_ROLL, + * - INV_PITCH, or + * - INV_YAW. + */ + +inv_error_t inv_set_control_data(unsigned short controlSignal, + unsigned short parameterArray, + unsigned short parameterAxis) +{ + INVENSENSE_FUNC_START; + unsigned char regs[2] = { DINA80 + 10, DINA20 }; + inv_error_t result; + + if (inv_get_state() != INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (parameterArray == INV_ANGULAR_VELOCITY) { + regs[0] = DINA80 + 5; + regs[1] = DINA00; + } + switch (controlSignal) { + case INV_CONTROL_1: + cntrl_params.parameterArray[0] = parameterArray; + switch (parameterAxis) { + case INV_PITCH: + regs[1] += 0x02; + cntrl_params.parameterAxis[0] = 0; + break; + case INV_ROLL: + regs[1] = DINA22; + cntrl_params.parameterAxis[0] = 1; + break; + case INV_YAW: + regs[1] = DINA42; + cntrl_params.parameterAxis[0] = 2; + break; + default: + return INV_ERROR_INVALID_PARAMETER; + } + result = inv_set_mpu_memory(KEY_CFG_3, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + break; + case INV_CONTROL_2: + cntrl_params.parameterArray[1] = parameterArray; + switch (parameterAxis) { + case INV_PITCH: + regs[1] += DINA0E; + cntrl_params.parameterAxis[1] = 0; + break; + case INV_ROLL: + regs[1] += DINA2E; + cntrl_params.parameterAxis[1] = 1; + break; + case INV_YAW: + regs[1] += DINA4E; + cntrl_params.parameterAxis[1] = 2; + break; + default: + return INV_ERROR_INVALID_PARAMETER; + } + result = inv_set_mpu_memory(KEY_CFG_3B, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + break; + case INV_CONTROL_3: + cntrl_params.parameterArray[2] = parameterArray; + switch (parameterAxis) { + case INV_PITCH: + regs[1] += DINA0E; + cntrl_params.parameterAxis[2] = 0; + break; + case INV_ROLL: + regs[1] += DINA2E; + cntrl_params.parameterAxis[2] = 1; + break; + case INV_YAW: + regs[1] += DINA4E; + cntrl_params.parameterAxis[2] = 2; + break; + default: + return INV_ERROR_INVALID_PARAMETER; + } + result = inv_set_mpu_memory(KEY_CFG_3C, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + break; + case INV_CONTROL_4: + cntrl_params.parameterArray[3] = parameterArray; + switch (parameterAxis) { + case INV_PITCH: + regs[1] += DINA0E; + cntrl_params.parameterAxis[3] = 0; + break; + case INV_ROLL: + regs[1] += DINA2E; + cntrl_params.parameterAxis[3] = 1; + break; + case INV_YAW: + regs[1] += DINA4E; + cntrl_params.parameterAxis[3] = 2; + break; + default: + return INV_ERROR_INVALID_PARAMETER; + } + result = inv_set_mpu_memory(KEY_CFG_3D, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + break; + default: + result = INV_ERROR_INVALID_PARAMETER; + break; + } + return result; +} + +/** + * @brief inv_get_control_data is used to get the current control data. + * + * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or + * inv_open_low_power_pedometer(). + * + * @param controlSignal Indicates which control signal is being queried. + * Must be one of: + * - INV_CONTROL_1, + * - INV_CONTROL_2, + * - INV_CONTROL_3 or + * - INV_CONTROL_4. + * + * @param gridNum A pointer to pass gridNum info back to the user. + * @param gridChange A pointer to pass gridChange info back to the user. + * + * @return Zero if the command is successful; an ML error code otherwise. + */ + +inv_error_t inv_get_control_data(long *controlSignal, long *gridNum, + long *gridChange) +{ + INVENSENSE_FUNC_START; + int_fast8_t i = 0; + + if (inv_get_state() != INV_STATE_DMP_STARTED) + return INV_ERROR_SM_IMPROPER_STATE; + + for (i = 0; i < 4; i++) { + controlSignal[i] = cntrl_obj.controlInt[i]; + gridNum[i] = cntrl_obj.gridNum[i]; + gridChange[i] = cntrl_obj.gridChange[i]; + } + return INV_SUCCESS; +} + +/** + * @internal + * @brief Update the ML Control engine. This function should be called + * every time new data from the MPU becomes available. + * Control engine outputs are written to the cntrl_obj data + * structure. + * @return INV_SUCCESS or an error code. +**/ +inv_error_t inv_update_control(struct inv_obj_t * inv_obj __unused) +{ + INVENSENSE_FUNC_START; + unsigned char i; + long gridTmp; + long tmp; + + inv_get_cntrl_data(cntrl_obj.mlGridNumDMP); + + for (i = 0; i < 4; i++) { + if (cntrl_params.functions & INV_GRID) { + if (cntrl_params.functions & INV_HYSTERESIS) { + cntrl_obj.mlGridNumDMP[i] += cntrl_obj.gridNumOffset[i]; + } + cntrl_obj.mlGridNumDMP[i] = + cntrl_obj.mlGridNumDMP[i] / 2 + 1073741824L; + cntrl_obj.controlInt[i] = + (cntrl_obj.mlGridNumDMP[i] % + (128 * cntrl_params.gridThreshold[i])) / 128; + gridTmp = + cntrl_obj.mlGridNumDMP[i] / (128 * + cntrl_params.gridThreshold[i]); + tmp = 1 + 16777216L / cntrl_params.gridThreshold[i]; + cntrl_obj.gridChange[i] = gridTmp - cntrl_obj.lastGridNum[i]; + if (cntrl_obj.gridChange[i] > tmp / 2) { + cntrl_obj.gridChange[i] = + gridTmp - tmp - cntrl_obj.lastGridNum[i]; + } else if (cntrl_obj.gridChange[i] < -tmp / 2) { + cntrl_obj.gridChange[i] = + gridTmp + tmp - cntrl_obj.lastGridNum[i]; + } + if ((cntrl_params.functions & INV_HYSTERESIS) + && (cntrl_obj.gridChange[i] != 0)) { + if (cntrl_obj.gridChange[i] > 0) { + cntrl_obj.gridNumOffset[i] += + 128 * cntrl_params.gridThreshold[i]; + cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2; + } + if (cntrl_obj.gridChange[i] < 0) { + cntrl_obj.gridNumOffset[i] -= + 128 * cntrl_params.gridThreshold[i]; + cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2; + } + } + cntrl_obj.gridNum[i] += cntrl_obj.gridChange[i]; + if (cntrl_obj.gridNum[i] >= cntrl_params.gridMaximum[i]) { + cntrl_obj.gridNum[i] = cntrl_params.gridMaximum[i]; + if (cntrl_obj.controlInt[i] >= + cntrl_params.gridThreshold[i] / 2) { + cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2; + } + } else if (cntrl_obj.gridNum[i] <= 0) { + cntrl_obj.gridNum[i] = 0; + if (cntrl_obj.controlInt[i] < cntrl_params.gridThreshold[i] / 2) { + cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2; + } + } + cntrl_obj.lastGridNum[i] = gridTmp; + if ((cntrl_params.gridCallback) && (cntrl_obj.gridChange[i] != 0)) { + cntrl_params.gridCallback((INV_CONTROL_1 << i), + cntrl_obj.gridNum, + cntrl_obj.gridChange); + } + + } else { + cntrl_obj.controlInt[i] = cntrl_obj.mlGridNumDMP[i]; + } + + } + + return INV_SUCCESS; +} + +/** + * @brief Enables the INV_CONTROL engine. + * + * @note This function replaces MLEnable(INV_CONTROL) + * + * @pre inv_dmp_open() with MLDmpDefaultOpen or MLDmpPedometerStandAlone() must + * have been called. + * + * @return INV_SUCCESS or non-zero error code + */ +inv_error_t inv_enable_control(void) +{ + INVENSENSE_FUNC_START; + + if (inv_get_state() != INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + memset(&cntrl_obj, 0, sizeof(cntrl_obj)); + + inv_register_fifo_rate_process(inv_update_control, INV_PRIORITY_CONTROL); // fixme, someone needs to send control data to the fifo + return INV_SUCCESS; +} + +/** + * @brief Disables the INV_CONTROL engine. + * + * @note This function replaces MLDisable(INV_CONTROL) + * + * @pre inv_dmp_open() with MLDmpDefaultOpen or MLDmpPedometerStandAlone() must + * have been called. + * + * @return INV_SUCCESS or non-zero error code + */ +inv_error_t inv_disable_control(void) +{ + INVENSENSE_FUNC_START; + + if (inv_get_state() < INV_STATE_DMP_STARTED) + return INV_ERROR_SM_IMPROPER_STATE; + + return INV_SUCCESS; +} + +/** + * @} + */ |