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 34typedef inv_error_t (*inv_process_cb_func)(struct inv_sensor_cal_t *data); 35 36struct process_t { 37 inv_process_cb_func func; 38 int priority; 39 int data_required; 40}; 41 42struct 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 57void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias); 58static void inv_set_contiguous(void); 59 60static struct inv_data_builder_t inv_data_builder; 61static 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*/ 68void 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*/ 78void 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*/ 89void 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 */ 95static 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 */ 113static 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 */ 120static 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 */ 128static 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 */ 135static 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 */ 143static 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*/ 151inv_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*/ 176long 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*/ 186long 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*/ 196long 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*/ 207void 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*/ 245void 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*/ 262void 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*/ 281void 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*/ 300static 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*/ 332static 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*/ 373int 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*/ 438int 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*/ 464int 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*/ 502void 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 518void inv_get_gyro_sample_rate_ms(long *sample_rate_ms) 519{ 520 *sample_rate_ms = sensors.gyro.sample_rate_ms; 521} 522 523void inv_get_accel_sample_rate_ms(long *sample_rate_ms) 524{ 525 *sample_rate_ms = sensors.accel.sample_rate_ms; 526} 527 528void 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*/ 536void 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*/ 552void 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*/ 560void 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*/ 568void 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*/ 576int 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*/ 584int 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*/ 592int 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*/ 601inv_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*/ 642void 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*/ 665void 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 678void 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*/ 691void 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*/ 715void 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*/ 726void 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*/ 740void inv_set_compass_disturbance(int dist) 741{ 742 inv_data_builder.compass_disturbance = dist; 743} 744 745int 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 */ 755void 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*/ 771void 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*/ 782void 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*/ 804void 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 */ 816void 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 */ 837void 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 */ 875int 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 */ 889void 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*/ 905void 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 */ 921void 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 */ 934int 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 */ 959void 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*/ 970void 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*/ 983void 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 */ 1007inv_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*/ 1046inv_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*/ 1077inv_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 */ 1119inv_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*/ 1150inv_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 1192inv_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*/ 1201void 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*/ 1215void 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*/ 1229void 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*/ 1243void 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 */ 1257void 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*/ 1273inv_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*/ 1330inv_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*/ 1356inv_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*/ 1401static 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*/ 1454void 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*/ 1472void 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*/ 1488void 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*/ 1502void 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*/ 1512void 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*/ 1531void 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 */ 1546void 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*/ 1558int 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*/ 1566int 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*/ 1576int inv_get_accel_accuracy(void) 1577{ 1578 return sensors.accel.accuracy; 1579} 1580 1581inv_error_t inv_get_gyro_orient(int *orient) 1582{ 1583 *orient = sensors.gyro.orientation; 1584 return 0; 1585} 1586 1587inv_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*/ 1600void 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*/ 1610void 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*/ 1622void 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*/ 1631void 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*/ 1644void 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*/ 1653void 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*/ 1662void 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*/ 1679void 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*/ 1699void 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*/ 1705void inv_disable_compass_soft_iron_matrix(void) { 1706 sensors.soft_iron.enable = 0; 1707} 1708 1709/** 1710 * @} 1711 */ 1712