diff options
Diffstat (limited to 'libsensors/mlsdk/mllite/mlcontrol.c')
| -rw-r--r-- | libsensors/mlsdk/mllite/mlcontrol.c | 670 |
1 files changed, 6 insertions, 664 deletions
diff --git a/libsensors/mlsdk/mllite/mlcontrol.c b/libsensors/mlsdk/mllite/mlcontrol.c index 278438a..78f8e05 100644 --- a/libsensors/mlsdk/mllite/mlcontrol.c +++ b/libsensors/mlsdk/mllite/mlcontrol.c @@ -94,545 +94,6 @@ extern const unsigned char *dmpConfig1; /* -------------- */ /** - * @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 @@ -650,10 +111,15 @@ inv_error_t inv_set_control_data(unsigned short controlSignal, * * @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) { + /* NOTE: + * This symbol is referenced by libinvensense_mpl.so + * That is the sole reason this is being kept around. + * It's integrated with some other junk here though, + * so for the moment I'm not comfortable moving it. + */ INVENSENSE_FUNC_START; int_fast8_t i = 0; @@ -669,129 +135,5 @@ inv_error_t inv_get_control_data(long *controlSignal, long *gridNum, } /** - * @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; -} - -/** * @} */ |