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// These 2 status bits are used to control when the 9 axis quaternion is updated 28#define INV_COMPASS_CORRECTION_SET 1 29#define INV_6_AXIS_QUAT_SET 2 30#define INV_GEOMAGNETIC_CORRECTION_SET 4 31 32struct results_t { 33 long nav_quat[4]; 34 long gam_quat[4]; 35 long geomag_quat[4]; 36 long accel_quat[4]; 37 inv_time_t nav_timestamp; 38 inv_time_t gam_timestamp; 39 inv_time_t geomag_timestamp; 40 long local_field[3]; /**< local earth's magnetic field */ 41 long mag_scale[3]; /**< scale factor to apply to magnetic field reading */ 42 long compass_correction[4]; /**< quaternion going from gyro,accel quaternion to 9 axis */ 43 long geomag_compass_correction[4]; /**< quaternion going from accel quaternion to geomag sensor fusion */ 44 int acc_state; /**< Describes accel state */ 45 int got_accel_bias; /**< Flag describing if accel bias is known */ 46 long compass_bias_error[3]; /**< Error Squared */ 47 unsigned char motion_state; 48 unsigned int motion_state_counter; /**< Incremented for each no motion event in a row */ 49 long compass_count; /**< compass state internal counter */ 50 int got_compass_bias; /**< Flag describing if compass bias is known */ 51 int large_mag_field; /**< Flag describing if there is a large magnetic field */ 52 int compass_state; /**< Internal compass state */ 53 long status; 54 struct inv_sensor_cal_t *sensor; 55 float quat_confidence_interval; 56 float geo_mag_confidence_interval; 57}; 58static struct results_t rh; 59 60/** @internal 61* Store a quaternion more suitable for gaming. This quaternion is often determined 62* using only gyro and accel. 63* @param[in] quat Length 4, Quaternion scaled by 2^30 64*/ 65void inv_store_gaming_quaternion(const long *quat, inv_time_t timestamp) 66{ 67 rh.status |= INV_6_AXIS_QUAT_SET; 68 memcpy(&rh.gam_quat, quat, sizeof(rh.gam_quat)); 69 rh.gam_timestamp = timestamp; 70} 71 72/** @internal 73* Store a quaternion computed from accelerometer correction. This quaternion is 74* determined * using only accel, and used for geomagnetic fusion. 75* @param[in] quat Length 4, Quaternion scaled by 2^30 76*/ 77void inv_store_accel_quaternion(const long *quat, inv_time_t timestamp) 78{ 79 // rh.status |= INV_6_AXIS_QUAT_SET; 80 memcpy(&rh.accel_quat, quat, sizeof(rh.accel_quat)); 81 rh.geomag_timestamp = timestamp; 82} 83/** @internal 84* Sets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion. 85* @param[in] data Quaternion Adjustment 86* @param[in] timestamp Timestamp of when this is valid 87*/ 88void inv_set_compass_correction(const long *data, inv_time_t timestamp) 89{ 90 rh.status |= INV_COMPASS_CORRECTION_SET; 91 memcpy(rh.compass_correction, data, sizeof(rh.compass_correction)); 92 rh.nav_timestamp = timestamp; 93} 94 95/** @internal 96* Sets the quaternion adjustment from 3 axis (accel) to 6 axis (with compass) quaternion. 97* @param[in] data Quaternion Adjustment 98* @param[in] timestamp Timestamp of when this is valid 99*/ 100void inv_set_geomagnetic_compass_correction(const long *data, inv_time_t timestamp) 101{ 102 rh.status |= INV_GEOMAGNETIC_CORRECTION_SET; 103 memcpy(rh.geomag_compass_correction, data, sizeof(rh.geomag_compass_correction)); 104 rh.geomag_timestamp = timestamp; 105} 106 107/** @internal 108* Gets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion. 109* @param[out] data Quaternion Adjustment 110* @param[out] timestamp Timestamp of when this is valid 111*/ 112void inv_get_compass_correction(long *data, inv_time_t *timestamp) 113{ 114 memcpy(data, rh.compass_correction, sizeof(rh.compass_correction)); 115 *timestamp = rh.nav_timestamp; 116} 117 118/** @internal 119* Gets the quaternion adjustment from 3 axis (accel) to 6 axis (with compass) quaternion. 120* @param[out] data Quaternion Adjustment 121* @param[out] timestamp Timestamp of when this is valid 122*/ 123void inv_get_geomagnetic_compass_correction(long *data, inv_time_t *timestamp) 124{ 125 memcpy(data, rh.geomag_compass_correction, sizeof(rh.geomag_compass_correction)); 126 *timestamp = rh.geomag_timestamp; 127} 128 129/** Returns non-zero if there is a large magnetic field. See inv_set_large_mag_field() for setting this variable. 130 * @return Returns non-zero if there is a large magnetic field. 131 */ 132int inv_get_large_mag_field() 133{ 134 return rh.large_mag_field; 135} 136 137/** Set to non-zero if there as a large magnetic field. See inv_get_large_mag_field() for getting this variable. 138 * @param[in] state value to set for magnetic field strength. Should be non-zero if it is large. 139 */ 140void inv_set_large_mag_field(int state) 141{ 142 rh.large_mag_field = state; 143} 144 145/** Gets the accel state set by inv_set_acc_state() 146 * @return accel state. 147 */ 148int inv_get_acc_state() 149{ 150 return rh.acc_state; 151} 152 153/** Sets the accel state. See inv_get_acc_state() to get the value. 154 * @param[in] state value to set accel state to. 155 */ 156void inv_set_acc_state(int state) 157{ 158 rh.acc_state = state; 159 return; 160} 161 162/** Returns the motion state 163* @param[out] cntr Number of previous times a no motion event has occured in a row. 164* @return Returns INV_SUCCESS if successful or an error code if not. 165*/ 166int inv_get_motion_state(unsigned int *cntr) 167{ 168 *cntr = rh.motion_state_counter; 169 return rh.motion_state; 170} 171 172/** Sets the motion state 173 * @param[in] state motion state where INV_NO_MOTION is not moving 174 * and INV_MOTION is moving. 175 */ 176void inv_set_motion_state(unsigned char state) 177{ 178 long set; 179 if (state == rh.motion_state) { 180 if (state == INV_NO_MOTION) { 181 rh.motion_state_counter++; 182 } else { 183 rh.motion_state_counter = 0; 184 } 185 return; 186 } 187 rh.motion_state_counter = 0; 188 rh.motion_state = state; 189 /* Equivalent to set = state, but #define's may change. */ 190 if (state == INV_MOTION) 191 set = INV_MSG_MOTION_EVENT; 192 else 193 set = INV_MSG_NO_MOTION_EVENT; 194 inv_set_message(set, (INV_MSG_MOTION_EVENT | INV_MSG_NO_MOTION_EVENT), 0); 195} 196 197/** Sets the local earth's magnetic field 198* @param[in] data Local earth's magnetic field in uT scaled by 2^16. 199* Length = 3. Y typically points north, Z typically points down in 200* northern hemisphere and up in southern hemisphere. 201*/ 202void inv_set_local_field(const long *data) 203{ 204 memcpy(rh.local_field, data, sizeof(rh.local_field)); 205} 206 207/** Gets the local earth's magnetic field 208* @param[out] data Local earth's magnetic field in uT scaled by 2^16. 209* Length = 3. Y typically points north, Z typically points down in 210* northern hemisphere and up in southern hemisphere. 211*/ 212void inv_get_local_field(long *data) 213{ 214 memcpy(data, rh.local_field, sizeof(rh.local_field)); 215} 216 217/** Sets the compass sensitivity 218 * @param[in] data Length 3, sensitivity for each compass axis 219 * scaled such that 1.0 = 2^30. 220 */ 221void inv_set_mag_scale(const long *data) 222{ 223 memcpy(rh.mag_scale, data, sizeof(rh.mag_scale)); 224} 225 226/** Gets the compass sensitivity 227 * @param[out] data Length 3, sensitivity for each compass axis 228 * scaled such that 1.0 = 2^30. 229 */ 230void inv_get_mag_scale(long *data) 231{ 232 memcpy(data, rh.mag_scale, sizeof(rh.mag_scale)); 233} 234 235/** Gets gravity vector 236 * @param[out] data gravity vector in body frame scaled such that 1.0 = 2^30. 237 * @return Returns INV_SUCCESS if successful or an error code if not. 238 */ 239inv_error_t inv_get_gravity(long *data) 240{ 241 data[0] = 242 inv_q29_mult(rh.nav_quat[1], rh.nav_quat[3]) - inv_q29_mult(rh.nav_quat[2], rh.nav_quat[0]); 243 data[1] = 244 inv_q29_mult(rh.nav_quat[2], rh.nav_quat[3]) + inv_q29_mult(rh.nav_quat[1], rh.nav_quat[0]); 245 data[2] = 246 (inv_q29_mult(rh.nav_quat[3], rh.nav_quat[3]) + inv_q29_mult(rh.nav_quat[0], rh.nav_quat[0])) - 247 1073741824L; 248 249 return INV_SUCCESS; 250} 251 252/** Returns a quaternion based only on accel. 253 * @param[out] data 3-axis accel quaternion scaled such that 1.0 = 2^30. 254 * @return Returns INV_SUCCESS if successful or an error code if not. 255 */ 256inv_error_t inv_get_accel_quaternion(long *data) 257{ 258 memcpy(data, rh.accel_quat, sizeof(rh.accel_quat)); 259 return INV_SUCCESS; 260} 261inv_error_t inv_get_gravity_6x(long *data) 262{ 263 data[0] = 264 inv_q29_mult(rh.gam_quat[1], rh.gam_quat[3]) - inv_q29_mult(rh.gam_quat[2], rh.gam_quat[0]); 265 data[1] = 266 inv_q29_mult(rh.gam_quat[2], rh.gam_quat[3]) + inv_q29_mult(rh.gam_quat[1], rh.gam_quat[0]); 267 data[2] = 268 (inv_q29_mult(rh.gam_quat[3], rh.gam_quat[3]) + inv_q29_mult(rh.gam_quat[0], rh.gam_quat[0])) - 269 1073741824L; 270 return INV_SUCCESS; 271} 272/** Returns a quaternion based only on gyro and accel. 273 * @param[out] data 6-axis gyro and accel quaternion scaled such that 1.0 = 2^30. 274 * @return Returns INV_SUCCESS if successful or an error code if not. 275 */ 276inv_error_t inv_get_6axis_quaternion(long *data, inv_time_t *timestamp) 277{ 278 memcpy(data, rh.gam_quat, sizeof(rh.gam_quat)); 279 *timestamp = rh.gam_timestamp; 280 return INV_SUCCESS; 281} 282 283/** Returns a quaternion. 284 * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30. 285 * @return Returns INV_SUCCESS if successful or an error code if not. 286 */ 287inv_error_t inv_get_quaternion(long *data) 288{ 289 if (rh.status & (INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET)) { 290 inv_q_mult(rh.compass_correction, rh.gam_quat, rh.nav_quat); 291 rh.status &= ~(INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET); 292 } 293 memcpy(data, rh.nav_quat, sizeof(rh.nav_quat)); 294 return INV_SUCCESS; 295} 296 297/** Returns a quaternion based only on compass and accel. 298 * @param[out] data 6-axis compass and accel quaternion scaled such that 1.0 = 2^30. 299 * @return Returns INV_SUCCESS if successful or an error code if not. 300 */ 301inv_error_t inv_get_geomagnetic_quaternion(long *data, inv_time_t *timestamp) 302{ 303 if (rh.status & INV_GEOMAGNETIC_CORRECTION_SET) { 304 inv_q_mult(rh.geomag_compass_correction, rh.accel_quat, rh.geomag_quat); 305 rh.status &= ~(INV_GEOMAGNETIC_CORRECTION_SET); 306 } 307 memcpy(data, rh.geomag_quat, sizeof(rh.geomag_quat)); 308 *timestamp = rh.geomag_timestamp; 309 return INV_SUCCESS; 310} 311 312/** Returns a quaternion. 313 * @param[out] data 9-axis quaternion. 314 * @return Returns INV_SUCCESS if successful or an error code if not. 315 */ 316inv_error_t inv_get_quaternion_float(float *data) 317{ 318 long ldata[4]; 319 inv_error_t result = inv_get_quaternion(ldata); 320 data[0] = inv_q30_to_float(ldata[0]); 321 data[1] = inv_q30_to_float(ldata[1]); 322 data[2] = inv_q30_to_float(ldata[2]); 323 data[3] = inv_q30_to_float(ldata[3]); 324 return result; 325} 326 327/** Returns a quaternion with accuracy and timestamp. 328 * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30. 329 * @param[out] accuracy Accuracy of quaternion, 0-3, where 3 is most accurate. 330 * @param[out] timestamp Timestamp of this quaternion in nanoseconds 331 */ 332void inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp) 333{ 334 inv_get_quaternion(data); 335 *timestamp = inv_get_last_timestamp(); 336 if (inv_get_compass_on()) { 337 *accuracy = inv_get_mag_accuracy(); 338 } else if (inv_get_gyro_on()) { 339 *accuracy = inv_get_gyro_accuracy(); 340 }else if (inv_get_accel_on()) { 341 *accuracy = inv_get_accel_accuracy(); 342 } else { 343 *accuracy = 0; 344 } 345} 346 347/** Callback that gets called everytime there is new data. It is 348 * registered by inv_start_results_holder(). 349 * @param[in] sensor_cal New sensor data to process. 350 * @return Returns INV_SUCCESS if successful or an error code if not. 351 */ 352inv_error_t inv_generate_results(struct inv_sensor_cal_t *sensor_cal) 353{ 354 rh.sensor = sensor_cal; 355 return INV_SUCCESS; 356} 357 358/** Function to turn on this module. This is automatically called by 359 * inv_enable_results_holder(). Typically not called by users. 360 * @return Returns INV_SUCCESS if successful or an error code if not. 361 */ 362inv_error_t inv_start_results_holder(void) 363{ 364 inv_register_data_cb(inv_generate_results, INV_PRIORITY_RESULTS_HOLDER, 365 INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW); 366 return INV_SUCCESS; 367} 368 369/** Initializes results holder. This is called automatically by the 370* enable function inv_enable_results_holder(). It may be called any time the feature is enabled, but 371* is typically not needed to be called by outside callers. 372* @return Returns INV_SUCCESS if successful or an error code if not. 373*/ 374inv_error_t inv_init_results_holder(void) 375{ 376 memset(&rh, 0, sizeof(rh)); 377 rh.mag_scale[0] = 1L<<30; 378 rh.mag_scale[1] = 1L<<30; 379 rh.mag_scale[2] = 1L<<30; 380 rh.compass_correction[0] = 1L<<30; 381 rh.gam_quat[0] = 1L<<30; 382 rh.nav_quat[0] = 1L<<30; 383 rh.geomag_quat[0] = 1L<<30; 384 rh.accel_quat[0] = 1L<<30; 385 rh.geomag_compass_correction[0] = 1L<<30; 386 rh.quat_confidence_interval = (float)M_PI; 387 return INV_SUCCESS; 388} 389 390/** Turns on storage of results. 391*/ 392inv_error_t inv_enable_results_holder() 393{ 394 inv_error_t result; 395 result = inv_init_results_holder(); 396 if ( result ) { 397 return result; 398 } 399 400 result = inv_register_mpl_start_notification(inv_start_results_holder); 401 return result; 402} 403 404/** Sets state of if we know the accel bias. 405 * @return return 1 if we know the accel bias, 0 if not. 406 * it is set with inv_set_accel_bias_found() 407 */ 408int inv_got_accel_bias() 409{ 410 return rh.got_accel_bias; 411} 412 413/** Sets whether we know the accel bias 414 * @param[in] state Set to 1 if we know the accel bias. 415 * Can be retrieved with inv_got_accel_bias() 416 */ 417void inv_set_accel_bias_found(int state) 418{ 419 rh.got_accel_bias = state; 420} 421 422/** Sets state of if we know the compass bias. 423 * @return return 1 if we know the compass bias, 0 if not. 424 * it is set with inv_set_compass_bias_found() 425 */ 426int inv_got_compass_bias() 427{ 428 return rh.got_compass_bias; 429} 430 431/** Sets whether we know the compass bias 432 * @param[in] state Set to 1 if we know the compass bias. 433 * Can be retrieved with inv_got_compass_bias() 434 */ 435void inv_set_compass_bias_found(int state) 436{ 437 rh.got_compass_bias = state; 438} 439 440/** Sets the compass state. 441 * @param[in] state Compass state. It can be retrieved with inv_get_compass_state(). 442 */ 443void inv_set_compass_state(int state) 444{ 445 rh.compass_state = state; 446} 447 448/** Get's the compass state 449 * @return the compass state that was set with inv_set_compass_state() 450 */ 451int inv_get_compass_state() 452{ 453 return rh.compass_state; 454} 455 456/** Set compass bias error. See inv_get_compass_bias_error() 457 * @param[in] bias_error Set's how accurate we know the compass bias. It is the 458 * error squared. 459 */ 460void inv_set_compass_bias_error(const long *bias_error) 461{ 462 memcpy(rh.compass_bias_error, bias_error, sizeof(rh.compass_bias_error)); 463} 464 465/** Get's compass bias error. See inv_set_compass_bias_error() for setting. 466 * @param[out] bias_error Accuracy as to how well the compass bias is known. It is the error squared. 467 */ 468void inv_get_compass_bias_error(long *bias_error) 469{ 470 memcpy(bias_error, rh.compass_bias_error, sizeof(rh.compass_bias_error)); 471} 472 473/** 474 * @brief Returns 3-element vector of accelerometer data in body frame 475 * with gravity removed 476 * @param[out] data 3-element vector of accelerometer data in body frame 477 * with gravity removed 478 * @return INV_SUCCESS if successful 479 * INV_ERROR_INVALID_PARAMETER if invalid input pointer 480 */ 481inv_error_t inv_get_linear_accel(long *data) 482{ 483 long gravity[3]; 484 485 if (data != NULL) 486 { 487 inv_get_accel_set(data, NULL, NULL); 488 inv_get_gravity(gravity); 489 data[0] -= gravity[0] >> 14; 490 data[1] -= gravity[1] >> 14; 491 data[2] -= gravity[2] >> 14; 492 return INV_SUCCESS; 493 } 494 else { 495 return INV_ERROR_INVALID_PARAMETER; 496 } 497} 498 499/** 500 * @brief Returns 3-element vector of accelerometer data in body frame 501 * @param[out] data 3-element vector of accelerometer data in body frame 502 * @return INV_SUCCESS if successful 503 * INV_ERROR_INVALID_PARAMETER if invalid input pointer 504 */ 505inv_error_t inv_get_accel(long *data) 506{ 507 if (data != NULL) { 508 inv_get_accel_set(data, NULL, NULL); 509 return INV_SUCCESS; 510 } 511 else { 512 return INV_ERROR_INVALID_PARAMETER; 513 } 514} 515 516/** 517 * @brief Returns 3-element vector of accelerometer float data 518 * @param[out] data 3-element vector of accelerometer float data 519 * @return INV_SUCCESS if successful 520 * INV_ERROR_INVALID_PARAMETER if invalid input pointer 521 */ 522inv_error_t inv_get_accel_float(float *data) 523{ 524 long tdata[3]; 525 unsigned char i; 526 527 if (data != NULL && !inv_get_accel(tdata)) { 528 for (i = 0; i < 3; ++i) { 529 data[i] = ((float)tdata[i] / (1L << 16)); 530 } 531 return INV_SUCCESS; 532 } 533 else { 534 return INV_ERROR_INVALID_PARAMETER; 535 } 536} 537 538/** 539 * @brief Returns 3-element vector of gyro float data 540 * @param[out] data 3-element vector of gyro float data 541 * @return INV_SUCCESS if successful 542 * INV_ERROR_INVALID_PARAMETER if invalid input pointer 543 */ 544inv_error_t inv_get_gyro_float(float *data) 545{ 546 long tdata[3]; 547 unsigned char i; 548 549 if (data != NULL) { 550 inv_get_gyro_set(tdata, NULL, NULL); 551 for (i = 0; i < 3; ++i) { 552 data[i] = ((float)tdata[i] / (1L << 16)); 553 } 554 return INV_SUCCESS; 555 } 556 else { 557 return INV_ERROR_INVALID_PARAMETER; 558 } 559} 560 561/** Set 9 axis 95% heading confidence interval for quaternion 562* @param[in] ci Confidence interval in radians. 563*/ 564void inv_set_heading_confidence_interval(float ci) 565{ 566 rh.quat_confidence_interval = ci; 567} 568 569/** Get 9 axis 95% heading confidence interval for quaternion 570* @return Confidence interval in radians. 571*/ 572float inv_get_heading_confidence_interval(void) 573{ 574 return rh.quat_confidence_interval; 575} 576 577/** Set 6 axis (accel and compass) 95% heading confidence interval for quaternion 578* @param[in] ci Confidence interval in radians. 579*/ 580void inv_set_accel_compass_confidence_interval(float ci) 581{ 582 rh.geo_mag_confidence_interval = ci; 583} 584 585/** Get 6 axis (accel and compass) 95% heading confidence interval for quaternion 586* @return Confidence interval in radians. 587*/ 588float inv_get_accel_compass_confidence_interval(void) 589{ 590 return rh.geo_mag_confidence_interval; 591} 592 593/** 594 * @brief Returns 3-element vector of linear accel float data 595 * @param[out] data 3-element vector of linear aceel float data 596 * @return INV_SUCCESS if successful 597 * INV_ERROR_INVALID_PARAMETER if invalid input pointer 598 */ 599inv_error_t inv_get_linear_accel_float(float *data) 600{ 601 long tdata[3]; 602 unsigned char i; 603 604 if (data != NULL && !inv_get_linear_accel(tdata)) { 605 for (i = 0; i < 3; ++i) { 606 data[i] = ((float)tdata[i] / (1L << 16)); 607 } 608 return INV_SUCCESS; 609 } 610 else { 611 return INV_ERROR_INVALID_PARAMETER; 612 } 613} 614 615/** 616 * @} 617 */ 618