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