149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* 249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow $License: 349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow See included License.txt for License information. 549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow $ 649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @defgroup Results_Holder results_holder 949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @brief Motion Library - Results Holder 1049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * Holds the data for MPL 1149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * 1249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @{ 1349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @file results_holder.c 1449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @brief Results Holder for HAL. 1549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 1649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <string.h> 1849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "results_holder.h" 2049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "ml_math_func.h" 2149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "mlmath.h" 2249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "start_manager.h" 2349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "data_builder.h" 2449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "message_layer.h" 2549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "log.h" 2649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 2749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow// These 2 status bits are used to control when the 9 axis quaternion is updated 2849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define INV_COMPASS_CORRECTION_SET 1 2949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define INV_6_AXIS_QUAT_SET 2 3049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define INV_GEOMAGNETIC_CORRECTION_SET 4 3149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 3249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstruct results_t { 3349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long nav_quat[4]; 3449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long gam_quat[4]; 3549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long geomag_quat[4]; 3649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long accel_quat[4]; 3749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t nav_timestamp; 3849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t gam_timestamp; 3949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_time_t geomag_timestamp; 4049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long local_field[3]; /**< local earth's magnetic field */ 4149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long mag_scale[3]; /**< scale factor to apply to magnetic field reading */ 4249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long compass_correction[4]; /**< quaternion going from gyro,accel quaternion to 9 axis */ 4349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long geomag_compass_correction[4]; /**< quaternion going from accel quaternion to geomag sensor fusion */ 4449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int acc_state; /**< Describes accel state */ 4549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int got_accel_bias; /**< Flag describing if accel bias is known */ 4649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long compass_bias_error[3]; /**< Error Squared */ 4749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned char motion_state; 4849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned int motion_state_counter; /**< Incremented for each no motion event in a row */ 4949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long compass_count; /**< compass state internal counter */ 5049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int got_compass_bias; /**< Flag describing if compass bias is known */ 5149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int large_mag_field; /**< Flag describing if there is a large magnetic field */ 5249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int compass_state; /**< Internal compass state */ 5349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long status; 5449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow struct inv_sensor_cal_t *sensor; 5549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float quat_confidence_interval; 5649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float geo_mag_confidence_interval; 5749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow}; 5849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic struct results_t rh; 5949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 6049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** @internal 6149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Store a quaternion more suitable for gaming. This quaternion is often determined 6249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* using only gyro and accel. 6349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] quat Length 4, Quaternion scaled by 2^30 6449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 6549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_store_gaming_quaternion(const long *quat, inv_time_t timestamp) 6649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 6749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.status |= INV_6_AXIS_QUAT_SET; 6849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(&rh.gam_quat, quat, sizeof(rh.gam_quat)); 6949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.gam_timestamp = timestamp; 7049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 7149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 7249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** @internal 7349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Store a quaternion computed from accelerometer correction. This quaternion is 7449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* determined * using only accel, and used for geomagnetic fusion. 7549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] quat Length 4, Quaternion scaled by 2^30 7649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 7749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_store_accel_quaternion(const long *quat, inv_time_t timestamp) 7849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 7949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // rh.status |= INV_6_AXIS_QUAT_SET; 8049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(&rh.accel_quat, quat, sizeof(rh.accel_quat)); 8149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.geomag_timestamp = timestamp; 8249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 8349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** @internal 8449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Sets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion. 8549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] data Quaternion Adjustment 8649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] timestamp Timestamp of when this is valid 8749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 8849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_compass_correction(const long *data, inv_time_t timestamp) 8949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 9049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.status |= INV_COMPASS_CORRECTION_SET; 9149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(rh.compass_correction, data, sizeof(rh.compass_correction)); 9249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.nav_timestamp = timestamp; 9349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 9449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 9549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** @internal 9649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Sets the quaternion adjustment from 3 axis (accel) to 6 axis (with compass) quaternion. 9749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] data Quaternion Adjustment 9849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] timestamp Timestamp of when this is valid 9949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 10049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_geomagnetic_compass_correction(const long *data, inv_time_t timestamp) 10149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 10249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.status |= INV_GEOMAGNETIC_CORRECTION_SET; 10349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(rh.geomag_compass_correction, data, sizeof(rh.geomag_compass_correction)); 10449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.geomag_timestamp = timestamp; 10549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 10649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 10749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** @internal 10849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Gets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion. 10949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] data Quaternion Adjustment 11049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] timestamp Timestamp of when this is valid 11149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 11249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_compass_correction(long *data, inv_time_t *timestamp) 11349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 11449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(data, rh.compass_correction, sizeof(rh.compass_correction)); 11549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *timestamp = rh.nav_timestamp; 11649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 11749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 11849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** @internal 11949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Gets the quaternion adjustment from 3 axis (accel) to 6 axis (with compass) quaternion. 12049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] data Quaternion Adjustment 12149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] timestamp Timestamp of when this is valid 12249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 12349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_geomagnetic_compass_correction(long *data, inv_time_t *timestamp) 12449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 12549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(data, rh.geomag_compass_correction, sizeof(rh.geomag_compass_correction)); 12649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *timestamp = rh.geomag_timestamp; 12749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 12849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 12949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Returns non-zero if there is a large magnetic field. See inv_set_large_mag_field() for setting this variable. 13049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return Returns non-zero if there is a large magnetic field. 13149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 13249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_large_mag_field() 13349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 13449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return rh.large_mag_field; 13549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 13649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 13749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Set to non-zero if there as a large magnetic field. See inv_get_large_mag_field() for getting this variable. 13849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] state value to set for magnetic field strength. Should be non-zero if it is large. 13949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 14049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_large_mag_field(int state) 14149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 14249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.large_mag_field = state; 14349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 14449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 14549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Gets the accel state set by inv_set_acc_state() 14649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return accel state. 14749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 14849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_acc_state() 14949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 15049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return rh.acc_state; 15149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 15249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 15349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Sets the accel state. See inv_get_acc_state() to get the value. 15449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] state value to set accel state to. 15549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 15649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_acc_state(int state) 15749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 15849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.acc_state = state; 15949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 16049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 16149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 16249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Returns the motion state 16349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] cntr Number of previous times a no motion event has occured in a row. 16449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns INV_SUCCESS if successful or an error code if not. 16549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 16649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_motion_state(unsigned int *cntr) 16749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 16849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *cntr = rh.motion_state_counter; 16949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return rh.motion_state; 17049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 17149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 17249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Sets the motion state 17349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] state motion state where INV_NO_MOTION is not moving 17449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * and INV_MOTION is moving. 17549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 17649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_motion_state(unsigned char state) 17749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 17849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long set; 17949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (state == rh.motion_state) { 18049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (state == INV_NO_MOTION) { 18149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.motion_state_counter++; 18249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 18349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.motion_state_counter = 0; 18449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 18549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 18649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 18749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.motion_state_counter = 0; 18849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.motion_state = state; 18949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Equivalent to set = state, but #define's may change. */ 19049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (state == INV_MOTION) 19149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow set = INV_MSG_MOTION_EVENT; 19249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else 19349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow set = INV_MSG_NO_MOTION_EVENT; 19449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_set_message(set, (INV_MSG_MOTION_EVENT | INV_MSG_NO_MOTION_EVENT), 0); 19549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 19649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 19749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Sets the local earth's magnetic field 19849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] data Local earth's magnetic field in uT scaled by 2^16. 19949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Length = 3. Y typically points north, Z typically points down in 20049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* northern hemisphere and up in southern hemisphere. 20149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 20249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_local_field(const long *data) 20349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 20449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(rh.local_field, data, sizeof(rh.local_field)); 20549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 20649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 20749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Gets the local earth's magnetic field 20849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] data Local earth's magnetic field in uT scaled by 2^16. 20949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Length = 3. Y typically points north, Z typically points down in 21049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* northern hemisphere and up in southern hemisphere. 21149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 21249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_local_field(long *data) 21349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 21449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(data, rh.local_field, sizeof(rh.local_field)); 21549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 21649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 21749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Sets the compass sensitivity 21849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] data Length 3, sensitivity for each compass axis 21949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * scaled such that 1.0 = 2^30. 22049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 22149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_mag_scale(const long *data) 22249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 22349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(rh.mag_scale, data, sizeof(rh.mag_scale)); 22449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 22549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 22649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Gets the compass sensitivity 22749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] data Length 3, sensitivity for each compass axis 22849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * scaled such that 1.0 = 2^30. 22949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 23049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_mag_scale(long *data) 23149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 23249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(data, rh.mag_scale, sizeof(rh.mag_scale)); 23349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 23449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 23549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Gets gravity vector 23649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] data gravity vector in body frame scaled such that 1.0 = 2^30. 23749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return Returns INV_SUCCESS if successful or an error code if not. 23849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 23949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_get_gravity(long *data) 24049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 24149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[0] = 24249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q29_mult(rh.nav_quat[1], rh.nav_quat[3]) - inv_q29_mult(rh.nav_quat[2], rh.nav_quat[0]); 24349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[1] = 24449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q29_mult(rh.nav_quat[2], rh.nav_quat[3]) + inv_q29_mult(rh.nav_quat[1], rh.nav_quat[0]); 24549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[2] = 24649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (inv_q29_mult(rh.nav_quat[3], rh.nav_quat[3]) + inv_q29_mult(rh.nav_quat[0], rh.nav_quat[0])) - 24749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1073741824L; 24849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 24949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 25049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 25149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 25249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Returns a quaternion based only on accel. 25349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] data 3-axis accel quaternion scaled such that 1.0 = 2^30. 25449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return Returns INV_SUCCESS if successful or an error code if not. 25549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 25649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_get_accel_quaternion(long *data) 25749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 25849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(data, rh.accel_quat, sizeof(rh.accel_quat)); 25949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 26049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 26149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_get_gravity_6x(long *data) 26249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 26349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[0] = 26449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q29_mult(rh.gam_quat[1], rh.gam_quat[3]) - inv_q29_mult(rh.gam_quat[2], rh.gam_quat[0]); 26549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[1] = 26649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q29_mult(rh.gam_quat[2], rh.gam_quat[3]) + inv_q29_mult(rh.gam_quat[1], rh.gam_quat[0]); 26749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[2] = 26849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (inv_q29_mult(rh.gam_quat[3], rh.gam_quat[3]) + inv_q29_mult(rh.gam_quat[0], rh.gam_quat[0])) - 26949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1073741824L; 27049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 27149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 27249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Returns a quaternion based only on gyro and accel. 27349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] data 6-axis gyro and accel quaternion scaled such that 1.0 = 2^30. 27449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return Returns INV_SUCCESS if successful or an error code if not. 27549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 276cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaroinv_error_t inv_get_6axis_quaternion(long *data, inv_time_t *timestamp) 27749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 27849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(data, rh.gam_quat, sizeof(rh.gam_quat)); 279cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro *timestamp = rh.gam_timestamp; 28049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 28149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 28249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 28349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Returns a quaternion. 28449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30. 28549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return Returns INV_SUCCESS if successful or an error code if not. 28649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 28749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_get_quaternion(long *data) 28849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 28949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (rh.status & (INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET)) { 29049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q_mult(rh.compass_correction, rh.gam_quat, rh.nav_quat); 29149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.status &= ~(INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET); 29249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 29349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(data, rh.nav_quat, sizeof(rh.nav_quat)); 29449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 29549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 29649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 29749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Returns a quaternion based only on compass and accel. 29849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] data 6-axis compass and accel quaternion scaled such that 1.0 = 2^30. 29949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return Returns INV_SUCCESS if successful or an error code if not. 30049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 30149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_get_geomagnetic_quaternion(long *data, inv_time_t *timestamp) 30249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 30349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (rh.status & INV_GEOMAGNETIC_CORRECTION_SET) { 30449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_q_mult(rh.geomag_compass_correction, rh.accel_quat, rh.geomag_quat); 30549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.status &= ~(INV_GEOMAGNETIC_CORRECTION_SET); 30649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 30749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(data, rh.geomag_quat, sizeof(rh.geomag_quat)); 30849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *timestamp = rh.geomag_timestamp; 30949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 31049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 31149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 31249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Returns a quaternion. 31349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] data 9-axis quaternion. 31449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return Returns INV_SUCCESS if successful or an error code if not. 31549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 31649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_get_quaternion_float(float *data) 31749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 31849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long ldata[4]; 31949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_error_t result = inv_get_quaternion(ldata); 32049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[0] = inv_q30_to_float(ldata[0]); 32149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[1] = inv_q30_to_float(ldata[1]); 32249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[2] = inv_q30_to_float(ldata[2]); 32349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[3] = inv_q30_to_float(ldata[3]); 32449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 32549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 32649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 32749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Returns a quaternion with accuracy and timestamp. 32849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30. 32949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] accuracy Accuracy of quaternion, 0-3, where 3 is most accurate. 33049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] timestamp Timestamp of this quaternion in nanoseconds 33149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 33249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp) 33349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 33449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_quaternion(data); 33549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *timestamp = inv_get_last_timestamp(); 33649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_get_compass_on()) { 33749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *accuracy = inv_get_mag_accuracy(); 33849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if (inv_get_gyro_on()) { 33949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *accuracy = inv_get_gyro_accuracy(); 34049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow }else if (inv_get_accel_on()) { 34149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *accuracy = inv_get_accel_accuracy(); 34249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 34349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *accuracy = 0; 34449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 34549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 34649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 34749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Callback that gets called everytime there is new data. It is 34849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * registered by inv_start_results_holder(). 34949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] sensor_cal New sensor data to process. 35049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return Returns INV_SUCCESS if successful or an error code if not. 35149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 35249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_generate_results(struct inv_sensor_cal_t *sensor_cal) 35349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 35449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.sensor = sensor_cal; 35549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 35649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 35749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 35849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Function to turn on this module. This is automatically called by 35949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * inv_enable_results_holder(). Typically not called by users. 36049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return Returns INV_SUCCESS if successful or an error code if not. 36149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 36249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_start_results_holder(void) 36349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 36449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_register_data_cb(inv_generate_results, INV_PRIORITY_RESULTS_HOLDER, 36549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW); 36649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 36749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 36849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 36949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Initializes results holder. This is called automatically by the 37049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* enable function inv_enable_results_holder(). It may be called any time the feature is enabled, but 37149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* is typically not needed to be called by outside callers. 37249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns INV_SUCCESS if successful or an error code if not. 37349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 37449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_init_results_holder(void) 37549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 37649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(&rh, 0, sizeof(rh)); 37749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.mag_scale[0] = 1L<<30; 37849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.mag_scale[1] = 1L<<30; 37949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.mag_scale[2] = 1L<<30; 38049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.compass_correction[0] = 1L<<30; 38149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.gam_quat[0] = 1L<<30; 38249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.nav_quat[0] = 1L<<30; 38349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.geomag_quat[0] = 1L<<30; 38449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.accel_quat[0] = 1L<<30; 38549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.geomag_compass_correction[0] = 1L<<30; 38649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.quat_confidence_interval = (float)M_PI; 38749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 38849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 38949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 39049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Turns on storage of results. 39149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 39249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_enable_results_holder() 39349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 39449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_error_t result; 39549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = inv_init_results_holder(); 39649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ( result ) { 39749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 39849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 39949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 40049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = inv_register_mpl_start_notification(inv_start_results_holder); 40149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 40249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 40349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 40449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Sets state of if we know the accel bias. 40549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return return 1 if we know the accel bias, 0 if not. 40649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * it is set with inv_set_accel_bias_found() 40749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 40849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_got_accel_bias() 40949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 41049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return rh.got_accel_bias; 41149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 41249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 41349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Sets whether we know the accel bias 41449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] state Set to 1 if we know the accel bias. 41549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * Can be retrieved with inv_got_accel_bias() 41649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 41749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_accel_bias_found(int state) 41849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 41949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.got_accel_bias = state; 42049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 42149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 42249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Sets state of if we know the compass bias. 42349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return return 1 if we know the compass bias, 0 if not. 42449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * it is set with inv_set_compass_bias_found() 42549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 42649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_got_compass_bias() 42749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 42849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return rh.got_compass_bias; 42949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 43049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 43149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Sets whether we know the compass bias 43249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] state Set to 1 if we know the compass bias. 43349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * Can be retrieved with inv_got_compass_bias() 43449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 43549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_compass_bias_found(int state) 43649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 43749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.got_compass_bias = state; 43849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 43949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 44049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Sets the compass state. 44149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] state Compass state. It can be retrieved with inv_get_compass_state(). 44249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 44349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_compass_state(int state) 44449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 44549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.compass_state = state; 44649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 44749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 44849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Get's the compass state 44949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return the compass state that was set with inv_set_compass_state() 45049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 45149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint inv_get_compass_state() 45249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 45349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return rh.compass_state; 45449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 45549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 45649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Set compass bias error. See inv_get_compass_bias_error() 45749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[in] bias_error Set's how accurate we know the compass bias. It is the 45849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * error squared. 45949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 46049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_compass_bias_error(const long *bias_error) 46149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 46249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(rh.compass_bias_error, bias_error, sizeof(rh.compass_bias_error)); 46349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 46449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 46549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Get's compass bias error. See inv_set_compass_bias_error() for setting. 46649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] bias_error Accuracy as to how well the compass bias is known. It is the error squared. 46749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 46849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_get_compass_bias_error(long *bias_error) 46949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 47049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(bias_error, rh.compass_bias_error, sizeof(rh.compass_bias_error)); 47149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 47249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 47349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 47449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @brief Returns 3-element vector of accelerometer data in body frame 47549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * with gravity removed 47649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] data 3-element vector of accelerometer data in body frame 47749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * with gravity removed 47849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return INV_SUCCESS if successful 47949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * INV_ERROR_INVALID_PARAMETER if invalid input pointer 48049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 48149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_get_linear_accel(long *data) 48249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 48349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long gravity[3]; 48449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 48549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (data != NULL) 48649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow { 48749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_accel_set(data, NULL, NULL); 48849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_gravity(gravity); 48949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[0] -= gravity[0] >> 14; 49049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[1] -= gravity[1] >> 14; 49149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[2] -= gravity[2] >> 14; 49249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 49349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 49449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else { 49549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_ERROR_INVALID_PARAMETER; 49649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 49749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 49849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 49949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 50049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @brief Returns 3-element vector of accelerometer data in body frame 50149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] data 3-element vector of accelerometer data in body frame 50249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return INV_SUCCESS if successful 50349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * INV_ERROR_INVALID_PARAMETER if invalid input pointer 50449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 50549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_get_accel(long *data) 50649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 50749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (data != NULL) { 50849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_accel_set(data, NULL, NULL); 50949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 51049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 51149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else { 51249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_ERROR_INVALID_PARAMETER; 51349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 51449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 51549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 51649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 51749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @brief Returns 3-element vector of accelerometer float data 51849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] data 3-element vector of accelerometer float data 51949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return INV_SUCCESS if successful 52049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * INV_ERROR_INVALID_PARAMETER if invalid input pointer 52149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 52249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_get_accel_float(float *data) 52349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 52449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long tdata[3]; 52549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned char i; 52649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 52749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (data != NULL && !inv_get_accel(tdata)) { 52849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i = 0; i < 3; ++i) { 52949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[i] = ((float)tdata[i] / (1L << 16)); 53049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 53149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 53249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 53349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else { 53449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_ERROR_INVALID_PARAMETER; 53549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 53649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 53749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 53849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 53949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @brief Returns 3-element vector of gyro float data 54049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] data 3-element vector of gyro float data 54149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return INV_SUCCESS if successful 54249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * INV_ERROR_INVALID_PARAMETER if invalid input pointer 54349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 54449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_get_gyro_float(float *data) 54549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 54649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long tdata[3]; 54749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned char i; 54849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 54949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (data != NULL) { 55049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_gyro_set(tdata, NULL, NULL); 55149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i = 0; i < 3; ++i) { 55249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[i] = ((float)tdata[i] / (1L << 16)); 55349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 55449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 55549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 55649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else { 55749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_ERROR_INVALID_PARAMETER; 55849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 55949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 56049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 56149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Set 9 axis 95% heading confidence interval for quaternion 56249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] ci Confidence interval in radians. 56349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 56449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_heading_confidence_interval(float ci) 56549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 56649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.quat_confidence_interval = ci; 56749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 56849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 56949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Get 9 axis 95% heading confidence interval for quaternion 57049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Confidence interval in radians. 57149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 57249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowfloat inv_get_heading_confidence_interval(void) 57349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 57449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return rh.quat_confidence_interval; 57549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 57649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 57749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Set 6 axis (accel and compass) 95% heading confidence interval for quaternion 57849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] ci Confidence interval in radians. 57949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 58049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_set_accel_compass_confidence_interval(float ci) 58149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 58249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rh.geo_mag_confidence_interval = ci; 58349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 58449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 58549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Get 6 axis (accel and compass) 95% heading confidence interval for quaternion 58649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Confidence interval in radians. 58749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 58849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowfloat inv_get_accel_compass_confidence_interval(void) 58949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 59049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return rh.geo_mag_confidence_interval; 59149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 59249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 59349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 59449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @brief Returns 3-element vector of linear accel float data 59549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @param[out] data 3-element vector of linear aceel float data 59649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return INV_SUCCESS if successful 59749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * INV_ERROR_INVALID_PARAMETER if invalid input pointer 59849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 59949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_get_linear_accel_float(float *data) 60049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 60149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long tdata[3]; 60249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned char i; 60349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 60449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (data != NULL && !inv_get_linear_accel(tdata)) { 60549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (i = 0; i < 3; ++i) { 60649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[i] = ((float)tdata[i] / (1L << 16)); 60749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 60849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 60949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 61049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else { 61149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_ERROR_INVALID_PARAMETER; 61249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 61349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 61449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 61549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 61649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @} 61749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 618