1c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/* 2c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro $License: 3c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 4c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro See included License.txt for License information. 5c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro $ 6c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 7c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** 8c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @defgroup Results_Holder results_holder 9c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @brief Motion Library - Results Holder 10c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * Holds the data for MPL 11c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * 12c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @{ 13c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @file results_holder.c 14c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @brief Results Holder for HAL. 15c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 16c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 17c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include <string.h> 18c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 19c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "results_holder.h" 20c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "ml_math_func.h" 21c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "mlmath.h" 22c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "start_manager.h" 23c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "data_builder.h" 24c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "message_layer.h" 25c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "log.h" 26c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 27c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostruct results_t { 28c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro float nav_quat[4]; 29c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro float game_quat[4]; 30c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long gam_quat[4]; 31c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro float geomag_quat[4]; 32c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long accel_quat[4]; 33c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t nav_timestamp; 34c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t gam_timestamp; 35c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_time_t geomag_timestamp; 36c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long mag_scale[3]; /**< scale factor to apply to magnetic field reading */ 37c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long compass_correction[4]; /**< quaternion going from gyro,accel quaternion to 9 axis */ 38c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long geomag_compass_correction[4]; /**< quaternion going from accel quaternion to geomag sensor fusion */ 39c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int acc_state; /**< Describes accel state */ 40c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int got_accel_bias; /**< Flag describing if accel bias is known */ 41c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long compass_bias_error[3]; /**< Error Squared */ 42c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro unsigned char motion_state; 43c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro unsigned int motion_state_counter; /**< Incremented for each no motion event in a row */ 44c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long compass_count; /**< compass state internal counter */ 45c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int got_compass_bias; /**< Flag describing if compass bias is known */ 46c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int large_mag_field; /**< Flag describing if there is a large magnetic field */ 47c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int compass_state; /**< Internal compass state */ 48c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long status; 49c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro struct inv_sensor_cal_t *sensor; 50c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro float quat_confidence_interval; 51c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro float geo_mag_confidence_interval; 52c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro struct local_field_t mag_local_field; 53c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro struct local_field_t mpl_compass_cal; 54c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int quat_validity; 55c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#ifdef WIN32 56c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long last_quat[4]; 57c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#endif 58c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro}; 59c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic struct results_t rh; 60c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 61c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** @internal 62c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* Store a quaternion more suitable for gaming. This quaternion is often determined 63c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* using only gyro and accel. 64c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] quat Length 4, Quaternion scaled by 2^30 65c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] timestamp Timestamp of the 6-axis quaternion 66c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 67c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_store_gaming_quaternion(const long *quat, inv_time_t timestamp) 68c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 69c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.status |= INV_6_AXIS_QUAT_SET; 70c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(&rh.gam_quat, quat, sizeof(rh.gam_quat)); 71c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.gam_timestamp = timestamp; 72c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 73c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 74c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** @internal 75c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* Store a 9-axis quaternion. 76c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] quat Length 4 in floating-point numbers 77c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] timestamp Timestamp of the 9-axis quaternion 78c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 79c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_store_nav_quaternion(const float *quat, inv_time_t timestamp) 80c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 81c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(&rh.nav_quat, quat, sizeof(rh.nav_quat)); 82c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.nav_timestamp = timestamp; 83c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 84c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 85c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** @internal 86c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* Store a 6-axis geomagnetic quaternion. 87c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] quat Length 4 in floating-point numbers 88c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] timestamp Timestamp of the geomag quaternion 89c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 90c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_store_geomag_quaternion(const float *quat, inv_time_t timestamp) 91c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 92c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(&rh.geomag_quat, quat, sizeof(rh.geomag_quat)); 93c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.geomag_timestamp = timestamp; 94c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 95c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 96c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** @internal 97c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* Store a floating-point quaternion more suitable for gaming. 98c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] quat Length 4 in floating-point numbers 99c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] timestamp Timestamp of the 6-axis quaternion 100c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 101c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_store_game_quaternion(const float *quat, inv_time_t timestamp) 102c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 103c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.status |= INV_6_AXIS_QUAT_SET; 104c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(&rh.game_quat, quat, sizeof(rh.game_quat)); 105c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.gam_timestamp = timestamp; 106c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 107c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 108c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** @internal 109c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* Store a quaternion computed from accelerometer correction. This quaternion is 110c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* determined * using only accel, and used for geomagnetic fusion. 111c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] quat Length 4, Quaternion scaled by 2^30 112c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 113c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_store_accel_quaternion(const long *quat, inv_time_t timestamp) 114c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 115c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // rh.status |= INV_6_AXIS_QUAT_SET; 116c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(&rh.accel_quat, quat, sizeof(rh.accel_quat)); 117c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.geomag_timestamp = timestamp; 118c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 119c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** @internal 120c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* Sets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion. 121c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] data Quaternion Adjustment 122c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] timestamp Timestamp of when this is valid 123c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 124c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_compass_correction(const long *data, inv_time_t timestamp) 125c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 126c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.status |= INV_COMPASS_CORRECTION_SET; 127c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(rh.compass_correction, data, sizeof(rh.compass_correction)); 128c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.nav_timestamp = timestamp; 129c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 130c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 131c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** @internal 132c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* Sets the quaternion adjustment from 3 axis (accel) to 6 axis (with compass) quaternion. 133c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] data Quaternion Adjustment 134c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] timestamp Timestamp of when this is valid 135c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 136c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_geomagnetic_compass_correction(const long *data, inv_time_t timestamp) 137c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 138c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.status |= INV_GEOMAGNETIC_CORRECTION_SET; 139c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(rh.geomag_compass_correction, data, sizeof(rh.geomag_compass_correction)); 140c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.geomag_timestamp = timestamp; 141c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 142c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 143c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** @internal 144c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* Gets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion. 145c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] data Quaternion Adjustment 146c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] timestamp Timestamp of when this is valid 147c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 148c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_get_compass_correction(long *data, inv_time_t *timestamp) 149c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 150c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(data, rh.compass_correction, sizeof(rh.compass_correction)); 151c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro *timestamp = rh.nav_timestamp; 152c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 153c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 154c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** @internal 155c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* Gets the quaternion adjustment from 3 axis (accel) to 6 axis (with compass) quaternion. 156c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] data Quaternion Adjustment 157c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] timestamp Timestamp of when this is valid 158c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 159c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_get_geomagnetic_compass_correction(long *data, inv_time_t *timestamp) 160c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 161c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(data, rh.geomag_compass_correction, sizeof(rh.geomag_compass_correction)); 162c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro *timestamp = rh.geomag_timestamp; 163c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 164c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 165c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Returns non-zero if there is a large magnetic field. See inv_set_large_mag_field() for setting this variable. 166c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return Returns non-zero if there is a large magnetic field. 167c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 168c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroint inv_get_large_mag_field() 169c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 170c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return rh.large_mag_field; 171c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 172c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 173c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Set to non-zero if there as a large magnetic field. See inv_get_large_mag_field() for getting this variable. 174c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[in] state value to set for magnetic field strength. Should be non-zero if it is large. 175c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 176c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_large_mag_field(int state) 177c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 178c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.large_mag_field = state; 179c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 180c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 181c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Gets the accel state set by inv_set_acc_state() 182c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return accel state. 183c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 184c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroint inv_get_acc_state() 185c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 186c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return rh.acc_state; 187c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 188c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 189c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Sets the accel state. See inv_get_acc_state() to get the value. 190c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[in] state value to set accel state to. 191c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 192c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_acc_state(int state) 193c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 194c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.acc_state = state; 195c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return; 196c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 197c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 198c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Returns the motion state 199c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[out] cntr Number of previous times a no motion event has occured in a row. 200c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @return Returns INV_SUCCESS if successful or an error code if not. 201c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 202c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroint inv_get_motion_state(unsigned int *cntr) 203c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 204c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro *cntr = rh.motion_state_counter; 205c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return rh.motion_state; 206c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 207c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 208c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Sets the motion state 209c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[in] state motion state where INV_NO_MOTION is not moving 210c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * and INV_MOTION is moving. 211c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 212c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_motion_state(unsigned char state) 213c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 214c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long set; 215c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (state == rh.motion_state) { 216c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (state == INV_NO_MOTION) { 217c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.motion_state_counter++; 218c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else { 219c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.motion_state_counter = 0; 220c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 221c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return; 222c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 223c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.motion_state_counter = 0; 224c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.motion_state = state; 225c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro /* Equivalent to set = state, but #define's may change. */ 226c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (state == INV_MOTION) 227c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro set = INV_MSG_MOTION_EVENT; 228c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else 229c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro set = INV_MSG_NO_MOTION_EVENT; 230c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_message(set, (INV_MSG_MOTION_EVENT | INV_MSG_NO_MOTION_EVENT), 0); 231c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 232c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 233c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Sets the compass sensitivity 234c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[in] data Length 3, sensitivity for each compass axis 235c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * scaled such that 1.0 = 2^30. 236c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 237c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_mag_scale(const long *data) 238c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 239c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(rh.mag_scale, data, sizeof(rh.mag_scale)); 240c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 241c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 242c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Gets the compass sensitivity 243c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] data Length 3, sensitivity for each compass axis 244c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * scaled such that 1.0 = 2^30. 245c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 246c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_get_mag_scale(long *data) 247c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 248c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(data, rh.mag_scale, sizeof(rh.mag_scale)); 249c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 250c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 251c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Gets gravity vector 252c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] data gravity vector in body frame scaled such that 1.0 = 2^30. 253c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return Returns INV_SUCCESS if successful or an error code if not. 254c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 255c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_get_gravity(long *data) 256c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 257c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long ldata[4]; 258c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_error_t result = inv_get_quaternion(ldata); 259c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 260c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[0] = 261c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_q29_mult(ldata[1], ldata[3]) - inv_q29_mult(ldata[2], ldata[0]); 262c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[1] = 263c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_q29_mult(ldata[2], ldata[3]) + inv_q29_mult(ldata[1], ldata[0]); 264c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[2] = 265c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro (inv_q29_mult(ldata[3], ldata[3]) + inv_q29_mult(ldata[0], ldata[0])) - 266c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 1073741824L; 267c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 268c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return result; 269c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 270c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 271c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Returns a quaternion based only on accel. 272c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] data 3-axis accel quaternion scaled such that 1.0 = 2^30. 273c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return Returns INV_SUCCESS if successful or an error code if not. 274c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 275c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_get_accel_quaternion(long *data) 276c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 277c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(data, rh.accel_quat, sizeof(rh.accel_quat)); 278c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 279c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 280c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_get_gravity_6x(long *data) 281c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 282c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[0] = 283c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_q29_mult(rh.gam_quat[1], rh.gam_quat[3]) - inv_q29_mult(rh.gam_quat[2], rh.gam_quat[0]); 284c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[1] = 285c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_q29_mult(rh.gam_quat[2], rh.gam_quat[3]) + inv_q29_mult(rh.gam_quat[1], rh.gam_quat[0]); 286c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[2] = 287c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro (inv_q29_mult(rh.gam_quat[3], rh.gam_quat[3]) + inv_q29_mult(rh.gam_quat[0], rh.gam_quat[0])) - 288c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 1073741824L; 289c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 290c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 291c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Returns a quaternion based only on gyro and accel. 292c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] data 6-axis gyro and accel quaternion scaled such that 1.0 = 2^30. 293c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return Returns INV_SUCCESS if successful or an error code if not. 294c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 295c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_get_6axis_quaternion(long *data, inv_time_t *timestamp) 296c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 297c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[0] = (long)MIN(MAX(rh.game_quat[0] * ((float)(1L << 30)), -2147483648.), 2147483647.); 298c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[1] = (long)MIN(MAX(rh.game_quat[1] * ((float)(1L << 30)), -2147483648.), 2147483647.); 299c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[2] = (long)MIN(MAX(rh.game_quat[2] * ((float)(1L << 30)), -2147483648.), 2147483647.); 300c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[3] = (long)MIN(MAX(rh.game_quat[3] * ((float)(1L << 30)), -2147483648.), 2147483647.); 301c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro *timestamp = rh.gam_timestamp; 302c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 303c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 304c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 305c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Returns a quaternion. 306c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30. 307c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return Returns INV_SUCCESS if successful or an error code if not. 308c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 309c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_get_quaternion(long *data) 310c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 311c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[0] = (long)MIN(MAX(rh.nav_quat[0] * ((float)(1L << 30)), -2147483648.), 2147483647.); 312c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[1] = (long)MIN(MAX(rh.nav_quat[1] * ((float)(1L << 30)), -2147483648.), 2147483647.); 313c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[2] = (long)MIN(MAX(rh.nav_quat[2] * ((float)(1L << 30)), -2147483648.), 2147483647.); 314c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[3] = (long)MIN(MAX(rh.nav_quat[3] * ((float)(1L << 30)), -2147483648.), 2147483647.); 315c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 316c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 317c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 318c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 319c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#ifdef WIN32 320c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Returns the last 9-axis quaternion genearted by MPL, so that 321c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * it can be written to the DMP. 322c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30. 323c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return Returns INV_SUCCESS if successful or an error code if not. 324c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 325c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_get_last_quaternion(long *data) 326c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 327c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(data, rh.last_quat, sizeof(rh.last_quat)); 328c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 329c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 330c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 331c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Saves the last 9-axis quaternion generated by MPL. 332c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[in] data 9-axis quaternion data. 333c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 334c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_set_last_quaternion(long *data) 335c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 336c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(rh.last_quat, data, sizeof(rh.last_quat)); 337c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 338c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 339c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#endif 340c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 341c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Returns the status of the result holder. 342c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] rh_status Result holder status. 343c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return Returns INV_SUCCESS if successful or an error code if not. 344c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 345c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_get_result_holder_status(long *rh_status) 346c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 347c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro *rh_status = rh.status; 348c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 349c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 350c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 351c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Set the status of the result holder. 352c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return Returns INV_SUCCESS if successful or an error code if not. 353c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 354c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_set_result_holder_status(long rh_status) 355c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 356c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.status = rh_status; 357c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 358c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 359c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 360c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Returns the status of the authenticity of the quaternion data. 361c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] value Authenticity of the quaternion data. 362c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return Returns INV_SUCCESS if successful or an error code if not. 363c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 364c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_get_quaternion_validity(int *value) 365c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 366c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro *value = rh.quat_validity; 367c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 368c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 369c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 370c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Set the status of the authenticity of the quaternion data. 371c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return Returns INV_SUCCESS if successful or an error code if not. 372c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 373c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_set_quaternion_validity(int value) 374c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 375c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.quat_validity = value; 376c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 377c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 378c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 379c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Returns a quaternion based only on compass and accel. 380c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] data 6-axis compass and accel quaternion scaled such that 1.0 = 2^30. 381c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return Returns INV_SUCCESS if successful or an error code if not. 382c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 383c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_get_geomagnetic_quaternion(long *data, inv_time_t *timestamp) 384c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 385c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[0] = (long)MIN(MAX(rh.geomag_quat[0] * ((float)(1L << 30)), -2147483648.), 2147483647.); 386c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[1] = (long)MIN(MAX(rh.geomag_quat[1] * ((float)(1L << 30)), -2147483648.), 2147483647.); 387c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[2] = (long)MIN(MAX(rh.geomag_quat[2] * ((float)(1L << 30)), -2147483648.), 2147483647.); 388c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[3] = (long)MIN(MAX(rh.geomag_quat[3] * ((float)(1L << 30)), -2147483648.), 2147483647.); 389c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro *timestamp = rh.geomag_timestamp; 390c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 391c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 392c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 393c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Returns a quaternion. 394c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] data 9-axis quaternion. 395c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return Returns INV_SUCCESS if successful or an error code if not. 396c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 397c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_get_quaternion_float(float *data) 398c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 399c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(data, rh.nav_quat, sizeof(rh.nav_quat)); 400c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 401c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 402c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 403c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Returns a quaternion based only on gyro and accel. 404c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] data 6-axis gyro and accel quaternion. 405c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return Returns INV_SUCCESS if successful or an error code if not. 406c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 407c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_get_6axis_quaternion_float(float *data, inv_time_t *timestamp) 408c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 409c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(data, rh.game_quat, sizeof(rh.game_quat)); 410c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro *timestamp = rh.gam_timestamp; 411c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 412c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 413c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 414c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Returns a quaternion based only on compass and accel. 415c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] data 6-axis compass and accel quaternion. 416c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return Returns INV_SUCCESS if successful or an error code if not. 417c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 418c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_get_geomagnetic_quaternion_float(float *data, inv_time_t *timestamp) 419c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 420c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(data, rh.geomag_quat, sizeof(rh.geomag_quat)); 421c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro *timestamp = rh.geomag_timestamp; 422c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 423c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 424c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 425c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Returns a quaternion with accuracy and timestamp. 426c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30. 427c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] accuracy Accuracy of quaternion, 0-3, where 3 is most accurate. 428c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] timestamp Timestamp of this quaternion in nanoseconds 429c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 430c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp) 431c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 432c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_quaternion(data); 433c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro *timestamp = inv_get_last_timestamp(); 434c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (inv_get_compass_on()) { 435c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro *accuracy = inv_get_mag_accuracy(); 436c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else if (inv_get_gyro_on()) { 437c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro *accuracy = inv_get_gyro_accuracy(); 438c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro }else if (inv_get_accel_on()) { 439c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro *accuracy = inv_get_accel_accuracy(); 440c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else { 441c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro *accuracy = 0; 442c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 443c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 444c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 445c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Callback that gets called everytime there is new data. It is 446c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * registered by inv_start_results_holder(). 447c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[in] sensor_cal New sensor data to process. 448c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return Returns INV_SUCCESS if successful or an error code if not. 449c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 450c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_generate_results(struct inv_sensor_cal_t *sensor_cal) 451c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 452c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.sensor = sensor_cal; 453c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 454c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 455c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 456c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Function to turn on this module. This is automatically called by 457c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * inv_enable_results_holder(). Typically not called by users. 458c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return Returns INV_SUCCESS if successful or an error code if not. 459c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 460c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_start_results_holder(void) 461c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 462c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_register_data_cb(inv_generate_results, INV_PRIORITY_RESULTS_HOLDER, 463c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW); 464c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 465c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 466c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 467c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Initializes results holder. This is called automatically by the 468c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* enable function inv_enable_results_holder(). It may be called any time the feature is enabled, but 469c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* is typically not needed to be called by outside callers. 470c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @return Returns INV_SUCCESS if successful or an error code if not. 471c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 472c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_init_results_holder(void) 473c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 474c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memset(&rh, 0, sizeof(rh)); 475c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.mag_scale[0] = 1L<<30; 476c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.mag_scale[1] = 1L<<30; 477c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.mag_scale[2] = 1L<<30; 478c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.compass_correction[0] = 1L<<30; 479c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.gam_quat[0] = 1L<<30; 480c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.nav_quat[0] = 1.; 481c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.geomag_quat[0] = 1.; 482c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.game_quat[0] = 1.; 483c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.accel_quat[0] = 1L<<30; 484c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.geomag_compass_correction[0] = 1L<<30; 485c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.quat_confidence_interval = (float)M_PI; 486c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#ifdef WIN32 487c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.last_quat[0] = 1L<<30; 488c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#endif 489c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 490c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 491c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#ifdef TEST 492c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro { 493c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro struct local_field_t local_field; 494c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro local_field.intensity = 48.0f; // uT 495c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro local_field.inclination = 60.0f; // degree 496c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro local_field.declination = 0.0f; // degree 497c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_earth_magnetic_local_field_parameter(&local_field); 498c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // inv_set_local_field_status(LOCAL_FILED_NOT_SET_BY_USER); 499c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_local_field_status(LOCAL_FILED_SET_BY_USER); 500c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_local_magnetic_field(48.0f, 59.0f, 0.0f); 501c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 502c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // set default mpl calibration result for testing 503c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro local_field.intensity = 50.0f; // uT 504c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro local_field.inclination = 59.0f; // degree 505c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro local_field.declination = 0.0f; // degree 506c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_mpl_magnetic_local_field_parameter(&local_field); 507c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_mpl_mag_field_status(LOCAL_FIELD_SET_BUT_NOT_MATCH_WITH_MPL); 508c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 509c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#endif 510c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 511c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 512c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 513c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 514c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Turns on storage of results. 515c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 516c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_enable_results_holder() 517c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 518c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_error_t result; 519c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro result = inv_init_results_holder(); 520c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if ( result ) { 521c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return result; 522c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 523c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 524c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro result = inv_register_mpl_start_notification(inv_start_results_holder); 525c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return result; 526c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 527c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 528c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Sets state of if we know the accel bias. 529c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return return 1 if we know the accel bias, 0 if not. 530c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * it is set with inv_set_accel_bias_found() 531c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 532c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroint inv_got_accel_bias() 533c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 534c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return rh.got_accel_bias; 535c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 536c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 537c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Sets whether we know the accel bias 538c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[in] state Set to 1 if we know the accel bias. 539c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * Can be retrieved with inv_got_accel_bias() 540c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 541c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_accel_bias_found(int state) 542c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 543c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.got_accel_bias = state; 544c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 545c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 546c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Sets state of if we know the compass bias. 547c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return return 1 if we know the compass bias, 0 if not. 548c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * it is set with inv_set_compass_bias_found() 549c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 550c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroint inv_got_compass_bias() 551c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 552c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return rh.got_compass_bias; 553c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 554c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 555c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Sets whether we know the compass bias 556c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[in] state Set to 1 if we know the compass bias. 557c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * Can be retrieved with inv_got_compass_bias() 558c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 559c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_compass_bias_found(int state) 560c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 561c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.got_compass_bias = state; 562c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 563c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 564c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Sets the compass state. 565c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[in] state Compass state. It can be retrieved with inv_get_compass_state(). 566c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 567c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_compass_state(int state) 568c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 569c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.compass_state = state; 570c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 571c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 572c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Get's the compass state 573c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return the compass state that was set with inv_set_compass_state() 574c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 575c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroint inv_get_compass_state() 576c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 577c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return rh.compass_state; 578c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 579c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 580c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Set compass bias error. See inv_get_compass_bias_error() 581c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[in] bias_error Set's how accurate we know the compass bias. It is the 582c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * error squared. 583c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 584c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_compass_bias_error(const long *bias_error) 585c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 586c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(rh.compass_bias_error, bias_error, sizeof(rh.compass_bias_error)); 587c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 588c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 589c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Get's compass bias error. See inv_set_compass_bias_error() for setting. 590c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] bias_error Accuracy as to how well the compass bias is known. It is the error squared. 591c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 592c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_get_compass_bias_error(long *bias_error) 593c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 594c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(bias_error, rh.compass_bias_error, sizeof(rh.compass_bias_error)); 595c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 596c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 597c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** 598c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @brief Returns 3-element vector of accelerometer data in body frame 599c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * with gravity removed 600c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] data 3-element vector of accelerometer data in body frame 601c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * with gravity removed 602c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return INV_SUCCESS if successful 603c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * INV_ERROR_INVALID_PARAMETER if invalid input pointer 604c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 605c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_get_linear_accel(long *data) 606c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 607c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long gravity[3]; 608c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 609c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (data != NULL) 610c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro { 611c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_accel_set(data, NULL, NULL); 612c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_gravity(gravity); 613c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[0] -= gravity[0] >> 14; 614c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[1] -= gravity[1] >> 14; 615c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[2] -= gravity[2] >> 14; 616c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 617c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 618c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else { 619c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_ERROR_INVALID_PARAMETER; 620c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 621c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 622c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 623c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** 624c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @brief Returns 3-element vector of accelerometer data in body frame 625c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] data 3-element vector of accelerometer data in body frame 626c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return INV_SUCCESS if successful 627c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * INV_ERROR_INVALID_PARAMETER if invalid input pointer 628c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 629c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_get_accel(long *data) 630c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 631c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (data != NULL) { 632c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_accel_set(data, NULL, NULL); 633c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 634c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 635c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else { 636c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_ERROR_INVALID_PARAMETER; 637c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 638c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 639c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 640c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** 641c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @brief Returns 3-element vector of accelerometer float data 642c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] data 3-element vector of accelerometer float data 643c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return INV_SUCCESS if successful 644c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * INV_ERROR_INVALID_PARAMETER if invalid input pointer 645c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 646c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_get_accel_float(float *data) 647c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 648c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long tdata[3]; 649c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro unsigned char i; 650c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 651c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (data != NULL && !inv_get_accel(tdata)) { 652c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro for (i = 0; i < 3; ++i) { 653c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[i] = ((float)tdata[i] / (1L << 16)); 654c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 655c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 656c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 657c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else { 658c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_ERROR_INVALID_PARAMETER; 659c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 660c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 661c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 662c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** 663c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @brief Returns 3-element vector of gyro float data 664c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] data 3-element vector of gyro float data 665c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return INV_SUCCESS if successful 666c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * INV_ERROR_INVALID_PARAMETER if invalid input pointer 667c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 668c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_get_gyro_float(float *data) 669c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 670c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long tdata[3]; 671c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro unsigned char i; 672c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 673c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (data != NULL) { 674c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_gyro_set(tdata, NULL, NULL); 675c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro for (i = 0; i < 3; ++i) { 676c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[i] = ((float)tdata[i] / (1L << 16)); 677c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 678c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 679c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 680c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else { 681c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_ERROR_INVALID_PARAMETER; 682c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 683c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 684c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 685c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Set 9 axis 95% heading confidence interval for quaternion 686c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] ci Confidence interval in radians. 687c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 688c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_heading_confidence_interval(float ci) 689c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 690c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.quat_confidence_interval = ci; 691c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 692c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 693c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Get 9 axis 95% heading confidence interval for quaternion 694c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @return Confidence interval in radians. 695c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 696c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarofloat inv_get_heading_confidence_interval(void) 697c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 698c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return rh.quat_confidence_interval; 699c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 700c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 701c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Set 6 axis (accel and compass) 95% heading confidence interval for quaternion 702c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] ci Confidence interval in radians. 703c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 704c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_accel_compass_confidence_interval(float ci) 705c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 706c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.geo_mag_confidence_interval = ci; 707c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 708c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 709c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Get 6 axis (accel and compass) 95% heading confidence interval for quaternion 710c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @return Confidence interval in radians. 711c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 712c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarofloat inv_get_accel_compass_confidence_interval(void) 713c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 714c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return rh.geo_mag_confidence_interval; 715c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 716c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 717c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** 718c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @brief Returns 3-element vector of linear accel float data 719c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] data 3-element vector of linear aceel float data 720c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return INV_SUCCESS if successful 721c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * INV_ERROR_INVALID_PARAMETER if invalid input pointer 722c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 723c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_get_linear_accel_float(float *data) 724c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 725c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro long tdata[3]; 726c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro unsigned char i; 727c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 728c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (data != NULL && !inv_get_linear_accel(tdata)) { 729c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro for (i = 0; i < 3; ++i) { 730c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro data[i] = ((float)tdata[i] / (1L << 16)); 731c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 732c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_SUCCESS; 733c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 734c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else { 735c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return INV_ERROR_INVALID_PARAMETER; 736c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 737c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 738c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 739c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** 740c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @brief Returns the status of earth magnetic field local field parameters 741c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] N/A 742c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return status of local field, defined in enum compass_local_field_e 743c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 744c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroenum compass_local_field_e inv_get_local_field_status(void) 745c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 746c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return rh.mag_local_field.mpl_match_status; 747c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 748c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 749c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Set the status of earth magnetic field local field parameters 750c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] status of earth magnetic field local field parameters. 751c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 752c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_local_field_status(enum compass_local_field_e status) 753c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 754c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.mag_local_field.mpl_match_status = status; 755c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 756c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 757c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Set the parameters of earth magnetic field local field 758c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] the earth magnetic field local field parameters. 759c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 760c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_earth_magnetic_local_field_parameter(struct local_field_t *parameters) 761c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 762c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.mag_local_field.intensity = parameters->intensity; // radius 763c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.mag_local_field.inclination = parameters->inclination; // dip angle 764c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.mag_local_field.declination = parameters->declination; // yaw deviation angle from true north 765c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 766c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_local_field_status(LOCAL_FILED_SET_BY_USER); 767c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 768c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 769c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** 770c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @brief Returns the parameters of earth magnetic field local field 771c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] the parameters of earth magnetic field local field 772c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return N/A 773c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 774c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_get_earth_magnetic_local_field_parameter(struct local_field_t *parameters) 775c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 776c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro parameters->intensity = rh.mag_local_field.intensity; // radius 777c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro parameters->inclination = rh.mag_local_field.inclination; // dip angle 778c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro parameters->declination = rh.mag_local_field.declination; // yaw deviation angle from true north 779c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro parameters->mpl_match_status = rh.mag_local_field.mpl_match_status; 780c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 781c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 782c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** 783c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @brief Returns the status of mpl calibrated magnetic field local field parameters 784c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] N/A 785c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return status of local field, defined in enum compass_local_field_e 786c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 787c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroenum compass_local_field_e inv_get_mpl_mag_field_status(void) 788c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 789c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return rh.mpl_compass_cal.mpl_match_status; 790c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 791c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 792c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Set the status of mpl calibrated magnetic field local field parameters 793c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] status of earth magnetic field local field parameters. 794c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 795c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_set_mpl_mag_field_status(enum compass_local_field_e status) 796c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 797c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.mpl_compass_cal.mpl_match_status = status; 798c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 799c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 800c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Set the magnetic field local field struct object 801c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] status of earth magnetic field local field parameters. 802c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro*/ 803c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_set_local_magnetic_field(float intensity, float inclination, float declination) 804c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 805c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro struct local_field_t local_field; 806c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro local_field.intensity = intensity; // radius 807c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro local_field.inclination = inclination; // dip angle angle degree 808c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro local_field.declination = declination; // yaw deviation angle from true north, eastward as positive 809c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro local_field.mpl_match_status = LOCAL_FILED_SET_BY_USER; 810c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 811c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_earth_magnetic_local_field_parameter(&local_field); 812c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return 0; 813c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 814c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 815c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** Set the parameters of mpl calibrated magnetic field local field 816c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* This API is used by mpl only. 817c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro* @param[in] the earth magnetic field local field parameters. 818c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return INV_SUCCESS if successful 819c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * INV_ERROR_INVALID_PARAMETER if invalid input pointer 820c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 821c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroinv_error_t inv_set_mpl_magnetic_local_field_parameter(struct local_field_t *parameters) 822c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 823c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enum compass_local_field_e mpl_status; 824c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro struct local_field_t local_field; 825c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_error_t status; 826c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 827c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.mpl_compass_cal.intensity = parameters->intensity; // radius 828c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.mpl_compass_cal.inclination = parameters->inclination; // dip angle 829c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro rh.mpl_compass_cal.declination = parameters->declination; // yaw deviation angle from true north 830c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 831c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro mpl_status = inv_get_mpl_mag_field_status(); 832c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_earth_magnetic_local_field_parameter(&local_field); 833c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 834c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro status = INV_SUCCESS; 835c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 836c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro switch (inv_get_local_field_status()) { 837c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case LOCAL_FILED_NOT_SET_BY_USER: 838c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (mpl_status == LOCAL_FILED_NOT_SET_BY_USER) { 839c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_mpl_mag_field_status(LOCAL_FILED_NOT_SET_BY_USER_BUT_SET_BY_MPL); 840c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else { 841c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // illegal status 842c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro status = INV_ERROR_INVALID_PARAMETER; 843c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 844c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 845c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case LOCAL_FILED_SET_BY_USER: 846c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro switch (mpl_status) { 847c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case LOCAL_FIELD_SET_BUT_NOT_MATCH_WITH_MPL: 848c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case LOCAL_FIELD_SET_MATCH_WITH_MPL: 849c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if ( (ABS(local_field.intensity - parameters->intensity) < 5.0f) && 850c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro (ABS(local_field.intensity - parameters->intensity) < 5.0f) ) { 851c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_mpl_mag_field_status(LOCAL_FIELD_SET_MATCH_WITH_MPL); 852c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else { 853c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_mpl_mag_field_status(LOCAL_FIELD_SET_BUT_NOT_MATCH_WITH_MPL); 854c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 855c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 856c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case LOCAL_FILED_NOT_SET_BY_USER_BUT_SET_BY_MPL: 857c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // no status update 858c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 859c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case LOCAL_FILED_NOT_SET_BY_USER: 860c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case LOCAL_FILED_SET_BY_USER: 861c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro default: 862c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // illegal status 863c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro status = INV_ERROR_INVALID_PARAMETER; 864c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 865c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 866c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 867c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case LOCAL_FILED_NOT_SET_BY_USER_BUT_SET_BY_MPL: 868c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case LOCAL_FIELD_SET_BUT_NOT_MATCH_WITH_MPL: 869c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case LOCAL_FIELD_SET_MATCH_WITH_MPL: 870c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro default: 871c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro // illegal status 872c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro status = INV_ERROR_INVALID_PARAMETER; 873c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 874c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 875c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return status; 876c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 877c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 878c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** 879c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @brief Returns the parameters of mpl calibrated magnetic field local field 880c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @param[out] the parameters of earth magnetic field local field 881c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @return N/A 882c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 883c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid inv_get_mpl_magnetic_local_field_parameter(struct local_field_t *parameters) 884c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 885c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro parameters->intensity = rh.mpl_compass_cal.intensity; // radius 886c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro parameters->inclination = rh.mpl_compass_cal.inclination; // dip angle 887c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro parameters->declination = rh.mpl_compass_cal.declination; // yaw deviation angle from true north 888c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro parameters->mpl_match_status = rh.mpl_compass_cal.mpl_match_status; 889c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 890c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 891c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/** 892c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * @} 893c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 894