| /* |
| $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 <string.h> |
| |
| #include "results_holder.h" |
| #include "ml_math_func.h" |
| #include "mlmath.h" |
| #include "start_manager.h" |
| #include "data_builder.h" |
| #include "message_layer.h" |
| #include "log.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 */ |
| int got_accel_bias; /**< Flag describing if accel bias is known */ |
| 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 accel bias. |
| * @return return 1 if we know the accel bias, 0 if not. |
| * it is set with inv_set_accel_bias_found() |
| */ |
| int inv_got_accel_bias() |
| { |
| return rh.got_accel_bias; |
| } |
| |
| /** Sets whether we know the accel bias |
| * @param[in] state Set to 1 if we know the accel bias. |
| * Can be retrieved with inv_got_accel_bias() |
| */ |
| void inv_set_accel_bias_found(int state) |
| { |
| rh.got_accel_bias = state; |
| } |
| |
| /** 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; |
| } |
| } |
| |
| /** |
| * @} |
| */ |