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
