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 32 struct 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 }; 58 static 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 */ 65 void 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 */ 77 void 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 */ 88 void 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 */ 100 void 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 */ 112 void 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 */ 123 void 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 */ 132 int 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 */ 140 void 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 */ 148 int 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 */ 156 void 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 */ 166 int 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 */ 176 void 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 */ 202 void 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 */ 212 void 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 */ 221 void 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 */ 230 void 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 */ 239 inv_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 */ 256 inv_error_t inv_get_accel_quaternion(long *data) 257 { 258 memcpy(data, rh.accel_quat, sizeof(rh.accel_quat)); 259 return INV_SUCCESS; 260 } 261 inv_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 */ 276 inv_error_t inv_get_6axis_quaternion(long *data) 277 { 278 memcpy(data, rh.gam_quat, sizeof(rh.gam_quat)); 279 return INV_SUCCESS; 280 } 281 282 /** Returns a quaternion. 283 * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30. 284 * @return Returns INV_SUCCESS if successful or an error code if not. 285 */ 286 inv_error_t inv_get_quaternion(long *data) 287 { 288 if (rh.status & (INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET)) { 289 inv_q_mult(rh.compass_correction, rh.gam_quat, rh.nav_quat); 290 rh.status &= ~(INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET); 291 } 292 memcpy(data, rh.nav_quat, sizeof(rh.nav_quat)); 293 return INV_SUCCESS; 294 } 295 296 /** Returns a quaternion based only on compass and accel. 297 * @param[out] data 6-axis compass and accel quaternion scaled such that 1.0 = 2^30. 298 * @return Returns INV_SUCCESS if successful or an error code if not. 299 */ 300 inv_error_t inv_get_geomagnetic_quaternion(long *data, inv_time_t *timestamp) 301 { 302 if (rh.status & INV_GEOMAGNETIC_CORRECTION_SET) { 303 inv_q_mult(rh.geomag_compass_correction, rh.accel_quat, rh.geomag_quat); 304 rh.status &= ~(INV_GEOMAGNETIC_CORRECTION_SET); 305 } 306 memcpy(data, rh.geomag_quat, sizeof(rh.geomag_quat)); 307 *timestamp = rh.geomag_timestamp; 308 return INV_SUCCESS; 309 } 310 311 /** Returns a quaternion. 312 * @param[out] data 9-axis quaternion. 313 * @return Returns INV_SUCCESS if successful or an error code if not. 314 */ 315 inv_error_t inv_get_quaternion_float(float *data) 316 { 317 long ldata[4]; 318 inv_error_t result = inv_get_quaternion(ldata); 319 data[0] = inv_q30_to_float(ldata[0]); 320 data[1] = inv_q30_to_float(ldata[1]); 321 data[2] = inv_q30_to_float(ldata[2]); 322 data[3] = inv_q30_to_float(ldata[3]); 323 return result; 324 } 325 326 /** Returns a quaternion with accuracy and timestamp. 327 * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30. 328 * @param[out] accuracy Accuracy of quaternion, 0-3, where 3 is most accurate. 329 * @param[out] timestamp Timestamp of this quaternion in nanoseconds 330 */ 331 void inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp) 332 { 333 inv_get_quaternion(data); 334 *timestamp = inv_get_last_timestamp(); 335 if (inv_get_compass_on()) { 336 *accuracy = inv_get_mag_accuracy(); 337 } else if (inv_get_gyro_on()) { 338 *accuracy = inv_get_gyro_accuracy(); 339 }else if (inv_get_accel_on()) { 340 *accuracy = inv_get_accel_accuracy(); 341 } else { 342 *accuracy = 0; 343 } 344 } 345 346 /** Callback that gets called everytime there is new data. It is 347 * registered by inv_start_results_holder(). 348 * @param[in] sensor_cal New sensor data to process. 349 * @return Returns INV_SUCCESS if successful or an error code if not. 350 */ 351 inv_error_t inv_generate_results(struct inv_sensor_cal_t *sensor_cal) 352 { 353 rh.sensor = sensor_cal; 354 return INV_SUCCESS; 355 } 356 357 /** Function to turn on this module. This is automatically called by 358 * inv_enable_results_holder(). Typically not called by users. 359 * @return Returns INV_SUCCESS if successful or an error code if not. 360 */ 361 inv_error_t inv_start_results_holder(void) 362 { 363 inv_register_data_cb(inv_generate_results, INV_PRIORITY_RESULTS_HOLDER, 364 INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW); 365 return INV_SUCCESS; 366 } 367 368 /** Initializes results holder. This is called automatically by the 369 * enable function inv_enable_results_holder(). It may be called any time the feature is enabled, but 370 * is typically not needed to be called by outside callers. 371 * @return Returns INV_SUCCESS if successful or an error code if not. 372 */ 373 inv_error_t inv_init_results_holder(void) 374 { 375 memset(&rh, 0, sizeof(rh)); 376 rh.mag_scale[0] = 1L<<30; 377 rh.mag_scale[1] = 1L<<30; 378 rh.mag_scale[2] = 1L<<30; 379 rh.compass_correction[0] = 1L<<30; 380 rh.gam_quat[0] = 1L<<30; 381 rh.nav_quat[0] = 1L<<30; 382 rh.geomag_quat[0] = 1L<<30; 383 rh.accel_quat[0] = 1L<<30; 384 rh.geomag_compass_correction[0] = 1L<<30; 385 rh.quat_confidence_interval = (float)M_PI; 386 return INV_SUCCESS; 387 } 388 389 /** Turns on storage of results. 390 */ 391 inv_error_t inv_enable_results_holder() 392 { 393 inv_error_t result; 394 result = inv_init_results_holder(); 395 if ( result ) { 396 return result; 397 } 398 399 result = inv_register_mpl_start_notification(inv_start_results_holder); 400 return result; 401 } 402 403 /** Sets state of if we know the accel bias. 404 * @return return 1 if we know the accel bias, 0 if not. 405 * it is set with inv_set_accel_bias_found() 406 */ 407 int inv_got_accel_bias() 408 { 409 return rh.got_accel_bias; 410 } 411 412 /** Sets whether we know the accel bias 413 * @param[in] state Set to 1 if we know the accel bias. 414 * Can be retrieved with inv_got_accel_bias() 415 */ 416 void inv_set_accel_bias_found(int state) 417 { 418 rh.got_accel_bias = state; 419 } 420 421 /** Sets state of if we know the compass bias. 422 * @return return 1 if we know the compass bias, 0 if not. 423 * it is set with inv_set_compass_bias_found() 424 */ 425 int inv_got_compass_bias() 426 { 427 return rh.got_compass_bias; 428 } 429 430 /** Sets whether we know the compass bias 431 * @param[in] state Set to 1 if we know the compass bias. 432 * Can be retrieved with inv_got_compass_bias() 433 */ 434 void inv_set_compass_bias_found(int state) 435 { 436 rh.got_compass_bias = state; 437 } 438 439 /** Sets the compass state. 440 * @param[in] state Compass state. It can be retrieved with inv_get_compass_state(). 441 */ 442 void inv_set_compass_state(int state) 443 { 444 rh.compass_state = state; 445 } 446 447 /** Get's the compass state 448 * @return the compass state that was set with inv_set_compass_state() 449 */ 450 int inv_get_compass_state() 451 { 452 return rh.compass_state; 453 } 454 455 /** Set compass bias error. See inv_get_compass_bias_error() 456 * @param[in] bias_error Set's how accurate we know the compass bias. It is the 457 * error squared. 458 */ 459 void inv_set_compass_bias_error(const long *bias_error) 460 { 461 memcpy(rh.compass_bias_error, bias_error, sizeof(rh.compass_bias_error)); 462 } 463 464 /** Get's compass bias error. See inv_set_compass_bias_error() for setting. 465 * @param[out] bias_error Accuracy as to how well the compass bias is known. It is the error squared. 466 */ 467 void inv_get_compass_bias_error(long *bias_error) 468 { 469 memcpy(bias_error, rh.compass_bias_error, sizeof(rh.compass_bias_error)); 470 } 471 472 /** 473 * @brief Returns 3-element vector of accelerometer data in body frame 474 * with gravity removed 475 * @param[out] data 3-element vector of accelerometer data in body frame 476 * with gravity removed 477 * @return INV_SUCCESS if successful 478 * INV_ERROR_INVALID_PARAMETER if invalid input pointer 479 */ 480 inv_error_t inv_get_linear_accel(long *data) 481 { 482 long gravity[3]; 483 484 if (data != NULL) 485 { 486 inv_get_accel_set(data, NULL, NULL); 487 inv_get_gravity(gravity); 488 data[0] -= gravity[0] >> 14; 489 data[1] -= gravity[1] >> 14; 490 data[2] -= gravity[2] >> 14; 491 return INV_SUCCESS; 492 } 493 else { 494 return INV_ERROR_INVALID_PARAMETER; 495 } 496 } 497 498 /** 499 * @brief Returns 3-element vector of accelerometer data in body frame 500 * @param[out] data 3-element vector of accelerometer data in body frame 501 * @return INV_SUCCESS if successful 502 * INV_ERROR_INVALID_PARAMETER if invalid input pointer 503 */ 504 inv_error_t inv_get_accel(long *data) 505 { 506 if (data != NULL) { 507 inv_get_accel_set(data, NULL, NULL); 508 return INV_SUCCESS; 509 } 510 else { 511 return INV_ERROR_INVALID_PARAMETER; 512 } 513 } 514 515 /** 516 * @brief Returns 3-element vector of accelerometer float data 517 * @param[out] data 3-element vector of accelerometer float data 518 * @return INV_SUCCESS if successful 519 * INV_ERROR_INVALID_PARAMETER if invalid input pointer 520 */ 521 inv_error_t inv_get_accel_float(float *data) 522 { 523 long tdata[3]; 524 unsigned char i; 525 526 if (data != NULL && !inv_get_accel(tdata)) { 527 for (i = 0; i < 3; ++i) { 528 data[i] = ((float)tdata[i] / (1L << 16)); 529 } 530 return INV_SUCCESS; 531 } 532 else { 533 return INV_ERROR_INVALID_PARAMETER; 534 } 535 } 536 537 /** 538 * @brief Returns 3-element vector of gyro float data 539 * @param[out] data 3-element vector of gyro float data 540 * @return INV_SUCCESS if successful 541 * INV_ERROR_INVALID_PARAMETER if invalid input pointer 542 */ 543 inv_error_t inv_get_gyro_float(float *data) 544 { 545 long tdata[3]; 546 unsigned char i; 547 548 if (data != NULL) { 549 inv_get_gyro_set(tdata, NULL, NULL); 550 for (i = 0; i < 3; ++i) { 551 data[i] = ((float)tdata[i] / (1L << 16)); 552 } 553 return INV_SUCCESS; 554 } 555 else { 556 return INV_ERROR_INVALID_PARAMETER; 557 } 558 } 559 560 /** Set 9 axis 95% heading confidence interval for quaternion 561 * @param[in] ci Confidence interval in radians. 562 */ 563 void inv_set_heading_confidence_interval(float ci) 564 { 565 rh.quat_confidence_interval = ci; 566 } 567 568 /** Get 9 axis 95% heading confidence interval for quaternion 569 * @return Confidence interval in radians. 570 */ 571 float inv_get_heading_confidence_interval(void) 572 { 573 return rh.quat_confidence_interval; 574 } 575 576 /** Set 6 axis (accel and compass) 95% heading confidence interval for quaternion 577 * @param[in] ci Confidence interval in radians. 578 */ 579 void inv_set_accel_compass_confidence_interval(float ci) 580 { 581 rh.geo_mag_confidence_interval = ci; 582 } 583 584 /** Get 6 axis (accel and compass) 95% heading confidence interval for quaternion 585 * @return Confidence interval in radians. 586 */ 587 float inv_get_accel_compass_confidence_interval(void) 588 { 589 return rh.geo_mag_confidence_interval; 590 } 591 592 /** 593 * @brief Returns 3-element vector of linear accel float data 594 * @param[out] data 3-element vector of linear aceel float data 595 * @return INV_SUCCESS if successful 596 * INV_ERROR_INVALID_PARAMETER if invalid input pointer 597 */ 598 inv_error_t inv_get_linear_accel_float(float *data) 599 { 600 long tdata[3]; 601 unsigned char i; 602 603 if (data != NULL && !inv_get_linear_accel(tdata)) { 604 for (i = 0; i < 3; ++i) { 605 data[i] = ((float)tdata[i] / (1L << 16)); 606 } 607 return INV_SUCCESS; 608 } 609 else { 610 return INV_ERROR_INVALID_PARAMETER; 611 } 612 } 613 614 /** 615 * @} 616 */ 617