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