diff options
Diffstat (limited to 'libsensors/software/core/mllite/results_holder.c')
| -rw-r--r-- | libsensors/software/core/mllite/results_holder.c | 500 |
1 files changed, 0 insertions, 500 deletions
diff --git a/libsensors/software/core/mllite/results_holder.c b/libsensors/software/core/mllite/results_holder.c deleted file mode 100644 index 97ffdec..0000000 --- a/libsensors/software/core/mllite/results_holder.c +++ /dev/null @@ -1,500 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -/** - * @defgroup Results_Holder results_holder - * @brief Motion Library - Results Holder - * Holds the data for MPL - * - * @{ - * @file results_holder.c - * @brief Results Holder for HAL. - */ -#include "results_holder.h" -#include "log.h" -#include "ml_math_func.h" -#include "mlmath.h" -#include "start_manager.h" -#include "data_builder.h" -#include "message_layer.h" - -// These 2 status bits are used to control when the 9 axis quaternion is updated -#define INV_COMPASS_CORRECTION_SET 1 -#define INV_6_AXIS_QUAT_SET 2 - -struct results_t { - long nav_quat[4]; - long gam_quat[4]; - inv_time_t nav_timestamp; - inv_time_t gam_timestamp; - long local_field[3]; /**< local earth's magnetic field */ - long mag_scale[3]; /**< scale factor to apply to magnetic field reading */ - long compass_correction[4]; /**< quaternion going from gyro,accel quaternion to 9 axis */ - int acc_state; /**< Describes accel state */ - long compass_bias_error[3]; /**< Error Squared */ - unsigned char motion_state; - unsigned int motion_state_counter; /**< Incremented for each no motion event in a row */ - long compass_count; /**< compass state internal counter */ - int got_compass_bias; /**< Flag describing if compass bias is known */ - int large_mag_field; /**< Flag describing if there is a large magnetic field */ - int compass_state; /**< Internal compass state */ - long status; - struct inv_sensor_cal_t *sensor; - float quat_confidence_interval; -}; -static struct results_t rh; - -/** @internal -* Store a quaternion more suitable for gaming. This quaternion is often determined -* using only gyro and accel. -* @param[in] quat Length 4, Quaternion scaled by 2^30 -*/ -void inv_store_gaming_quaternion(const long *quat, inv_time_t timestamp) -{ - rh.status |= INV_6_AXIS_QUAT_SET; - memcpy(&rh.gam_quat, quat, sizeof(rh.gam_quat)); - rh.gam_timestamp = timestamp; -} - -/** @internal -* Sets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion. -* @param[in] data Quaternion Adjustment -* @param[in] timestamp Timestamp of when this is valid -*/ -void inv_set_compass_correction(const long *data, inv_time_t timestamp) -{ - rh.status |= INV_COMPASS_CORRECTION_SET; - memcpy(rh.compass_correction, data, sizeof(rh.compass_correction)); - rh.nav_timestamp = timestamp; -} - -/** @internal -* Gets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion. -* @param[out] data Quaternion Adjustment -* @param[out] timestamp Timestamp of when this is valid -*/ -void inv_get_compass_correction(long *data, inv_time_t *timestamp) -{ - memcpy(data, rh.compass_correction, sizeof(rh.compass_correction)); - *timestamp = rh.nav_timestamp; -} - -/** Returns non-zero if there is a large magnetic field. See inv_set_large_mag_field() for setting this variable. - * @return Returns non-zero if there is a large magnetic field. - */ -int inv_get_large_mag_field() -{ - return rh.large_mag_field; -} - -/** Set to non-zero if there as a large magnetic field. See inv_get_large_mag_field() for getting this variable. - * @param[in] state value to set for magnetic field strength. Should be non-zero if it is large. - */ -void inv_set_large_mag_field(int state) -{ - rh.large_mag_field = state; -} - -/** Gets the accel state set by inv_set_acc_state() - * @return accel state. - */ -int inv_get_acc_state() -{ - return rh.acc_state; -} - -/** Sets the accel state. See inv_get_acc_state() to get the value. - * @param[in] state value to set accel state to. - */ -void inv_set_acc_state(int state) -{ - rh.acc_state = state; - return; -} - -/** Returns the motion state -* @param[out] cntr Number of previous times a no motion event has occured in a row. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -int inv_get_motion_state(unsigned int *cntr) -{ - *cntr = rh.motion_state_counter; - return rh.motion_state; -} - -/** Sets the motion state - * @param[in] state motion state where INV_NO_MOTION is not moving - * and INV_MOTION is moving. - */ -void inv_set_motion_state(unsigned char state) -{ - long set; - if (state == rh.motion_state) { - if (state == INV_NO_MOTION) { - rh.motion_state_counter++; - } else { - rh.motion_state_counter = 0; - } - return; - } - rh.motion_state_counter = 0; - rh.motion_state = state; - /* Equivalent to set = state, but #define's may change. */ - if (state == INV_MOTION) - set = INV_MSG_MOTION_EVENT; - else - set = INV_MSG_NO_MOTION_EVENT; - inv_set_message(set, (INV_MSG_MOTION_EVENT | INV_MSG_NO_MOTION_EVENT), 0); -} - -/** Sets the local earth's magnetic field -* @param[in] data Local earth's magnetic field in uT scaled by 2^16. -* Length = 3. Y typically points north, Z typically points down in -* northern hemisphere and up in southern hemisphere. -*/ -void inv_set_local_field(const long *data) -{ - memcpy(rh.local_field, data, sizeof(rh.local_field)); -} - -/** Gets the local earth's magnetic field -* @param[out] data Local earth's magnetic field in uT scaled by 2^16. -* Length = 3. Y typically points north, Z typically points down in -* northern hemisphere and up in southern hemisphere. -*/ -void inv_get_local_field(long *data) -{ - memcpy(data, rh.local_field, sizeof(rh.local_field)); -} - -/** Sets the compass sensitivity - * @param[in] data Length 3, sensitivity for each compass axis - * scaled such that 1.0 = 2^30. - */ -void inv_set_mag_scale(const long *data) -{ - memcpy(rh.mag_scale, data, sizeof(rh.mag_scale)); -} - -/** Gets the compass sensitivity - * @param[out] data Length 3, sensitivity for each compass axis - * scaled such that 1.0 = 2^30. - */ -void inv_get_mag_scale(long *data) -{ - memcpy(data, rh.mag_scale, sizeof(rh.mag_scale)); -} - -/** Gets gravity vector - * @param[out] data gravity vector in body frame scaled such that 1.0 = 2^30. - * @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_get_gravity(long *data) -{ - data[0] = - inv_q29_mult(rh.nav_quat[1], rh.nav_quat[3]) - inv_q29_mult(rh.nav_quat[2], rh.nav_quat[0]); - data[1] = - inv_q29_mult(rh.nav_quat[2], rh.nav_quat[3]) + inv_q29_mult(rh.nav_quat[1], rh.nav_quat[0]); - data[2] = - (inv_q29_mult(rh.nav_quat[3], rh.nav_quat[3]) + inv_q29_mult(rh.nav_quat[0], rh.nav_quat[0])) - - 1073741824L; - - return INV_SUCCESS; -} - -/** Returns a quaternion based only on gyro and accel. - * @param[out] data 6-axis gyro and accel quaternion scaled such that 1.0 = 2^30. - * @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_get_6axis_quaternion(long *data) -{ - memcpy(data, rh.gam_quat, sizeof(rh.gam_quat)); - return INV_SUCCESS; -} - -/** Returns a quaternion. - * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30. - * @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_get_quaternion(long *data) -{ - if (rh.status & (INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET)) { - inv_q_mult(rh.compass_correction, rh.gam_quat, rh.nav_quat); - rh.status &= ~(INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET); - } - memcpy(data, rh.nav_quat, sizeof(rh.nav_quat)); - return INV_SUCCESS; -} - -/** Returns a quaternion. - * @param[out] data 9-axis quaternion. - * @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_get_quaternion_float(float *data) -{ - long ldata[4]; - inv_error_t result = inv_get_quaternion(ldata); - data[0] = inv_q30_to_float(ldata[0]); - data[1] = inv_q30_to_float(ldata[1]); - data[2] = inv_q30_to_float(ldata[2]); - data[3] = inv_q30_to_float(ldata[3]); - return result; -} - -/** Returns a quaternion with accuracy and timestamp. - * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30. - * @param[out] accuracy Accuracy of quaternion, 0-3, where 3 is most accurate. - * @param[out] timestamp Timestamp of this quaternion in nanoseconds - */ -void inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp) -{ - inv_get_quaternion(data); - *timestamp = inv_get_last_timestamp(); - if (inv_get_compass_on()) { - *accuracy = inv_get_mag_accuracy(); - } else if (inv_get_gyro_on()) { - *accuracy = inv_get_gyro_accuracy(); - }else if (inv_get_accel_on()) { - *accuracy = inv_get_accel_accuracy(); - } else { - *accuracy = 0; - } -} - -/** Callback that gets called everytime there is new data. It is - * registered by inv_start_results_holder(). - * @param[in] sensor_cal New sensor data to process. - * @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_generate_results(struct inv_sensor_cal_t *sensor_cal) -{ - rh.sensor = sensor_cal; - return INV_SUCCESS; -} - -/** Function to turn on this module. This is automatically called by - * inv_enable_results_holder(). Typically not called by users. - * @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_start_results_holder(void) -{ - inv_register_data_cb(inv_generate_results, INV_PRIORITY_RESULTS_HOLDER, - INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW); - return INV_SUCCESS; -} - -/** Initializes results holder. This is called automatically by the -* enable function inv_enable_results_holder(). It may be called any time the feature is enabled, but -* is typically not needed to be called by outside callers. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_init_results_holder(void) -{ - memset(&rh, 0, sizeof(rh)); - rh.mag_scale[0] = 1L<<30; - rh.mag_scale[1] = 1L<<30; - rh.mag_scale[2] = 1L<<30; - rh.compass_correction[0] = 1L<<30; - rh.gam_quat[0] = 1L<<30; - rh.nav_quat[0] = 1L<<30; - rh.quat_confidence_interval = (float)M_PI; - return INV_SUCCESS; -} - -/** Turns on storage of results. -*/ -inv_error_t inv_enable_results_holder() -{ - inv_error_t result; - result = inv_init_results_holder(); - if ( result ) { - return result; - } - - result = inv_register_mpl_start_notification(inv_start_results_holder); - return result; -} - -/** Sets state of if we know the compass bias. - * @return return 1 if we know the compass bias, 0 if not. - * it is set with inv_set_compass_bias_found() - */ -int inv_got_compass_bias() -{ - return rh.got_compass_bias; -} - -/** Sets whether we know the compass bias - * @param[in] state Set to 1 if we know the compass bias. - * Can be retrieved with inv_got_compass_bias() - */ -void inv_set_compass_bias_found(int state) -{ - rh.got_compass_bias = state; -} - -/** Sets the compass state. - * @param[in] state Compass state. It can be retrieved with inv_get_compass_state(). - */ -void inv_set_compass_state(int state) -{ - rh.compass_state = state; -} - -/** Get's the compass state - * @return the compass state that was set with inv_set_compass_state() - */ -int inv_get_compass_state() -{ - return rh.compass_state; -} - -/** Set compass bias error. See inv_get_compass_bias_error() - * @param[in] bias_error Set's how accurate we know the compass bias. It is the - * error squared. - */ -void inv_set_compass_bias_error(const long *bias_error) -{ - memcpy(rh.compass_bias_error, bias_error, sizeof(rh.compass_bias_error)); -} - -/** Get's compass bias error. See inv_set_compass_bias_error() for setting. - * @param[out] bias_error Accuracy as to how well the compass bias is known. It is the error squared. - */ -void inv_get_compass_bias_error(long *bias_error) -{ - memcpy(bias_error, rh.compass_bias_error, sizeof(rh.compass_bias_error)); -} - -/** - * @brief Returns 3-element vector of accelerometer data in body frame - * with gravity removed - * @param[out] data 3-element vector of accelerometer data in body frame - * with gravity removed - * @return INV_SUCCESS if successful - * INV_ERROR_INVALID_PARAMETER if invalid input pointer - */ -inv_error_t inv_get_linear_accel(long *data) -{ - long gravity[3]; - - if (data != NULL) - { - inv_get_accel_set(data, NULL, NULL); - inv_get_gravity(gravity); - data[0] -= gravity[0] >> 14; - data[1] -= gravity[1] >> 14; - data[2] -= gravity[2] >> 14; - return INV_SUCCESS; - } - else { - return INV_ERROR_INVALID_PARAMETER; - } -} - -/** - * @brief Returns 3-element vector of accelerometer data in body frame - * @param[out] data 3-element vector of accelerometer data in body frame - * @return INV_SUCCESS if successful - * INV_ERROR_INVALID_PARAMETER if invalid input pointer - */ -inv_error_t inv_get_accel(long *data) -{ - if (data != NULL) { - inv_get_accel_set(data, NULL, NULL); - return INV_SUCCESS; - } - else { - return INV_ERROR_INVALID_PARAMETER; - } -} - -/** - * @brief Returns 3-element vector of accelerometer float data - * @param[out] data 3-element vector of accelerometer float data - * @return INV_SUCCESS if successful - * INV_ERROR_INVALID_PARAMETER if invalid input pointer - */ -inv_error_t inv_get_accel_float(float *data) -{ - long tdata[3]; - unsigned char i; - - if (data != NULL && !inv_get_accel(tdata)) { - for (i = 0; i < 3; ++i) { - data[i] = ((float)tdata[i] / (1L << 16)); - } - return INV_SUCCESS; - } - else { - return INV_ERROR_INVALID_PARAMETER; - } -} - -/** - * @brief Returns 3-element vector of gyro float data - * @param[out] data 3-element vector of gyro float data - * @return INV_SUCCESS if successful - * INV_ERROR_INVALID_PARAMETER if invalid input pointer - */ -inv_error_t inv_get_gyro_float(float *data) -{ - long tdata[3]; - unsigned char i; - - if (data != NULL) { - inv_get_gyro_set(tdata, NULL, NULL); - for (i = 0; i < 3; ++i) { - data[i] = ((float)tdata[i] / (1L << 16)); - } - return INV_SUCCESS; - } - else { - return INV_ERROR_INVALID_PARAMETER; - } -} - -/** Set 9 axis 95% heading confidence interval for quaternion -* @param[in] ci Confidence interval in radians. -*/ -void inv_set_heading_confidence_interval(float ci) -{ - rh.quat_confidence_interval = ci; -} - -/** Get 9 axis 95% heading confidence interval for quaternion -* @return Confidence interval in radians. -*/ -float inv_get_heading_confidence_interval(void) -{ - return rh.quat_confidence_interval; -} - -/** - * @brief Returns 3-element vector of linear accel float data - * @param[out] data 3-element vector of linear aceel float data - * @return INV_SUCCESS if successful - * INV_ERROR_INVALID_PARAMETER if invalid input pointer - */ -inv_error_t inv_get_linear_accel_float(float *data) -{ - long tdata[3]; - unsigned char i; - - if (data != NULL && !inv_get_linear_accel(tdata)) { - for (i = 0; i < 3; ++i) { - data[i] = ((float)tdata[i] / (1L << 16)); - } - return INV_SUCCESS; - } - else { - return INV_ERROR_INVALID_PARAMETER; - } -} - -/** - * @} - */ |