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 /** 9 * @defgroup Data_Builder data_builder 10 * @brief Motion Library - Data Builder 11 * Constructs and Creates the data for MPL 12 * 13 * @{ 14 * @file data_builder.c 15 * @brief Data Builder. 16 */ 17 18 #undef MPL_LOG_NDEBUG 19 #define MPL_LOG_NDEBUG 1 /* Use 0 to turn on MPL_LOGV output */ 20 21 #include <string.h> 22 23 #include "ml_math_func.h" 24 #include "data_builder.h" 25 #include "mlmath.h" 26 #include "storage_manager.h" 27 #include "message_layer.h" 28 #include "results_holder.h" 29 30 #include "log.h" 31 #undef MPL_LOG_TAG 32 #define MPL_LOG_TAG "MLLITE" 33 34 typedef inv_error_t (*inv_process_cb_func)(struct inv_sensor_cal_t *data); 35 36 struct process_t { 37 inv_process_cb_func func; 38 int priority; 39 int data_required; 40 }; 41 42 struct inv_data_builder_t { 43 int num_cb; 44 struct process_t process[INV_MAX_DATA_CB]; 45 struct inv_db_save_t save; 46 struct inv_db_save_mpl_t save_mpl; 47 struct inv_db_save_accel_mpl_t save_accel_mpl; 48 int compass_disturbance; 49 int mode; 50 #ifdef INV_PLAYBACK_DBG 51 int debug_mode; 52 int last_mode; 53 FILE *file; 54 #endif 55 }; 56 57 void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias); 58 static void inv_set_contiguous(void); 59 60 static struct inv_data_builder_t inv_data_builder; 61 static struct inv_sensor_cal_t sensors; 62 63 #ifdef INV_PLAYBACK_DBG 64 65 /** Turn on data logging to allow playback of same scenario at a later time. 66 * @param[in] file File to write to, must be open. 67 */ 68 void inv_turn_on_data_logging(FILE *file) 69 { 70 MPL_LOGV("input data logging started\n"); 71 inv_data_builder.file = file; 72 inv_data_builder.debug_mode = RD_RECORD; 73 } 74 75 /** Turn off data logging to allow playback of same scenario at a later time. 76 * File passed to inv_turn_on_data_logging() must be closed after calling this. 77 */ 78 void inv_turn_off_data_logging() 79 { 80 MPL_LOGV("input data logging stopped\n"); 81 inv_data_builder.debug_mode = RD_NO_DEBUG; 82 inv_data_builder.file = NULL; 83 } 84 #endif 85 86 /** Gets last value of raw compass data. 87 * @param[out] raw Raw compass data in mounting frame in hardware units. Length 3. 88 */ 89 void inv_get_raw_compass(short *raw) 90 { 91 memcpy(raw, sensors.compass.raw, sizeof(sensors.compass.raw)); 92 } 93 94 /** This function receives the data that was stored in non-volatile memory between power off */ 95 static inv_error_t inv_db_load_func(const unsigned char *data) 96 { 97 memcpy(&inv_data_builder.save, data, sizeof(inv_data_builder.save)); 98 // copy in the saved accuracy in the actual sensors accuracy 99 sensors.gyro.accuracy = inv_data_builder.save.gyro_accuracy; 100 sensors.accel.accuracy = inv_data_builder.save.accel_accuracy; 101 sensors.compass.accuracy = inv_data_builder.save.compass_accuracy; 102 // TODO 103 if (sensors.accel.accuracy == 3) { 104 inv_set_accel_bias_found(1); 105 } 106 if (sensors.compass.accuracy == 3) { 107 inv_set_compass_bias_found(1); 108 } 109 return INV_SUCCESS; 110 } 111 112 /** This function returns the data to be stored in non-volatile memory between power off */ 113 static inv_error_t inv_db_save_func(unsigned char *data) 114 { 115 memcpy(data, &inv_data_builder.save, sizeof(inv_data_builder.save)); 116 return INV_SUCCESS; 117 } 118 119 /** This function receives the data for mpl that was stored in non-volatile memory between power off */ 120 static inv_error_t inv_db_load_mpl_func(const unsigned char *data) 121 { 122 memcpy(&inv_data_builder.save_mpl, data, sizeof(inv_data_builder.save_mpl)); 123 124 return INV_SUCCESS; 125 } 126 127 /** This function returns the data for mpl to be stored in non-volatile memory between power off */ 128 static inv_error_t inv_db_save_mpl_func(unsigned char *data) 129 { 130 memcpy(data, &inv_data_builder.save_mpl, sizeof(inv_data_builder.save_mpl)); 131 return INV_SUCCESS; 132 } 133 134 /** This function receives the data for mpl that was stored in non-volatile memory between power off */ 135 static inv_error_t inv_db_load_accel_mpl_func(const unsigned char *data) 136 { 137 memcpy(&inv_data_builder.save_accel_mpl, data, sizeof(inv_data_builder.save_accel_mpl)); 138 139 return INV_SUCCESS; 140 } 141 142 /** This function returns the data for mpl to be stored in non-volatile memory between power off */ 143 static inv_error_t inv_db_save_accel_mpl_func(unsigned char *data) 144 { 145 memcpy(data, &inv_data_builder.save_accel_mpl, sizeof(inv_data_builder.save_accel_mpl)); 146 return INV_SUCCESS; 147 } 148 149 /** Initialize the data builder 150 */ 151 inv_error_t inv_init_data_builder(void) 152 { 153 /* TODO: Hardcode temperature scale/offset here. */ 154 memset(&inv_data_builder, 0, sizeof(inv_data_builder)); 155 memset(&sensors, 0, sizeof(sensors)); 156 157 // disable the soft iron transform process 158 inv_reset_compass_soft_iron_matrix(); 159 160 return ((inv_register_load_store(inv_db_load_func, inv_db_save_func, 161 sizeof(inv_data_builder.save), 162 INV_DB_SAVE_KEY)) 163 | (inv_register_load_store(inv_db_load_mpl_func, inv_db_save_mpl_func, 164 sizeof(inv_data_builder.save_mpl), 165 INV_DB_SAVE_MPL_KEY)) 166 | (inv_register_load_store(inv_db_load_accel_mpl_func, inv_db_save_accel_mpl_func, 167 sizeof(inv_data_builder.save_accel_mpl), 168 INV_DB_SAVE_ACCEL_MPL_KEY)) ); 169 } 170 171 /** Gyro sensitivity. 172 * @return A scale factor to convert device units to degrees per second scaled by 2^16 173 * such that degrees_per_second = device_units * sensitivity / 2^30. Typically 174 * it works out to be the maximum rate * 2^15. 175 */ 176 long inv_get_gyro_sensitivity(void) 177 { 178 return sensors.gyro.sensitivity; 179 } 180 181 /** Accel sensitivity. 182 * @return A scale factor to convert device units to g's scaled by 2^16 183 * such that g_s = device_units * sensitivity / 2^30. Typically 184 * it works out to be the maximum accel value in g's * 2^15. 185 */ 186 long inv_get_accel_sensitivity(void) 187 { 188 return sensors.accel.sensitivity; 189 } 190 191 /** Compass sensitivity. 192 * @return A scale factor to convert device units to micro Tesla scaled by 2^16 193 * such that uT = device_units * sensitivity / 2^30. Typically 194 * it works out to be the maximum uT * 2^15. 195 */ 196 long inv_get_compass_sensitivity(void) 197 { 198 return sensors.compass.sensitivity; 199 } 200 201 /** Sets orientation and sensitivity field for a sensor. 202 * @param[out] sensor Structure to apply settings to 203 * @param[in] orientation Orientation description of how part is mounted. 204 * @param[in] sensitivity A Scale factor to convert from hardware units to 205 * standard units (dps, uT, g). 206 */ 207 void set_sensor_orientation_and_scale(struct inv_single_sensor_t *sensor, 208 int orientation, long sensitivity) 209 { 210 int error = 0; 211 212 if (!sensitivity) { 213 // Sensitivity can't be zero 214 sensitivity = 1L<<16; 215 MPL_LOGE("\n\nCritical error! Sensitivity is zero.\n\n"); 216 } 217 218 sensor->sensitivity = sensitivity; 219 // Make sure we don't describe some impossible orientation 220 if ((orientation & 3) == 3) { 221 error = 1; 222 } 223 if ((orientation & 0x18) == 0x18) { 224 error = 1; 225 } 226 if ((orientation & 0xc0) == 0xc0) { 227 error = 1; 228 } 229 if (error) { 230 orientation = 0x88; // Identity 231 MPL_LOGE("\n\nCritical error! Impossible mounting orientation given. Using Identity instead\n\n"); 232 } 233 sensor->orientation = orientation; 234 } 235 236 /** Sets the Orientation and Sensitivity of the gyro data. 237 * @param[in] orientation A scalar defining the transformation from chip mounting 238 * to the body frame. The function inv_orientation_matrix_to_scalar() 239 * can convert the transformation matrix to this scalar and describes the 240 * scalar in further detail. 241 * @param[in] sensitivity A scale factor to convert device units to degrees per second scaled by 2^16 242 * such that degrees_per_second = device_units * sensitivity / 2^30. Typically 243 * it works out to be the maximum rate * 2^15. 244 */ 245 void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity) 246 { 247 #ifdef INV_PLAYBACK_DBG 248 if (inv_data_builder.debug_mode == RD_RECORD) { 249 int type = PLAYBACK_DBG_TYPE_G_ORIENT; 250 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 251 fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); 252 fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); 253 } 254 #endif 255 set_sensor_orientation_and_scale(&sensors.gyro, orientation, 256 sensitivity); 257 } 258 259 /** Set Gyro Sample rate in micro seconds. 260 * @param[in] sample_rate_us Set Gyro Sample rate in us 261 */ 262 void inv_set_gyro_sample_rate(long sample_rate_us) 263 { 264 #ifdef INV_PLAYBACK_DBG 265 if (inv_data_builder.debug_mode == RD_RECORD) { 266 int type = PLAYBACK_DBG_TYPE_G_SAMPLE_RATE; 267 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 268 fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); 269 } 270 #endif 271 sensors.gyro.sample_rate_us = sample_rate_us; 272 sensors.gyro.sample_rate_ms = sample_rate_us / 1000; 273 if (sensors.gyro.bandwidth == 0) { 274 sensors.gyro.bandwidth = (int)(1000000L / sample_rate_us); 275 } 276 } 277 278 /** Set Accel Sample rate in micro seconds. 279 * @param[in] sample_rate_us Set Accel Sample rate in us 280 */ 281 void inv_set_accel_sample_rate(long sample_rate_us) 282 { 283 #ifdef INV_PLAYBACK_DBG 284 if (inv_data_builder.debug_mode == RD_RECORD) { 285 int type = PLAYBACK_DBG_TYPE_A_SAMPLE_RATE; 286 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 287 fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); 288 } 289 #endif 290 sensors.accel.sample_rate_us = sample_rate_us; 291 sensors.accel.sample_rate_ms = sample_rate_us / 1000; 292 if (sensors.accel.bandwidth == 0) { 293 sensors.accel.bandwidth = (int)(1000000L / sample_rate_us); 294 } 295 } 296 297 /** Pick the smallest non-negative number. Priority to td1 on equal 298 * If both are negative, return the largest. 299 */ 300 static int inv_pick_best_time_difference(long td1, long td2) 301 { 302 if (td1 >= 0) { 303 if (td2 >= 0) { 304 if (td1 <= td2) { 305 // td1 306 return 0; 307 } else { 308 // td2 309 return 1; 310 } 311 } else { 312 // td1 313 return 0; 314 } 315 } else if (td2 >= 0) { 316 // td2 317 return 1; 318 } else { 319 // Both are negative 320 if (td1 >= td2) { 321 // td1 322 return 0; 323 } else { 324 // td2 325 return 1; 326 } 327 } 328 } 329 330 /** Returns timestame based upon a raw sensor, and returns if that sample has a new piece of data. 331 */ 332 static int inv_raw_sensor_timestamp(int sensor_number, inv_time_t *ts) 333 { 334 int status = 0; 335 switch (sensor_number) { 336 case 0: // Quat 337 *ts = sensors.quat.timestamp; 338 if (inv_data_builder.mode & INV_QUAT_NEW) 339 if (sensors.quat.timestamp_prev != sensors.quat.timestamp) 340 status = 1; 341 return status; 342 case 1: // Gyro 343 *ts = sensors.gyro.timestamp; 344 if (inv_data_builder.mode & INV_GYRO_NEW) 345 if (sensors.gyro.timestamp_prev != sensors.gyro.timestamp) 346 status = 1; 347 return status; 348 case 2: // Accel 349 *ts = sensors.accel.timestamp; 350 if (inv_data_builder.mode & INV_ACCEL_NEW) 351 if (sensors.accel.timestamp_prev != sensors.accel.timestamp) 352 status = 1; 353 return status; 354 case 3: // Compass 355 *ts = sensors.compass.timestamp; 356 if (inv_data_builder.mode & INV_MAG_NEW) 357 if (sensors.compass.timestamp_prev != sensors.compass.timestamp) 358 status = 1; 359 return status; 360 default: 361 *ts = 0; 362 return 0; 363 } 364 return 0; 365 } 366 367 /** Gets best timestamp and if there is a new piece of data for a 9-axis sensor combination. 368 * It does this by finding a raw sensor that has the closest sample rate that is at least as 369 * often desired. It also returns if that raw sensor has a new piece of data. 370 * Priority is 9-axis quat, 6-axis quat, 3-axis quat, gyro, compass, accel on ties. 371 * @return Returns 1, if the raw sensor being attached has new data, 0 otherwise. 372 */ 373 int inv_get_9_axis_timestamp(long sample_rate_us, inv_time_t *ts) 374 { 375 int status = 0; 376 long td[3]; 377 int idx,idx2; 378 379 if ((sensors.quat.status & (INV_QUAT_9AXIS | INV_SENSOR_ON)) == (INV_QUAT_9AXIS | INV_SENSOR_ON)) { 380 // 9-axis from DMP 381 *ts = sensors.quat.timestamp; 382 if (inv_data_builder.mode & INV_QUAT_NEW) 383 if (sensors.quat.timestamp_prev != sensors.quat.timestamp) 384 status = 1; 385 return status; 386 } 387 388 if ((sensors.compass.status & INV_SENSOR_ON) == 0) { 389 return 0; // Compass must be on 390 } 391 392 // At this point, we know compass is on. Check if accel or 6-axis quat is on 393 td[0] = sample_rate_us - sensors.quat.sample_rate_us; 394 td[1] = sample_rate_us - sensors.compass.sample_rate_us; 395 if ((sensors.quat.status & (INV_QUAT_6AXIS | INV_SENSOR_ON)) == (INV_QUAT_6AXIS | INV_SENSOR_ON)) { 396 // Sensor tied to compass or 6-axis 397 idx = inv_pick_best_time_difference(td[0], td[1]); 398 idx *= 3; // Sensor number is 0 (Quat) or 3 (Compass) 399 return inv_raw_sensor_timestamp(idx, ts); 400 } else if ((sensors.accel.status & INV_SENSOR_ON) == 0) { 401 return 0; // Accel must be on or 6-axis quat must be on 402 } 403 404 // At this point, we know accel is on. Check if 3-axis quat is on 405 td[2] = sample_rate_us - sensors.accel.sample_rate_us; 406 if ((sensors.quat.status & (INV_QUAT_3AXIS | INV_SENSOR_ON)) == (INV_QUAT_3AXIS | INV_SENSOR_ON)) { 407 idx = inv_pick_best_time_difference(td[0], td[1]); 408 idx2 = inv_pick_best_time_difference(td[idx], td[2]); 409 if (idx2 == 1) 410 idx = 2; 411 if (idx > 0) 412 idx++; // Skip gyro sensor in next function call 413 // 0 = quat, 2 = compass, 3=accel 414 return inv_raw_sensor_timestamp(idx, ts); 415 } 416 417 // No Quaternion data from outside, Gyro must be on 418 if ((sensors.gyro.status & INV_SENSOR_ON) == 0) { 419 return 0; // Gyro must be on 420 } 421 422 td[0] = sample_rate_us - sensors.gyro.sample_rate_us; 423 idx = inv_pick_best_time_difference(td[0], td[1]); 424 idx2 = inv_pick_best_time_difference(td[idx], td[2]); 425 if (idx2 == 1) 426 idx = 2; 427 idx++; 428 // 1 = gyro, 2 = compass, 3=accel 429 return inv_raw_sensor_timestamp(idx, ts); 430 } 431 432 /** Gets best timestamp and if there is a new piece of data for a 9-axis sensor combination. 433 * It does this by finding a raw sensor that has the closest sample rate that is at least as 434 * often desired. It also returns if that raw sensor has a new piece of data. 435 * Priority compass, accel on a tie 436 * @return Returns 1, if the raw sensor being attached has new data, 0 otherwise. 437 */ 438 int inv_get_6_axis_compass_accel_timestamp(long sample_rate_us, inv_time_t *ts) 439 { 440 long td[2]; 441 int idx; 442 443 if ((sensors.compass.status & INV_SENSOR_ON) == 0) { 444 return 0; // Compass must be on 445 } 446 if ((sensors.accel.status & INV_SENSOR_ON) == 0) { 447 return 0; // Accel must be on 448 } 449 450 // At this point, we know compass & accel are both on. 451 td[0] = sample_rate_us - sensors.accel.sample_rate_us; 452 td[1] = sample_rate_us - sensors.compass.sample_rate_us; 453 idx = inv_pick_best_time_difference(td[0], td[1]); 454 idx += 2; 455 return inv_raw_sensor_timestamp(idx, ts); 456 } 457 458 /** Gets best timestamp and if there is a new piece of data for a 9-axis sensor combination. 459 * It does this by finding a raw sensor that has the closest sample rate that is at least as 460 * often desired. It also returns if that raw sensor has a new piece of data. 461 * Priority is Quaternion-6axis, Quaternion 3-axis, Gyro, Accel 462 * @return Returns 1, if the raw sensor being attached has new data, 0 otherwise. 463 */ 464 int inv_get_6_axis_gyro_accel_timestamp(long sample_rate_us, inv_time_t *ts) 465 { 466 long td[2]; 467 int idx; 468 469 if ((sensors.quat.status & (INV_QUAT_6AXIS | INV_SENSOR_ON)) == (INV_QUAT_6AXIS | INV_SENSOR_ON)) { 470 // Sensor number is 0 (Quat) 471 return inv_raw_sensor_timestamp(0, ts); 472 } else if ((sensors.accel.status & INV_SENSOR_ON) == 0) { 473 return 0; // Accel must be on or 6-axis quat must be on 474 } 475 476 // At this point, we know accel is on. Check if 3-axis quat is on 477 td[0] = sample_rate_us - sensors.quat.sample_rate_us; 478 td[1] = sample_rate_us - sensors.accel.sample_rate_us; 479 if ((sensors.quat.status & (INV_QUAT_3AXIS | INV_SENSOR_ON)) == (INV_QUAT_3AXIS | INV_SENSOR_ON)) { 480 idx = inv_pick_best_time_difference(td[0], td[1]); 481 idx *= 2; 482 // 0 = quat, 3=accel 483 return inv_raw_sensor_timestamp(idx, ts); 484 } 485 486 // No Quaternion data from outside, Gyro must be on 487 if ((sensors.gyro.status & INV_SENSOR_ON) == 0) { 488 return 0; // Gyro must be on 489 } 490 491 td[0] = sample_rate_us - sensors.gyro.sample_rate_us; 492 idx = inv_pick_best_time_difference(td[0], td[1]); 493 idx *= 2; // 0=gyro 2=accel 494 idx++; 495 // 1 = gyro, 3=accel 496 return inv_raw_sensor_timestamp(idx, ts); 497 } 498 499 /** Set Compass Sample rate in micro seconds. 500 * @param[in] sample_rate_us Set Gyro Sample rate in micro seconds. 501 */ 502 void inv_set_compass_sample_rate(long sample_rate_us) 503 { 504 #ifdef INV_PLAYBACK_DBG 505 if (inv_data_builder.debug_mode == RD_RECORD) { 506 int type = PLAYBACK_DBG_TYPE_C_SAMPLE_RATE; 507 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 508 fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); 509 } 510 #endif 511 sensors.compass.sample_rate_us = sample_rate_us; 512 sensors.compass.sample_rate_ms = sample_rate_us / 1000; 513 if (sensors.compass.bandwidth == 0) { 514 sensors.compass.bandwidth = (int)(1000000L / sample_rate_us); 515 } 516 } 517 518 void inv_get_gyro_sample_rate_ms(long *sample_rate_ms) 519 { 520 *sample_rate_ms = sensors.gyro.sample_rate_ms; 521 } 522 523 void inv_get_accel_sample_rate_ms(long *sample_rate_ms) 524 { 525 *sample_rate_ms = sensors.accel.sample_rate_ms; 526 } 527 528 void inv_get_compass_sample_rate_ms(long *sample_rate_ms) 529 { 530 *sample_rate_ms = sensors.compass.sample_rate_ms; 531 } 532 533 /** Set Quat Sample rate in micro seconds. 534 * @param[in] sample_rate_us Set Quat Sample rate in us 535 */ 536 void inv_set_quat_sample_rate(long sample_rate_us) 537 { 538 #ifdef INV_PLAYBACK_DBG 539 if (inv_data_builder.debug_mode == RD_RECORD) { 540 int type = PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE; 541 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 542 fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); 543 } 544 #endif 545 sensors.quat.sample_rate_us = sample_rate_us; 546 sensors.quat.sample_rate_ms = sample_rate_us / 1000; 547 } 548 549 /** Set Gyro Bandwidth in Hz 550 * @param[in] bandwidth_hz Gyro bandwidth in Hz 551 */ 552 void inv_set_gyro_bandwidth(int bandwidth_hz) 553 { 554 sensors.gyro.bandwidth = bandwidth_hz; 555 } 556 557 /** Set Accel Bandwidth in Hz 558 * @param[in] bandwidth_hz Gyro bandwidth in Hz 559 */ 560 void inv_set_accel_bandwidth(int bandwidth_hz) 561 { 562 sensors.accel.bandwidth = bandwidth_hz; 563 } 564 565 /** Set Compass Bandwidth in Hz 566 * @param[in] bandwidth_hz Gyro bandwidth in Hz 567 */ 568 void inv_set_compass_bandwidth(int bandwidth_hz) 569 { 570 sensors.compass.bandwidth = bandwidth_hz; 571 } 572 573 /** Helper function stating whether the compass is on or off. 574 * @return TRUE if compass if on, 0 if compass if off 575 */ 576 int inv_get_compass_on() 577 { 578 return (sensors.compass.status & INV_SENSOR_ON) == INV_SENSOR_ON; 579 } 580 581 /** Helper function stating whether the gyro is on or off. 582 * @return TRUE if gyro if on, 0 if gyro if off 583 */ 584 int inv_get_gyro_on() 585 { 586 return (sensors.gyro.status & INV_SENSOR_ON) == INV_SENSOR_ON; 587 } 588 589 /** Helper function stating whether the acceleromter is on or off. 590 * @return TRUE if accel if on, 0 if accel if off 591 */ 592 int inv_get_accel_on() 593 { 594 return (sensors.accel.status & INV_SENSOR_ON) == INV_SENSOR_ON; 595 } 596 597 /** Get last timestamp across all 3 sensors that are on. 598 * This find out which timestamp has the largest value for sensors that are on. 599 * @return Returns INV_SUCCESS if successful or an error code if not. 600 */ 601 inv_time_t inv_get_last_timestamp() 602 { 603 inv_time_t timestamp = 0; 604 if (sensors.accel.status & INV_SENSOR_ON) { 605 timestamp = sensors.accel.timestamp; 606 } 607 if (sensors.gyro.status & INV_SENSOR_ON) { 608 if (timestamp < sensors.gyro.timestamp) { 609 timestamp = sensors.gyro.timestamp; 610 } 611 MPL_LOGV("g ts: %lld", timestamp); 612 } 613 if (sensors.compass.status & INV_SENSOR_ON) { 614 if (timestamp < sensors.compass.timestamp) { 615 timestamp = sensors.compass.timestamp; 616 } 617 } 618 if (sensors.temp.status & INV_SENSOR_ON) { 619 if (timestamp < sensors.temp.timestamp) { 620 timestamp = sensors.temp.timestamp; 621 } 622 } 623 if (sensors.quat.status & INV_SENSOR_ON) { 624 if (timestamp < sensors.quat.timestamp) { 625 timestamp = sensors.quat.timestamp; 626 } 627 MPL_LOGV("q ts: %lld", timestamp); 628 } 629 630 return timestamp; 631 } 632 633 /** Sets the orientation and sensitivity of the gyro data. 634 * @param[in] orientation A scalar defining the transformation from chip mounting 635 * to the body frame. The function inv_orientation_matrix_to_scalar() 636 * can convert the transformation matrix to this scalar and describes the 637 * scalar in further detail. 638 * @param[in] sensitivity A scale factor to convert device units to g's 639 * such that g's = device_units * sensitivity / 2^30. Typically 640 * it works out to be the maximum g_value * 2^15. 641 */ 642 void inv_set_accel_orientation_and_scale(int orientation, long sensitivity) 643 { 644 #ifdef INV_PLAYBACK_DBG 645 if (inv_data_builder.debug_mode == RD_RECORD) { 646 int type = PLAYBACK_DBG_TYPE_A_ORIENT; 647 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 648 fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); 649 fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); 650 } 651 #endif 652 set_sensor_orientation_and_scale(&sensors.accel, orientation, 653 sensitivity); 654 } 655 656 /** Sets the Orientation and Sensitivity of the gyro data. 657 * @param[in] orientation A scalar defining the transformation from chip mounting 658 * to the body frame. The function inv_orientation_matrix_to_scalar() 659 * can convert the transformation matrix to this scalar and describes the 660 * scalar in further detail. 661 * @param[in] sensitivity A scale factor to convert device units to uT 662 * such that uT = device_units * sensitivity / 2^30. Typically 663 * it works out to be the maximum uT_value * 2^15. 664 */ 665 void inv_set_compass_orientation_and_scale(int orientation, long sensitivity) 666 { 667 #ifdef INV_PLAYBACK_DBG 668 if (inv_data_builder.debug_mode == RD_RECORD) { 669 int type = PLAYBACK_DBG_TYPE_C_ORIENT; 670 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 671 fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); 672 fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); 673 } 674 #endif 675 set_sensor_orientation_and_scale(&sensors.compass, orientation, sensitivity); 676 } 677 678 void inv_matrix_vector_mult(const long *A, const long *x, long *y) 679 { 680 y[0] = inv_q30_mult(A[0], x[0]) + inv_q30_mult(A[1], x[1]) + inv_q30_mult(A[2], x[2]); 681 y[1] = inv_q30_mult(A[3], x[0]) + inv_q30_mult(A[4], x[1]) + inv_q30_mult(A[5], x[2]); 682 y[2] = inv_q30_mult(A[6], x[0]) + inv_q30_mult(A[7], x[1]) + inv_q30_mult(A[8], x[2]); 683 } 684 685 /** Takes raw data stored in the sensor, removes bias, and converts it to 686 * calibrated data in the body frame. Also store raw data for body frame. 687 * @param[in,out] sensor structure to modify 688 * @param[in] bias bias in the mounting frame, in hardware units scaled by 689 * 2^16. Length 3. 690 */ 691 void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias) 692 { 693 long raw32[3]; 694 695 // Convert raw to calibrated 696 raw32[0] = (long)sensor->raw[0] << 15; 697 raw32[1] = (long)sensor->raw[1] << 15; 698 raw32[2] = (long)sensor->raw[2] << 15; 699 700 inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_scaled); 701 702 raw32[0] -= bias[0] >> 1; 703 raw32[1] -= bias[1] >> 1; 704 raw32[2] -= bias[2] >> 1; 705 706 inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->calibrated); 707 708 sensor->status |= INV_CALIBRATED; 709 } 710 711 /** Returns the current bias for the compass 712 * @param[out] bias Compass bias in hardware units scaled by 2^16. In mounting frame. 713 * Length 3. 714 */ 715 void inv_get_compass_bias(long *bias) 716 { 717 if (bias != NULL) { 718 memcpy(bias, inv_data_builder.save.compass_bias, sizeof(inv_data_builder.save.compass_bias)); 719 } 720 } 721 722 /** Sets the compass bias 723 * @param[in] bias Length 3, in body frame, in hardware units scaled by 2^16 to allow fractional bit correction. 724 * @param[in] accuracy Accuracy of compass data, where 3=most accurate, and 0=least accurate. 725 */ 726 void inv_set_compass_bias(const long *bias, int accuracy) 727 { 728 if (memcmp(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias))) { 729 memcpy(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias)); 730 inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias); 731 } 732 sensors.compass.accuracy = accuracy; 733 inv_data_builder.save.compass_accuracy = accuracy; 734 inv_set_message(INV_MSG_NEW_CB_EVENT, INV_MSG_NEW_CB_EVENT, 0); 735 } 736 737 /** Set the state of a compass disturbance 738 * @param[in] dist 1=disturbance, 0=no disturbance 739 */ 740 void inv_set_compass_disturbance(int dist) 741 { 742 inv_data_builder.compass_disturbance = dist; 743 } 744 745 int inv_get_compass_disturbance(void) { 746 return inv_data_builder.compass_disturbance; 747 } 748 749 /** 750 * Sets the factory accel bias 751 * @param[in] bias 752 * Accel bias in hardware units (+/- 2 gee full scale assumed) 753 * scaled by 2^16. In chip mounting frame. Length of 3. 754 */ 755 void inv_set_accel_bias(const long *bias) 756 { 757 if (!bias) 758 return; 759 760 if (memcmp(inv_data_builder.save.factory_accel_bias, bias, 761 sizeof(inv_data_builder.save.factory_accel_bias))) { 762 memcpy(inv_data_builder.save.factory_accel_bias, bias, 763 sizeof(inv_data_builder.save.factory_accel_bias)); 764 } 765 inv_set_message(INV_MSG_NEW_FAB_EVENT, INV_MSG_NEW_FAB_EVENT, 0); 766 } 767 768 /** Sets the accel accuracy. 769 * @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. 770 */ 771 void inv_set_accel_accuracy(int accuracy) 772 { 773 sensors.accel.accuracy = accuracy; 774 inv_data_builder.save.accel_accuracy = accuracy; 775 } 776 777 /** Sets the accel bias with control over which axis. 778 * @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame 779 * @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. 780 * @param[in] mask Mask to select axis to apply bias set. 781 */ 782 void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask) 783 { 784 if (bias) { 785 if (mask & 1){ 786 inv_data_builder.save_accel_mpl.accel_bias[0] = bias[0]; 787 } 788 if (mask & 2){ 789 inv_data_builder.save_accel_mpl.accel_bias[1] = bias[1]; 790 } 791 if (mask & 4){ 792 inv_data_builder.save_accel_mpl.accel_bias[2] = bias[2]; 793 } 794 795 inv_apply_calibration(&sensors.accel, inv_data_builder.save_accel_mpl.accel_bias); 796 } 797 inv_set_accel_accuracy(accuracy); 798 inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); 799 } 800 801 #ifdef WIN32 802 /** Sends out a message to activate writing 9-axis quaternion to the DMP. 803 */ 804 void inv_overwrite_dmp_9quat(void) 805 { 806 inv_set_message(INV_MSG_NEW_DMP_QUAT_WRITE_EVENT, INV_MSG_NEW_DMP_QUAT_WRITE_EVENT, 0); 807 } 808 #endif 809 810 /** 811 * Sets the factory gyro bias 812 * @param[in] bias 813 * Gyro bias in hardware units (+/- 2000 dps full scale assumed) 814 * scaled by 2^16. In chip mounting frame. Length of 3. 815 */ 816 void inv_set_gyro_bias(const long *bias) 817 { 818 if (!bias) 819 return; 820 821 if (memcmp(inv_data_builder.save.factory_gyro_bias, bias, 822 sizeof(inv_data_builder.save.factory_gyro_bias))) { 823 memcpy(inv_data_builder.save.factory_gyro_bias, bias, 824 sizeof(inv_data_builder.save.factory_gyro_bias)); 825 } 826 inv_set_message(INV_MSG_NEW_FGB_EVENT, INV_MSG_NEW_FGB_EVENT, 0); 827 } 828 829 /** 830 * Sets the mpl gyro bias 831 * @param[in] bias 832 * Gyro bias in hardware units scaled by 2^16 (+/- 2000 dps full 833 * scale assumed). In chip mounting frame. Length 3. 834 * @param[in] accuracy 835 * Accuracy of bias. 0 = least accurate, 3 = most accurate. 836 */ 837 void inv_set_mpl_gyro_bias(const long *bias, int accuracy) 838 { 839 if (bias != NULL) { 840 if (memcmp(inv_data_builder.save_mpl.gyro_bias, bias, 841 sizeof(inv_data_builder.save_mpl.gyro_bias))) { 842 memcpy(inv_data_builder.save_mpl.gyro_bias, bias, 843 sizeof(inv_data_builder.save_mpl.gyro_bias)); 844 inv_apply_calibration(&sensors.gyro, 845 inv_data_builder.save_mpl.gyro_bias); 846 } 847 } 848 sensors.gyro.accuracy = accuracy; 849 inv_data_builder.save.gyro_accuracy = accuracy; 850 851 /* TODO: What should we do if there's no temperature data? */ 852 if (sensors.temp.calibrated[0]) 853 inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0]; 854 else 855 /* Set to 27 deg C for now until we've got a better solution. */ 856 inv_data_builder.save.gyro_temp = 27L << 16; 857 inv_set_message(INV_MSG_NEW_GB_EVENT, INV_MSG_NEW_GB_EVENT, 0); 858 859 /* TODO: this flag works around the synchronization problem seen with using 860 the user-exposed message layer to signal the temperature compensation 861 module that gyro biases were set. 862 A better, cleaner method is certainly needed. */ 863 inv_data_builder.save.gyro_bias_tc_set = true; 864 } 865 866 /** 867 * @internal 868 * @brief Return whether gyro biases were set to signal the temperature 869 * compensation algorithm that it can collect a data point to build 870 * the temperature slope while in no motion state. 871 * The flag clear automatically after is read. 872 * @return true if the flag was set, indicating gyro biases were set. 873 * false if the flag was not set. 874 */ 875 int inv_get_gyro_bias_tc_set(void) 876 { 877 int flag = (inv_data_builder.save.gyro_bias_tc_set == true); 878 inv_data_builder.save.gyro_bias_tc_set = false; 879 return flag; 880 } 881 882 883 /** 884 * Get the mpl gyro biases 885 * @param[in] bias 886 * Gyro calibrated bias. 887 * Length 3. 888 */ 889 void inv_get_mpl_gyro_bias(long *bias, long *temp) 890 { 891 if (bias != NULL) 892 memcpy(bias, inv_data_builder.save_mpl.gyro_bias, 893 sizeof(inv_data_builder.save_mpl.gyro_bias)); 894 895 if (temp != NULL) 896 temp[0] = inv_data_builder.save.gyro_temp; 897 } 898 899 /** Gyro Bias in the form used by the DMP. 900 * @param[out] bias Gyro Bias in the form used by the DMP. It is scaled appropriately 901 * and is in the body frame as needed. If this bias is applied in the DMP 902 * then any quaternion must have the flag INV_BIAS_APPLIED set if it is a 903 * 3-axis quaternion, or INV_QUAT_6AXIS if it is a 6-axis quaternion 904 */ 905 void inv_get_gyro_bias_dmp_units(long *bias) 906 { 907 if (bias == NULL) 908 return; 909 inv_convert_to_body_with_scale(sensors.gyro.orientation, 46850825L, 910 inv_data_builder.save_mpl.gyro_bias, bias); 911 } 912 913 /* TODO: Add this information to inv_sensor_cal_t */ 914 /** 915 * Get the gyro biases and temperature record from MPL 916 * @param[in] bias 917 * Gyro bias in hardware units scaled by 2^16. 918 * In chip mounting frame. 919 * Length 3. 920 */ 921 void inv_get_gyro_bias(long *bias) 922 { 923 if (bias != NULL) 924 memcpy(bias, inv_data_builder.save.factory_gyro_bias, 925 sizeof(inv_data_builder.save.factory_gyro_bias)); 926 } 927 928 /** 929 * Get factory accel bias mask 930 * @param[in] bias 931 * Accel bias mask 932 * 1 is set, 0 is not set, Length 3 = x,y,z. 933 */ 934 int inv_get_factory_accel_bias_mask() 935 { 936 long bias[3]; 937 int bias_mask = 0; 938 inv_get_accel_bias(bias); 939 if (bias != NULL) { 940 int i; 941 for(i = 0; i < 3; i++) { 942 if(bias[i] != 0) { 943 bias_mask |= 1 << i; 944 } else { 945 bias_mask &= ~ (1 << i); 946 } 947 } 948 } 949 return bias_mask; 950 } 951 952 /** 953 * Get accel bias from MPL 954 * @param[in] bias 955 * Accel bias in hardware units scaled by 2^16. 956 * In chp mounting frame. 957 * Length 3. 958 */ 959 void inv_get_accel_bias(long *bias) 960 { 961 if (bias != NULL) 962 memcpy(bias, inv_data_builder.save.factory_accel_bias, 963 sizeof(inv_data_builder.save.factory_accel_bias)); 964 } 965 966 /** Get Accel Bias 967 * @param[out] bias Accel bias 968 * @param[out] temp Temperature where 1 C = 2^16 969 */ 970 void inv_get_mpl_accel_bias(long *bias, long *temp) 971 { 972 if (bias != NULL) 973 memcpy(bias, inv_data_builder.save_accel_mpl.accel_bias, 974 sizeof(inv_data_builder.save_accel_mpl.accel_bias)); 975 if (temp != NULL) 976 temp[0] = inv_data_builder.save.accel_temp; 977 } 978 979 /** Accel Bias in the form used by the DMP. 980 * @param[out] bias Accel Bias in the form used by the DMP. It is scaled appropriately 981 * and is in the body frame as needed. 982 */ 983 void inv_get_accel_bias_dmp_units(long *bias) 984 { 985 if (bias == NULL) 986 return; 987 inv_convert_to_body_with_scale(sensors.accel.orientation, 536870912L, 988 inv_data_builder.save_accel_mpl.accel_bias, bias); 989 } 990 991 /** 992 * Record new accel data for use when inv_execute_on_data() is called 993 * @param[in] accel 994 * accel data, length 3. 995 * Calibrated data is in m/s^2 scaled by 2^16 in body frame. 996 * Raw data is in device units in chip mounting frame. 997 * @param[in] status 998 * Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 999 * being most accurate. 1000 * The upper bit INV_CALIBRATED, is set if the data was calibrated 1001 * outside MPL and it is not set if the data being passed is raw. 1002 * Raw data should be in device units, typically in a 16-bit range. 1003 * @param[in] timestamp 1004 * Monotonic time stamp, for Android it's in nanoseconds. 1005 * @return Returns INV_SUCCESS if successful or an error code if not. 1006 */ 1007 inv_error_t inv_build_accel(const long *accel, int status, inv_time_t timestamp) 1008 { 1009 #ifdef INV_PLAYBACK_DBG 1010 if (inv_data_builder.debug_mode == RD_RECORD) { 1011 int type = PLAYBACK_DBG_TYPE_ACCEL; 1012 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 1013 fwrite(accel, sizeof(accel[0]), 3, inv_data_builder.file); 1014 fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); 1015 } 1016 #endif 1017 1018 if ((status & INV_CALIBRATED) == 0) { 1019 sensors.accel.raw[0] = (short)accel[0]; 1020 sensors.accel.raw[1] = (short)accel[1]; 1021 sensors.accel.raw[2] = (short)accel[2]; 1022 sensors.accel.status |= INV_RAW_DATA; 1023 inv_apply_calibration(&sensors.accel, inv_data_builder.save_accel_mpl.accel_bias); 1024 } else { 1025 sensors.accel.calibrated[0] = accel[0]; 1026 sensors.accel.calibrated[1] = accel[1]; 1027 sensors.accel.calibrated[2] = accel[2]; 1028 sensors.accel.status |= INV_CALIBRATED; 1029 sensors.accel.accuracy = status & 3; 1030 inv_data_builder.save.accel_accuracy = status & 3; 1031 } 1032 sensors.accel.status |= INV_NEW_DATA | INV_SENSOR_ON; 1033 sensors.accel.timestamp_prev = sensors.accel.timestamp; 1034 sensors.accel.timestamp = timestamp; 1035 1036 return INV_SUCCESS; 1037 } 1038 1039 /** Record new gyro data and calls inv_execute_on_data() if previous 1040 * sample has not been processed. 1041 * @param[in] gyro Data is in device units. Length 3. 1042 * @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds. 1043 * @param[out] executed Set to 1 if data processing was done. 1044 * @return Returns INV_SUCCESS if successful or an error code if not. 1045 */ 1046 inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp) 1047 { 1048 #ifdef INV_PLAYBACK_DBG 1049 if (inv_data_builder.debug_mode == RD_RECORD) { 1050 int type = PLAYBACK_DBG_TYPE_GYRO; 1051 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 1052 fwrite(gyro, sizeof(gyro[0]), 3, inv_data_builder.file); 1053 fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); 1054 } 1055 #endif 1056 1057 memcpy(sensors.gyro.raw, gyro, 3 * sizeof(short)); 1058 sensors.gyro.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; 1059 sensors.gyro.timestamp_prev = sensors.gyro.timestamp; 1060 sensors.gyro.timestamp = timestamp; 1061 inv_apply_calibration(&sensors.gyro, inv_data_builder.save_mpl.gyro_bias); 1062 1063 return INV_SUCCESS; 1064 } 1065 1066 /** Record new compass data for use when inv_execute_on_data() is called 1067 * @param[in] compass Compass data, if it was calibrated outside MPL, the units are uT scaled by 2^16. 1068 * Length 3. 1069 * @param[in] status Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 being most accurate. 1070 * The upper bit INV_CALIBRATED, is set if the data was calibrated outside MPL and it is 1071 * not set if the data being passed is raw. Raw data should be in device units, typically 1072 * in a 16-bit range. 1073 * @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds. 1074 * @param[out] executed Set to 1 if data processing was done. 1075 * @return Returns INV_SUCCESS if successful or an error code if not. 1076 */ 1077 inv_error_t inv_build_compass(const long *compass, int status, 1078 inv_time_t timestamp) 1079 { 1080 #ifdef INV_PLAYBACK_DBG 1081 if (inv_data_builder.debug_mode == RD_RECORD) { 1082 int type = PLAYBACK_DBG_TYPE_COMPASS; 1083 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 1084 fwrite(compass, sizeof(compass[0]), 3, inv_data_builder.file); 1085 fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); 1086 } 1087 #endif 1088 1089 if ((status & INV_CALIBRATED) == 0) { 1090 long data[3]; 1091 inv_set_compass_soft_iron_input_data(compass); 1092 inv_get_compass_soft_iron_output_data(data); 1093 sensors.compass.raw[0] = (short)data[0]; 1094 sensors.compass.raw[1] = (short)data[1]; 1095 sensors.compass.raw[2] = (short)data[2]; 1096 inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias); 1097 sensors.compass.status |= INV_RAW_DATA; 1098 } else { 1099 sensors.compass.calibrated[0] = compass[0]; 1100 sensors.compass.calibrated[1] = compass[1]; 1101 sensors.compass.calibrated[2] = compass[2]; 1102 sensors.compass.status |= INV_CALIBRATED; 1103 sensors.compass.accuracy = status & 3; 1104 inv_data_builder.save.compass_accuracy = status & 3; 1105 } 1106 sensors.compass.timestamp_prev = sensors.compass.timestamp; 1107 sensors.compass.timestamp = timestamp; 1108 sensors.compass.status |= INV_NEW_DATA | INV_SENSOR_ON; 1109 1110 return INV_SUCCESS; 1111 } 1112 1113 /** Record new temperature data for use when inv_execute_on_data() is called. 1114 * @param[in] temp Temperature data in q16 format. 1115 * @param[in] timestamp Monotonic time stamp; for Android it's in 1116 * nanoseconds. 1117 * @return Returns INV_SUCCESS if successful or an error code if not. 1118 */ 1119 inv_error_t inv_build_temp(const long temp, inv_time_t timestamp) 1120 { 1121 #ifdef INV_PLAYBACK_DBG 1122 if (inv_data_builder.debug_mode == RD_RECORD) { 1123 int type = PLAYBACK_DBG_TYPE_TEMPERATURE; 1124 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 1125 fwrite(&temp, sizeof(temp), 1, inv_data_builder.file); 1126 fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); 1127 } 1128 #endif 1129 sensors.temp.calibrated[0] = temp; 1130 sensors.temp.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; 1131 sensors.temp.timestamp_prev = sensors.temp.timestamp; 1132 sensors.temp.timestamp = timestamp; 1133 /* TODO: Apply scale, remove offset. */ 1134 1135 return INV_SUCCESS; 1136 } 1137 /** quaternion data 1138 * @param[in] quat Quaternion data. 2^30 = 1.0 or 2^14=1 for 16-bit data. 1139 * Real part first. Length 4. 1140 * @param[in] status number of axis, 16-bit or 32-bit 1141 * set INV_QUAT_3ELEMENT if input quaternion has only 3 elements (no scalar). 1142 * inv_compute_scalar_part() assumes 32-bit data. If using 16-bit quaternion, 1143 * shift 16 bits first before calling this function. 1144 * @param[in] timestamp 1145 * @param[in] timestamp Monotonic time stamp; for Android it's in 1146 * nanoseconds. 1147 * @param[out] executed Set to 1 if data processing was done. 1148 * @return Returns INV_SUCCESS if successful or an error code if not. 1149 */ 1150 inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp) 1151 { 1152 #ifdef INV_PLAYBACK_DBG 1153 if (inv_data_builder.debug_mode == RD_RECORD) { 1154 int type = PLAYBACK_DBG_TYPE_QUAT; 1155 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 1156 fwrite(quat, sizeof(quat[0]), 4, inv_data_builder.file); 1157 fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); 1158 } 1159 #endif 1160 1161 /* Android version DMP does not have scalar part */ 1162 if (status & INV_QUAT_3ELEMENT) { 1163 long resultQuat[4]; 1164 MPL_LOGV("3q: %ld,%ld,%ld\n", quat[0], quat[1], quat[2]); 1165 inv_compute_scalar_part(quat, resultQuat); 1166 MPL_LOGV("4q: %ld,%ld,%ld,%ld\n", resultQuat[0], resultQuat[1], resultQuat[2], resultQuat[3]); 1167 memcpy(sensors.quat.raw, resultQuat, sizeof(sensors.quat.raw)); 1168 } else { 1169 memcpy(sensors.quat.raw, quat, sizeof(sensors.quat.raw)); 1170 } 1171 sensors.quat.timestamp_prev = sensors.quat.timestamp; 1172 sensors.quat.timestamp = timestamp; 1173 sensors.quat.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; 1174 sensors.quat.status |= (INV_QUAT_3AXIS & status); 1175 sensors.quat.status |= (INV_QUAT_6AXIS & status); 1176 sensors.quat.status |= (INV_QUAT_9AXIS & status); 1177 sensors.quat.status |= (INV_BIAS_APPLIED & status); 1178 sensors.quat.status |= (INV_DMP_BIAS_APPLIED & status); 1179 sensors.quat.status |= (INV_QUAT_3ELEMENT & status); 1180 MPL_LOGV("quat.status: %d", sensors.quat.status); 1181 if (sensors.quat.status & (INV_QUAT_9AXIS | INV_QUAT_6AXIS)) { 1182 // Set quaternion 1183 inv_store_gaming_quaternion(quat, timestamp); 1184 } 1185 if (sensors.quat.status & INV_QUAT_9AXIS) { 1186 long identity[4] = {(1L<<30), 0, 0, 0}; 1187 inv_set_compass_correction(identity, timestamp); 1188 } 1189 return INV_SUCCESS; 1190 } 1191 1192 inv_error_t inv_build_pressure(const long pressure, int status, inv_time_t timestamp) 1193 { 1194 sensors.pressure.status |= INV_NEW_DATA; 1195 return INV_SUCCESS; 1196 } 1197 1198 /** This should be called when the accel has been turned off. This is so 1199 * that we will know if the data is contiguous. 1200 */ 1201 void inv_accel_was_turned_off() 1202 { 1203 #ifdef INV_PLAYBACK_DBG 1204 if (inv_data_builder.debug_mode == RD_RECORD) { 1205 int type = PLAYBACK_DBG_TYPE_COMPASS_OFF; 1206 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 1207 } 1208 #endif 1209 sensors.accel.status = 0; 1210 } 1211 1212 /** This should be called when the compass has been turned off. This is so 1213 * that we will know if the data is contiguous. 1214 */ 1215 void inv_compass_was_turned_off() 1216 { 1217 #ifdef INV_PLAYBACK_DBG 1218 if (inv_data_builder.debug_mode == RD_RECORD) { 1219 int type = PLAYBACK_DBG_TYPE_COMPASS_OFF; 1220 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 1221 } 1222 #endif 1223 sensors.compass.status = 0; 1224 } 1225 1226 /** This should be called when the quaternion data from the DMP has been turned off. This is so 1227 * that we will know if the data is contiguous. 1228 */ 1229 void inv_quaternion_sensor_was_turned_off(void) 1230 { 1231 #ifdef INV_PLAYBACK_DBG 1232 if (inv_data_builder.debug_mode == RD_RECORD) { 1233 int type = PLAYBACK_DBG_TYPE_QUAT_OFF; 1234 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 1235 } 1236 #endif 1237 sensors.quat.status = 0; 1238 } 1239 1240 /** This should be called when the gyro has been turned off. This is so 1241 * that we will know if the data is contiguous. 1242 */ 1243 void inv_gyro_was_turned_off() 1244 { 1245 #ifdef INV_PLAYBACK_DBG 1246 if (inv_data_builder.debug_mode == RD_RECORD) { 1247 int type = PLAYBACK_DBG_TYPE_GYRO_OFF; 1248 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 1249 } 1250 #endif 1251 sensors.gyro.status = 0; 1252 } 1253 1254 /** This should be called when the temperature sensor has been turned off. 1255 * This is so that we will know if the data is contiguous. 1256 */ 1257 void inv_temperature_was_turned_off() 1258 { 1259 sensors.temp.status = 0; 1260 } 1261 1262 /** Registers to receive a callback when there is new sensor data. 1263 * @internal 1264 * @param[in] func Function pointer to receive callback when there is new sensor data 1265 * @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority 1266 * numbers must be unique. 1267 * @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be 1268 * a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW = 1269 * gyro data, INV_MAG_NEW = compass data. So passing in 1270 * INV_ACCEL_NEW | INV_MAG_NEW, a 1271 * callback would be generated if there was new magnetomer data OR new accel data. 1272 */ 1273 inv_error_t inv_register_data_cb( 1274 inv_error_t (*func)(struct inv_sensor_cal_t *data), 1275 int priority, int sensor_type) 1276 { 1277 inv_error_t result = INV_SUCCESS; 1278 int kk, nn; 1279 1280 // Make sure we haven't registered this function already 1281 // Or used the same priority 1282 for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { 1283 if ((inv_data_builder.process[kk].func == func) || 1284 (inv_data_builder.process[kk].priority == priority)) { 1285 return INV_ERROR_INVALID_PARAMETER; //fixme give a warning 1286 } 1287 } 1288 1289 // Make sure we have not filled up our number of allowable callbacks 1290 if (inv_data_builder.num_cb <= INV_MAX_DATA_CB - 1) { 1291 kk = 0; 1292 if (inv_data_builder.num_cb != 0) { 1293 // set kk to be where this new callback goes in the array 1294 while ((kk < inv_data_builder.num_cb) && 1295 (inv_data_builder.process[kk].priority < priority)) { 1296 kk++; 1297 } 1298 if (kk != inv_data_builder.num_cb) { 1299 // We need to move the others 1300 for (nn = inv_data_builder.num_cb; nn > kk; --nn) { 1301 inv_data_builder.process[nn] = 1302 inv_data_builder.process[nn - 1]; 1303 } 1304 } 1305 } 1306 // Add new callback 1307 inv_data_builder.process[kk].func = func; 1308 inv_data_builder.process[kk].priority = priority; 1309 inv_data_builder.process[kk].data_required = sensor_type; 1310 inv_data_builder.num_cb++; 1311 } else { 1312 MPL_LOGE("Unable to add feature callback as too many were already registered\n"); 1313 result = INV_ERROR_MEMORY_EXAUSTED; 1314 } 1315 1316 return result; 1317 } 1318 1319 /** Unregisters the callback that happens when new sensor data is received. 1320 * @internal 1321 * @param[in] func Function pointer to receive callback when there is new sensor data 1322 * @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority 1323 * numbers must be unique. 1324 * @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be 1325 * a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW = 1326 * gyro data, INV_MAG_NEW = compass data. So passing in 1327 * INV_ACCEL_NEW | INV_MAG_NEW, a 1328 * callback would be generated if there was new magnetomer data OR new accel data. 1329 */ 1330 inv_error_t inv_unregister_data_cb( 1331 inv_error_t (*func)(struct inv_sensor_cal_t *data)) 1332 { 1333 int kk, nn; 1334 1335 for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { 1336 if (inv_data_builder.process[kk].func == func) { 1337 // Delete this callback 1338 for (nn = kk + 1; nn < inv_data_builder.num_cb; ++nn) { 1339 inv_data_builder.process[nn - 1] = 1340 inv_data_builder.process[nn]; 1341 } 1342 inv_data_builder.num_cb--; 1343 return INV_SUCCESS; 1344 } 1345 } 1346 1347 return INV_SUCCESS; // We did not find the callback 1348 } 1349 1350 /** After at least one of inv_build_gyro(), inv_build_accel(), or 1351 * inv_build_compass() has been called, this function should be called. 1352 * It will process the data it has received and update all the internal states 1353 * and features that have been turned on. 1354 * @return Returns INV_SUCCESS if successful or an error code if not. 1355 */ 1356 inv_error_t inv_execute_on_data(void) 1357 { 1358 inv_error_t result, first_error; 1359 int kk; 1360 1361 #ifdef INV_PLAYBACK_DBG 1362 if (inv_data_builder.debug_mode == RD_RECORD) { 1363 int type = PLAYBACK_DBG_TYPE_EXECUTE; 1364 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 1365 } 1366 #endif 1367 // Determine what new data we have 1368 inv_data_builder.mode = 0; 1369 if (sensors.gyro.status & INV_NEW_DATA) 1370 inv_data_builder.mode |= INV_GYRO_NEW; 1371 if (sensors.accel.status & INV_NEW_DATA) 1372 inv_data_builder.mode |= INV_ACCEL_NEW; 1373 if (sensors.compass.status & INV_NEW_DATA) 1374 inv_data_builder.mode |= INV_MAG_NEW; 1375 if (sensors.temp.status & INV_NEW_DATA) 1376 inv_data_builder.mode |= INV_TEMP_NEW; 1377 if (sensors.quat.status & INV_NEW_DATA) 1378 inv_data_builder.mode |= INV_QUAT_NEW; 1379 if (sensors.pressure.status & INV_NEW_DATA) 1380 inv_data_builder.mode |= INV_PRESSURE_NEW; 1381 1382 first_error = INV_SUCCESS; 1383 1384 for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { 1385 if (inv_data_builder.mode & inv_data_builder.process[kk].data_required) { 1386 result = inv_data_builder.process[kk].func(&sensors); 1387 if (result && !first_error) { 1388 first_error = result; 1389 } 1390 } 1391 } 1392 1393 inv_set_contiguous(); 1394 1395 return first_error; 1396 } 1397 1398 /** Cleans up status bits after running all the callbacks. It sets the contiguous flag. 1399 * 1400 */ 1401 static void inv_set_contiguous(void) 1402 { 1403 inv_time_t current_time = 0; 1404 if (sensors.gyro.status & INV_NEW_DATA) { 1405 sensors.gyro.status |= INV_CONTIGUOUS; 1406 current_time = sensors.gyro.timestamp; 1407 } 1408 if (sensors.accel.status & INV_NEW_DATA) { 1409 sensors.accel.status |= INV_CONTIGUOUS; 1410 current_time = MAX(current_time, sensors.accel.timestamp); 1411 } 1412 if (sensors.compass.status & INV_NEW_DATA) { 1413 sensors.compass.status |= INV_CONTIGUOUS; 1414 current_time = MAX(current_time, sensors.compass.timestamp); 1415 } 1416 if (sensors.temp.status & INV_NEW_DATA) { 1417 sensors.temp.status |= INV_CONTIGUOUS; 1418 current_time = MAX(current_time, sensors.temp.timestamp); 1419 } 1420 if (sensors.quat.status & INV_NEW_DATA) { 1421 sensors.quat.status |= INV_CONTIGUOUS; 1422 current_time = MAX(current_time, sensors.quat.timestamp); 1423 } 1424 1425 #if 0 1426 /* See if sensors are still on. These should be turned off by inv_*_was_turned_off() 1427 * type functions. This is just in case that breaks down. We make sure 1428 * all the data is within 2 seconds of the newest piece of data*/ 1429 if (inv_delta_time_ms(current_time, sensors.gyro.timestamp) >= 2000) 1430 inv_gyro_was_turned_off(); 1431 if (inv_delta_time_ms(current_time, sensors.accel.timestamp) >= 2000) 1432 inv_accel_was_turned_off(); 1433 if (inv_delta_time_ms(current_time, sensors.compass.timestamp) >= 2000) 1434 inv_compass_was_turned_off(); 1435 /* TODO: Temperature might not need to be read this quickly. */ 1436 if (inv_delta_time_ms(current_time, sensors.temp.timestamp) >= 2000) 1437 inv_temperature_was_turned_off(); 1438 #endif 1439 1440 /* clear bits */ 1441 sensors.gyro.status &= ~INV_NEW_DATA; 1442 sensors.accel.status &= ~INV_NEW_DATA; 1443 sensors.compass.status &= ~INV_NEW_DATA; 1444 sensors.temp.status &= ~INV_NEW_DATA; 1445 sensors.quat.status &= ~INV_NEW_DATA; 1446 sensors.pressure.status &= ~INV_NEW_DATA; 1447 } 1448 1449 /** Gets a whole set of accel data including data, accuracy and timestamp. 1450 * @param[out] data Accel Data where 1g = 2^16 1451 * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. 1452 * @param[out] timestamp The timestamp of the data sample. 1453 */ 1454 void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t *timestamp) 1455 { 1456 if (data != NULL) { 1457 memcpy(data, sensors.accel.calibrated, sizeof(sensors.accel.calibrated)); 1458 } 1459 if (timestamp != NULL) { 1460 *timestamp = sensors.accel.timestamp; 1461 } 1462 if (accuracy != NULL) { 1463 *accuracy = sensors.accel.accuracy; 1464 } 1465 } 1466 1467 /** Gets a whole set of gyro data including data, accuracy and timestamp. 1468 * @param[out] data Gyro Data where 1 dps = 2^16 1469 * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. 1470 * @param[out] timestamp The timestamp of the data sample. 1471 */ 1472 void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t *timestamp) 1473 { 1474 memcpy(data, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated)); 1475 if (timestamp != NULL) { 1476 *timestamp = sensors.gyro.timestamp; 1477 } 1478 if (accuracy != NULL) { 1479 *accuracy = sensors.gyro.accuracy; 1480 } 1481 } 1482 1483 /** Gets a whole set of gyro raw data including data, accuracy and timestamp. 1484 * @param[out] data Gyro Data where 1 dps = 2^16 1485 * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. 1486 * @param[out] timestamp The timestamp of the data sample. 1487 */ 1488 void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp) 1489 { 1490 memcpy(data, sensors.gyro.raw_scaled, sizeof(sensors.gyro.raw_scaled)); 1491 if (timestamp != NULL) { 1492 *timestamp = sensors.gyro.timestamp; 1493 } 1494 if (accuracy != NULL) { 1495 *accuracy = 0; 1496 } 1497 } 1498 1499 /** Get's latest gyro data. 1500 * @param[out] gyro Gyro Data, Length 3. 1 dps = 2^16. 1501 */ 1502 void inv_get_gyro(long *gyro) 1503 { 1504 memcpy(gyro, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated)); 1505 } 1506 1507 /** Gets a whole set of compass data including data, accuracy and timestamp. 1508 * @param[out] data Compass Data where 1 uT = 2^16 1509 * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. 1510 * @param[out] timestamp The timestamp of the data sample. 1511 */ 1512 void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t *timestamp) 1513 { 1514 memcpy(data, sensors.compass.calibrated, sizeof(sensors.compass.calibrated)); 1515 if (timestamp != NULL) { 1516 *timestamp = sensors.compass.timestamp; 1517 } 1518 if (accuracy != NULL) { 1519 if (inv_data_builder.compass_disturbance) 1520 *accuracy = 0; 1521 else 1522 *accuracy = sensors.compass.accuracy; 1523 } 1524 } 1525 1526 /** Gets a whole set of compass raw data including data, accuracy and timestamp. 1527 * @param[out] data Compass Data where 1 uT = 2^16 1528 * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. 1529 * @param[out] timestamp The timestamp of the data sample. 1530 */ 1531 void inv_get_compass_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp) 1532 { 1533 memcpy(data, sensors.compass.raw_scaled, sizeof(sensors.compass.raw_scaled)); 1534 if (timestamp != NULL) { 1535 *timestamp = sensors.compass.timestamp; 1536 } 1537 //per Michele, since data is raw, accuracy should = 0 1538 *accuracy = 0; 1539 } 1540 1541 /** Gets a whole set of temperature data including data, accuracy and timestamp. 1542 * @param[out] data Temperature data where 1 degree C = 2^16 1543 * @param[out] accuracy 0 to 3, where 3 is most accurate. 1544 * @param[out] timestamp The timestamp of the data sample. 1545 */ 1546 void inv_get_temp_set(long *data, int *accuracy, inv_time_t *timestamp) 1547 { 1548 data[0] = sensors.temp.calibrated[0]; 1549 if (timestamp) 1550 *timestamp = sensors.temp.timestamp; 1551 if (accuracy) 1552 *accuracy = sensors.temp.accuracy; 1553 } 1554 1555 /** Returns accuracy of gyro. 1556 * @return Accuracy of gyro with 0 being not accurate, and 3 being most accurate. 1557 */ 1558 int inv_get_gyro_accuracy(void) 1559 { 1560 return sensors.gyro.accuracy; 1561 } 1562 1563 /** Returns accuracy of compass. 1564 * @return Accuracy of compass with 0 being not accurate, and 3 being most accurate. 1565 */ 1566 int inv_get_mag_accuracy(void) 1567 { 1568 if (inv_data_builder.compass_disturbance) 1569 return 0; 1570 return sensors.compass.accuracy; 1571 } 1572 1573 /** Returns accuracy of accel. 1574 * @return Accuracy of accel with 0 being not accurate, and 3 being most accurate. 1575 */ 1576 int inv_get_accel_accuracy(void) 1577 { 1578 return sensors.accel.accuracy; 1579 } 1580 1581 inv_error_t inv_get_gyro_orient(int *orient) 1582 { 1583 *orient = sensors.gyro.orientation; 1584 return 0; 1585 } 1586 1587 inv_error_t inv_get_accel_orient(int *orient) 1588 { 1589 *orient = sensors.accel.orientation; 1590 return 0; 1591 } 1592 1593 /*======================================================================*/ 1594 /* compass soft iron module */ 1595 /*======================================================================*/ 1596 1597 /** Gets the 3x3 compass transform matrix in 32 bit Q30 fixed point format. 1598 * @param[out] the pointer of the 3x3 matrix in Q30 format 1599 */ 1600 void inv_get_compass_soft_iron_matrix_d(long *matrix) { 1601 int i; 1602 for (i=0; i<9; i++) { 1603 matrix[i] = sensors.soft_iron.matrix_d[i]; 1604 } 1605 } 1606 1607 /** Sets the 3x3 compass transform matrix in 32 bit Q30 fixed point format. 1608 * @param[in] the pointer of the 3x3 matrix in Q30 format 1609 */ 1610 void inv_set_compass_soft_iron_matrix_d(long *matrix) { 1611 int i; 1612 for (i=0; i<9; i++) { 1613 // set the floating point matrix 1614 sensors.soft_iron.matrix_d[i] = matrix[i]; 1615 // convert to Q30 format 1616 sensors.soft_iron.matrix_f[i] = inv_q30_to_float(matrix[i]); 1617 } 1618 } 1619 /** Gets the 3x3 compass transform matrix in 32 bit floating point format. 1620 * @param[out] the pointer of the 3x3 matrix in floating point format 1621 */ 1622 void inv_get_compass_soft_iron_matrix_f(float *matrix) { 1623 int i; 1624 for (i=0; i<9; i++) { 1625 matrix[i] = sensors.soft_iron.matrix_f[i]; 1626 } 1627 } 1628 /** Sets the 3x3 compass transform matrix in 32 bit floating point format. 1629 * @param[in] the pointer of the 3x3 matrix in floating point format 1630 */ 1631 void inv_set_compass_soft_iron_matrix_f(float *matrix) { 1632 int i; 1633 for (i=0; i<9; i++) { 1634 // set the floating point matrix 1635 sensors.soft_iron.matrix_f[i] = matrix[i]; 1636 // convert to Q30 format 1637 sensors.soft_iron.matrix_d[i] = (long )(matrix[i]*ROT_MATRIX_SCALE_LONG); 1638 } 1639 } 1640 1641 /** This subroutine gets the fixed point Q30 compass data after the soft iron transformation. 1642 * @param[out] the pointer of the 3x1 vector compass data in MPL format 1643 */ 1644 void inv_get_compass_soft_iron_output_data(long *data) { 1645 int i; 1646 for (i=0; i<3; i++) { 1647 data[i] = sensors.soft_iron.trans[i]; 1648 } 1649 } 1650 /** This subroutine gets the fixed point Q30 compass data before the soft iron transformation. 1651 * @param[out] the pointer of the 3x1 vector compass data in MPL format 1652 */ 1653 void inv_get_compass_soft_iron_input_data(long *data) { 1654 int i; 1655 for (i=0; i<3; i++) { 1656 data[i] = sensors.soft_iron.raw[i]; 1657 } 1658 } 1659 /** This subroutine sets the compass raw data for the soft iron transformation. 1660 * @param[int] the pointer of the 3x1 vector compass raw data in MPL format 1661 */ 1662 void inv_set_compass_soft_iron_input_data(const long *data) { 1663 int i; 1664 for (i=0; i<3; i++) { 1665 sensors.soft_iron.raw[i] = data[i]; 1666 } 1667 if (sensors.soft_iron.enable == 1) { 1668 mlMatrixVectorMult(sensors.soft_iron.matrix_d, data, sensors.soft_iron.trans); 1669 } else { 1670 for (i=0; i<3; i++) { 1671 sensors.soft_iron.trans[i] = data[i]; 1672 } 1673 } 1674 } 1675 1676 /** This subroutine resets the the soft iron transformation to unity matrix and 1677 * disable the soft iron transformation process by default. 1678 */ 1679 void inv_reset_compass_soft_iron_matrix(void) { 1680 int i; 1681 for (i=0; i<9; i++) { 1682 sensors.soft_iron.matrix_f[i] = 0.0f; 1683 } 1684 1685 memset(&sensors.soft_iron.matrix_d,0,sizeof(sensors.soft_iron.matrix_d)); 1686 1687 for (i=0; i<3; i++) { 1688 // set the floating point matrix 1689 sensors.soft_iron.matrix_f[i*4] = 1.0; 1690 // set the fixed point matrix 1691 sensors.soft_iron.matrix_d[i*4] = ROT_MATRIX_SCALE_LONG; 1692 } 1693 1694 inv_disable_compass_soft_iron_matrix(); 1695 } 1696 1697 /** This subroutine enables the the soft iron transformation process. 1698 */ 1699 void inv_enable_compass_soft_iron_matrix(void) { 1700 sensors.soft_iron.enable = 1; 1701 } 1702 1703 /** This subroutine disables the the soft iron transformation process. 1704 */ 1705 void inv_disable_compass_soft_iron_matrix(void) { 1706 sensors.soft_iron.enable = 0; 1707 } 1708 1709 /** 1710 * @} 1711 */ 1712
