data_builder.c revision 6a4d9a48ffde124c498496f6dae1e77c2cae6864
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 0 /* 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 "MPL" 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_db_save_t { 43 /** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */ 44 long compass_bias[3]; 45 /** Gyro Bias in Chip Frame in Hardware units scaled by 2^16 */ 46 long gyro_bias[3]; 47 /** Temperature when *gyro_bias was stored. */ 48 long gyro_temp; 49 /** Accel Bias in Chip Frame in Hardware units scaled by 2^16 */ 50 long accel_bias[3]; 51 /** Temperature when accel bias was stored. */ 52 long accel_temp; 53 long gyro_temp_slope[3]; 54 /** Sensor Accuracy */ 55 int gyro_accuracy; 56 int accel_accuracy; 57 int compass_accuracy; 58}; 59 60struct inv_data_builder_t { 61 int num_cb; 62 struct process_t process[INV_MAX_DATA_CB]; 63 struct inv_db_save_t save; 64 int compass_disturbance; 65#ifdef INV_PLAYBACK_DBG 66 int debug_mode; 67 int last_mode; 68 FILE *file; 69#endif 70}; 71 72void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias); 73static void inv_set_contiguous(void); 74 75static struct inv_data_builder_t inv_data_builder; 76static struct inv_sensor_cal_t sensors; 77 78/** Change this key if the data being stored by this file changes */ 79#define INV_DB_SAVE_KEY 53395 80 81#ifdef INV_PLAYBACK_DBG 82 83/** Turn on data logging to allow playback of same scenario at a later time. 84* @param[in] file File to write to, must be open. 85*/ 86void inv_turn_on_data_logging(FILE *file) 87{ 88 MPL_LOGV("input data logging started\n"); 89 inv_data_builder.file = file; 90 inv_data_builder.debug_mode = RD_RECORD; 91} 92 93/** Turn off data logging to allow playback of same scenario at a later time. 94* File passed to inv_turn_on_data_logging() must be closed after calling this. 95*/ 96void inv_turn_off_data_logging() 97{ 98 MPL_LOGV("input data logging stopped\n"); 99 inv_data_builder.debug_mode = RD_NO_DEBUG; 100 inv_data_builder.file = NULL; 101} 102#endif 103 104/** This function receives the data that was stored in non-volatile memory between power off */ 105static inv_error_t inv_db_load_func(const unsigned char *data) 106{ 107 memcpy(&inv_data_builder.save, data, sizeof(inv_data_builder.save)); 108 // copy in the saved accuracy in the actual sensors accuracy 109 sensors.gyro.accuracy = inv_data_builder.save.gyro_accuracy; 110 sensors.accel.accuracy = inv_data_builder.save.accel_accuracy; 111 sensors.compass.accuracy = inv_data_builder.save.compass_accuracy; 112 // TODO 113 if (sensors.compass.accuracy == 3) { 114 inv_set_compass_bias_found(1); 115 } 116 return INV_SUCCESS; 117} 118 119/** This function returns the data to be stored in non-volatile memory between power off */ 120static inv_error_t inv_db_save_func(unsigned char *data) 121{ 122 memcpy(data, &inv_data_builder.save, sizeof(inv_data_builder.save)); 123 return INV_SUCCESS; 124} 125 126/** Initialize the data builder 127*/ 128inv_error_t inv_init_data_builder(void) 129{ 130 /* TODO: Hardcode temperature scale/offset here. */ 131 memset(&inv_data_builder, 0, sizeof(inv_data_builder)); 132 memset(&sensors, 0, sizeof(sensors)); 133 return inv_register_load_store(inv_db_load_func, inv_db_save_func, 134 sizeof(inv_data_builder.save), 135 INV_DB_SAVE_KEY); 136} 137 138/** Gyro sensitivity. 139* @return A scale factor to convert device units to degrees per second scaled by 2^16 140* such that degrees_per_second = device_units * sensitivity / 2^30. Typically 141* it works out to be the maximum rate * 2^15. 142*/ 143long inv_get_gyro_sensitivity() 144{ 145 return sensors.gyro.sensitivity; 146} 147 148/** Accel sensitivity. 149* @return A scale factor to convert device units to g's scaled by 2^16 150* such that g_s = device_units * sensitivity / 2^30. Typically 151* it works out to be the maximum accel value in g's * 2^15. 152*/ 153long inv_get_accel_sensitivity(void) 154{ 155 return sensors.accel.sensitivity; 156} 157 158/** Compass sensitivity. 159* @return A scale factor to convert device units to micro Tesla scaled by 2^16 160* such that uT = device_units * sensitivity / 2^30. Typically 161* it works out to be the maximum uT * 2^15. 162*/ 163long inv_get_compass_sensitivity(void) 164{ 165 return sensors.compass.sensitivity; 166} 167 168/** Sets orientation and sensitivity field for a sensor. 169* @param[out] sensor Structure to apply settings to 170* @param[in] orientation Orientation description of how part is mounted. 171* @param[in] sensitivity A Scale factor to convert from hardware units to 172* standard units (dps, uT, g). 173*/ 174void set_sensor_orientation_and_scale(struct inv_single_sensor_t *sensor, 175 int orientation, long sensitivity) 176{ 177 sensor->sensitivity = sensitivity; 178 sensor->orientation = orientation; 179} 180 181/** Sets the Orientation and Sensitivity of the gyro data. 182* @param[in] orientation A scalar defining the transformation from chip mounting 183* to the body frame. The function inv_orientation_matrix_to_scalar() 184* can convert the transformation matrix to this scalar and describes the 185* scalar in further detail. 186* @param[in] sensitivity A scale factor to convert device units to degrees per second scaled by 2^16 187* such that degrees_per_second = device_units * sensitivity / 2^30. Typically 188* it works out to be the maximum rate * 2^15. 189*/ 190void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity) 191{ 192#ifdef INV_PLAYBACK_DBG 193 if (inv_data_builder.debug_mode == RD_RECORD) { 194 int type = PLAYBACK_DBG_TYPE_G_ORIENT; 195 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 196 fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); 197 fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); 198 } 199#endif 200 set_sensor_orientation_and_scale(&sensors.gyro, orientation, 201 sensitivity); 202} 203 204/** Set Gyro Sample rate in micro seconds. 205* @param[in] sample_rate_us Set Gyro Sample rate in us 206*/ 207void inv_set_gyro_sample_rate(long sample_rate_us) 208{ 209#ifdef INV_PLAYBACK_DBG 210 if (inv_data_builder.debug_mode == RD_RECORD) { 211 int type = PLAYBACK_DBG_TYPE_G_SAMPLE_RATE; 212 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 213 fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); 214 } 215#endif 216 sensors.gyro.sample_rate_us = sample_rate_us; 217 sensors.gyro.sample_rate_ms = sample_rate_us / 1000; 218 if (sensors.gyro.bandwidth == 0) { 219 sensors.gyro.bandwidth = (int)(1000000L / sample_rate_us); 220 } 221} 222 223/** Set Accel Sample rate in micro seconds. 224* @param[in] sample_rate_us Set Accel Sample rate in us 225*/ 226void inv_set_accel_sample_rate(long sample_rate_us) 227{ 228#ifdef INV_PLAYBACK_DBG 229 if (inv_data_builder.debug_mode == RD_RECORD) { 230 int type = PLAYBACK_DBG_TYPE_A_SAMPLE_RATE; 231 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 232 fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); 233 } 234#endif 235 sensors.accel.sample_rate_us = sample_rate_us; 236 sensors.accel.sample_rate_ms = sample_rate_us / 1000; 237 if (sensors.accel.bandwidth == 0) { 238 sensors.accel.bandwidth = (int)(1000000L / sample_rate_us); 239 } 240} 241 242/** Set Compass Sample rate in micro seconds. 243* @param[in] sample_rate_us Set Gyro Sample rate in micro seconds. 244*/ 245void inv_set_compass_sample_rate(long sample_rate_us) 246{ 247#ifdef INV_PLAYBACK_DBG 248 if (inv_data_builder.debug_mode == RD_RECORD) { 249 int type = PLAYBACK_DBG_TYPE_C_SAMPLE_RATE; 250 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 251 fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); 252 } 253#endif 254 sensors.compass.sample_rate_us = sample_rate_us; 255 sensors.compass.sample_rate_ms = sample_rate_us / 1000; 256 if (sensors.compass.bandwidth == 0) { 257 sensors.compass.bandwidth = (int)(1000000L / sample_rate_us); 258 } 259} 260 261void inv_get_gyro_sample_rate_ms(long *sample_rate_ms) 262{ 263 *sample_rate_ms = sensors.gyro.sample_rate_ms; 264} 265 266void inv_get_accel_sample_rate_ms(long *sample_rate_ms) 267{ 268 *sample_rate_ms = sensors.accel.sample_rate_ms; 269} 270 271void inv_get_compass_sample_rate_ms(long *sample_rate_ms) 272{ 273 *sample_rate_ms = sensors.compass.sample_rate_ms; 274} 275 276/** Set Quat Sample rate in micro seconds. 277* @param[in] sample_rate_us Set Quat Sample rate in us 278*/ 279void inv_set_quat_sample_rate(long sample_rate_us) 280{ 281#ifdef INV_PLAYBACK_DBG 282 if (inv_data_builder.debug_mode == RD_RECORD) { 283 int type = PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE; 284 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 285 fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); 286 } 287#endif 288 sensors.quat.sample_rate_us = sample_rate_us; 289 sensors.quat.sample_rate_ms = sample_rate_us / 1000; 290} 291 292/** Set Gyro Bandwidth in Hz 293* @param[in] bandwidth_hz Gyro bandwidth in Hz 294*/ 295void inv_set_gyro_bandwidth(int bandwidth_hz) 296{ 297 sensors.gyro.bandwidth = bandwidth_hz; 298} 299 300/** Set Accel Bandwidth in Hz 301* @param[in] bandwidth_hz Gyro bandwidth in Hz 302*/ 303void inv_set_accel_bandwidth(int bandwidth_hz) 304{ 305 sensors.accel.bandwidth = bandwidth_hz; 306} 307 308/** Set Compass Bandwidth in Hz 309* @param[in] bandwidth_hz Gyro bandwidth in Hz 310*/ 311void inv_set_compass_bandwidth(int bandwidth_hz) 312{ 313 sensors.compass.bandwidth = bandwidth_hz; 314} 315 316/** Helper function stating whether the compass is on or off. 317 * @return TRUE if compass if on, 0 if compass if off 318*/ 319int inv_get_compass_on() 320{ 321 return (sensors.compass.status & INV_SENSOR_ON) == INV_SENSOR_ON; 322} 323 324/** Helper function stating whether the gyro is on or off. 325 * @return TRUE if gyro if on, 0 if gyro if off 326*/ 327int inv_get_gyro_on() 328{ 329 return (sensors.gyro.status & INV_SENSOR_ON) == INV_SENSOR_ON; 330} 331 332/** Helper function stating whether the acceleromter is on or off. 333 * @return TRUE if accel if on, 0 if accel if off 334*/ 335int inv_get_accel_on() 336{ 337 return (sensors.accel.status & INV_SENSOR_ON) == INV_SENSOR_ON; 338} 339 340/** Get last timestamp across all 3 sensors that are on. 341* This find out which timestamp has the largest value for sensors that are on. 342* @return Returns INV_SUCCESS if successful or an error code if not. 343*/ 344inv_time_t inv_get_last_timestamp() 345{ 346 inv_time_t timestamp = 0; 347 if (sensors.accel.status & INV_SENSOR_ON) { 348 timestamp = sensors.accel.timestamp; 349 } 350 if (sensors.gyro.status & INV_SENSOR_ON) { 351 if (timestamp < sensors.gyro.timestamp) { 352 timestamp = sensors.gyro.timestamp; 353 } 354 } 355 if (sensors.compass.status & INV_SENSOR_ON) { 356 if (timestamp < sensors.compass.timestamp) { 357 timestamp = sensors.compass.timestamp; 358 } 359 } 360 if (sensors.temp.status & INV_SENSOR_ON) { 361 if (timestamp < sensors.temp.timestamp) 362 timestamp = sensors.temp.timestamp; 363 } 364 return timestamp; 365} 366 367/** Sets the orientation and sensitivity of the gyro data. 368* @param[in] orientation A scalar defining the transformation from chip mounting 369* to the body frame. The function inv_orientation_matrix_to_scalar() 370* can convert the transformation matrix to this scalar and describes the 371* scalar in further detail. 372* @param[in] sensitivity A scale factor to convert device units to g's 373* such that g's = device_units * sensitivity / 2^30. Typically 374* it works out to be the maximum g_value * 2^15. 375*/ 376void inv_set_accel_orientation_and_scale(int orientation, long sensitivity) 377{ 378#ifdef INV_PLAYBACK_DBG 379 if (inv_data_builder.debug_mode == RD_RECORD) { 380 int type = PLAYBACK_DBG_TYPE_A_ORIENT; 381 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 382 fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); 383 fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); 384 } 385#endif 386 set_sensor_orientation_and_scale(&sensors.accel, orientation, 387 sensitivity); 388} 389 390/** Sets the Orientation and Sensitivity of the gyro data. 391* @param[in] orientation A scalar defining the transformation from chip mounting 392* to the body frame. The function inv_orientation_matrix_to_scalar() 393* can convert the transformation matrix to this scalar and describes the 394* scalar in further detail. 395* @param[in] sensitivity A scale factor to convert device units to uT 396* such that uT = device_units * sensitivity / 2^30. Typically 397* it works out to be the maximum uT_value * 2^15. 398*/ 399void inv_set_compass_orientation_and_scale(int orientation, long sensitivity) 400{ 401#ifdef INV_PLAYBACK_DBG 402 if (inv_data_builder.debug_mode == RD_RECORD) { 403 int type = PLAYBACK_DBG_TYPE_C_ORIENT; 404 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 405 fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); 406 fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); 407 } 408#endif 409 set_sensor_orientation_and_scale(&sensors.compass, orientation, sensitivity); 410} 411 412void inv_matrix_vector_mult(const long *A, const long *x, long *y) 413{ 414 y[0] = inv_q30_mult(A[0], x[0]) + inv_q30_mult(A[1], x[1]) + inv_q30_mult(A[2], x[2]); 415 y[1] = inv_q30_mult(A[3], x[0]) + inv_q30_mult(A[4], x[1]) + inv_q30_mult(A[5], x[2]); 416 y[2] = inv_q30_mult(A[6], x[0]) + inv_q30_mult(A[7], x[1]) + inv_q30_mult(A[8], x[2]); 417} 418 419/** Takes raw data stored in the sensor, removes bias, and converts it to 420* calibrated data in the body frame. Also store raw data for body frame. 421* @param[in,out] sensor structure to modify 422* @param[in] bias bias in the mounting frame, in hardware units scaled by 423* 2^16. Length 3. 424*/ 425void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias) 426{ 427 long raw32[3]; 428 429 // Convert raw to calibrated 430 raw32[0] = (long)sensor->raw[0] << 15; 431 raw32[1] = (long)sensor->raw[1] << 15; 432 raw32[2] = (long)sensor->raw[2] << 15; 433 434 inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_scaled); 435 436 raw32[0] -= bias[0] >> 1; 437 raw32[1] -= bias[1] >> 1; 438 raw32[2] -= bias[2] >> 1; 439 440 inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->calibrated); 441 442 sensor->status |= INV_CALIBRATED; 443} 444 445/** Returns the current bias for the compass 446* @param[out] bias Compass bias in hardware units scaled by 2^16. In mounting frame. 447* Length 3. 448*/ 449void inv_get_compass_bias(long *bias) 450{ 451 if (bias != NULL) { 452 memcpy(bias, inv_data_builder.save.compass_bias, sizeof(inv_data_builder.save.compass_bias)); 453 } 454} 455 456void inv_set_compass_bias(const long *bias, int accuracy) 457{ 458 if (memcmp(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias))) { 459 memcpy(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias)); 460 inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias); 461 } 462 sensors.compass.accuracy = accuracy; 463 inv_data_builder.save.compass_accuracy = accuracy; 464 inv_set_message(INV_MSG_NEW_CB_EVENT, INV_MSG_NEW_CB_EVENT, 0); 465} 466 467/** Set the state of a compass disturbance 468* @param[in] dist 1=disturbance, 0=no disturbance 469*/ 470void inv_set_compass_disturbance(int dist) 471{ 472 inv_data_builder.compass_disturbance = dist; 473} 474 475int inv_get_compass_disturbance(void) { 476 return inv_data_builder.compass_disturbance; 477} 478/** Sets the accel bias. 479* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame 480* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. 481*/ 482void inv_set_accel_bias(const long *bias, int accuracy) 483{ 484 if (bias) { 485 if (memcmp(inv_data_builder.save.accel_bias, bias, sizeof(inv_data_builder.save.accel_bias))) { 486 memcpy(inv_data_builder.save.accel_bias, bias, sizeof(inv_data_builder.save.accel_bias)); 487 inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); 488 } 489 } 490 sensors.accel.accuracy = accuracy; 491 inv_data_builder.save.accel_accuracy = accuracy; 492 inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); 493} 494 495/** Sets the accel accuracy. 496* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. 497*/ 498void inv_set_accel_accuracy(int accuracy) 499{ 500 sensors.accel.accuracy = accuracy; 501 inv_data_builder.save.accel_accuracy = accuracy; 502 inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); 503} 504 505/** Sets the accel bias with control over which axis. 506* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame 507* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. 508* @param[in] mask Mask to select axis to apply bias set. 509*/ 510void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask) 511{ 512 if (bias) { 513 if (mask & 1){ 514 inv_data_builder.save.accel_bias[0] = bias[0]; 515 } 516 if (mask & 2){ 517 inv_data_builder.save.accel_bias[1] = bias[1]; 518 } 519 if (mask & 4){ 520 inv_data_builder.save.accel_bias[2] = bias[2]; 521 } 522 523 inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); 524 } 525 sensors.accel.accuracy = accuracy; 526 inv_data_builder.save.accel_accuracy = accuracy; 527 inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); 528} 529 530 531/** Sets the gyro bias 532* @param[in] bias Gyro bias in hardware units scaled by 2^16. In chip mounting frame. 533* Length 3. 534* @param[in] accuracy Accuracy of bias. 0 = least accurate, 3 = most accurate. 535*/ 536void inv_set_gyro_bias(const long *bias, int accuracy) 537{ 538 if (bias != NULL) { 539 if (memcmp(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias))) { 540 memcpy(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias)); 541 inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias); 542 } 543 } 544 sensors.gyro.accuracy = accuracy; 545 inv_data_builder.save.gyro_accuracy = accuracy; 546 547 /* TODO: What should we do if there's no temperature data? */ 548 if (sensors.temp.calibrated[0]) 549 inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0]; 550 else 551 /* Set to 27 deg C for now until we've got a better solution. */ 552 inv_data_builder.save.gyro_temp = 1769472L; 553 inv_set_message(INV_MSG_NEW_GB_EVENT, INV_MSG_NEW_GB_EVENT, 0); 554} 555 556/* TODO: Add this information to inv_sensor_cal_t */ 557/** 558 * Get the gyro biases and temperature record from MPL 559 * @param[in] bias 560 * Gyro bias in hardware units scaled by 2^16. 561 * In chip mounting frame. 562 * Length 3. 563 * @param[in] temp 564 * Tempearature in degrees C. 565 */ 566void inv_get_gyro_bias(long *bias, long *temp) 567{ 568 if (bias != NULL) 569 memcpy(bias, inv_data_builder.save.gyro_bias, 570 sizeof(inv_data_builder.save.gyro_bias)); 571 if (temp != NULL) 572 temp[0] = inv_data_builder.save.gyro_temp; 573} 574 575/** Get Accel Bias 576* @param[out] bias Accel bias where 577* @param[out] temp Temperature where 1 C = 2^16 578*/ 579void inv_get_accel_bias(long *bias, long *temp) 580{ 581 if (bias != NULL) 582 memcpy(bias, inv_data_builder.save.accel_bias, 583 sizeof(inv_data_builder.save.accel_bias)); 584 if (temp != NULL) 585 temp[0] = inv_data_builder.save.accel_temp; 586} 587 588/** 589 * Record new accel data for use when inv_execute_on_data() is called 590 * @param[in] accel accel data. 591 * Length 3. 592 * Calibrated data is in m/s^2 scaled by 2^16 in body frame. 593 * Raw data is in device units in chip mounting frame. 594 * @param[in] status 595 * Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 596 * being most accurate. 597 * The upper bit INV_CALIBRATED, is set if the data was calibrated 598 * outside MPL and it is not set if the data being passed is raw. 599 * Raw data should be in device units, typically in a 16-bit range. 600 * @param[in] timestamp 601 * Monotonic time stamp, for Android it's in nanoseconds. 602 * @return Returns INV_SUCCESS if successful or an error code if not. 603 */ 604inv_error_t inv_build_accel(const long *accel, int status, inv_time_t timestamp) 605{ 606#ifdef INV_PLAYBACK_DBG 607 if (inv_data_builder.debug_mode == RD_RECORD) { 608 int type = PLAYBACK_DBG_TYPE_ACCEL; 609 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 610 fwrite(accel, sizeof(accel[0]), 3, inv_data_builder.file); 611 fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); 612 } 613#endif 614 615 if ((status & INV_CALIBRATED) == 0) { 616 sensors.accel.raw[0] = (short)accel[0]; 617 sensors.accel.raw[1] = (short)accel[1]; 618 sensors.accel.raw[2] = (short)accel[2]; 619 sensors.accel.status |= INV_RAW_DATA; 620 inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); 621 } else { 622 sensors.accel.calibrated[0] = accel[0]; 623 sensors.accel.calibrated[1] = accel[1]; 624 sensors.accel.calibrated[2] = accel[2]; 625 sensors.accel.status |= INV_CALIBRATED; 626 sensors.accel.accuracy = status & 3; 627 inv_data_builder.save.accel_accuracy = status & 3; 628 } 629 sensors.accel.status |= INV_NEW_DATA | INV_SENSOR_ON; 630 sensors.accel.timestamp_prev = sensors.accel.timestamp; 631 sensors.accel.timestamp = timestamp; 632 633 return INV_SUCCESS; 634} 635 636/** Record new gyro data and calls inv_execute_on_data() if previous 637* sample has not been processed. 638* @param[in] gyro Data is in device units. Length 3. 639* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds. 640* @param[out] executed Set to 1 if data processing was done. 641* @return Returns INV_SUCCESS if successful or an error code if not. 642*/ 643inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp) 644{ 645#ifdef INV_PLAYBACK_DBG 646 if (inv_data_builder.debug_mode == RD_RECORD) { 647 int type = PLAYBACK_DBG_TYPE_GYRO; 648 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 649 fwrite(gyro, sizeof(gyro[0]), 3, inv_data_builder.file); 650 fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); 651 } 652#endif 653 654 memcpy(sensors.gyro.raw, gyro, 3 * sizeof(short)); 655 sensors.gyro.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; 656 sensors.gyro.timestamp_prev = sensors.gyro.timestamp; 657 sensors.gyro.timestamp = timestamp; 658 inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias); 659 660 return INV_SUCCESS; 661} 662 663/** Record new compass data for use when inv_execute_on_data() is called 664* @param[in] compass Compass data, if it was calibrated outside MPL, the units are uT scaled by 2^16. 665* Length 3. 666* @param[in] status Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 being most accurate. 667* The upper bit INV_CALIBRATED, is set if the data was calibrated outside MPL and it is 668* not set if the data being passed is raw. Raw data should be in device units, typically 669* in a 16-bit range. 670* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds. 671* @param[out] executed Set to 1 if data processing was done. 672* @return Returns INV_SUCCESS if successful or an error code if not. 673*/ 674inv_error_t inv_build_compass(const long *compass, int status, 675 inv_time_t timestamp) 676{ 677#ifdef INV_PLAYBACK_DBG 678 if (inv_data_builder.debug_mode == RD_RECORD) { 679 int type = PLAYBACK_DBG_TYPE_COMPASS; 680 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 681 fwrite(compass, sizeof(compass[0]), 3, inv_data_builder.file); 682 fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); 683 } 684#endif 685 686 if ((status & INV_CALIBRATED) == 0) { 687 sensors.compass.raw[0] = (short)compass[0]; 688 sensors.compass.raw[1] = (short)compass[1]; 689 sensors.compass.raw[2] = (short)compass[2]; 690 inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias); 691 sensors.compass.status |= INV_RAW_DATA; 692 } else { 693 sensors.compass.calibrated[0] = compass[0]; 694 sensors.compass.calibrated[1] = compass[1]; 695 sensors.compass.calibrated[2] = compass[2]; 696 sensors.compass.status |= INV_CALIBRATED; 697 sensors.compass.accuracy = status & 3; 698 inv_data_builder.save.compass_accuracy = status & 3; 699 } 700 sensors.compass.timestamp_prev = sensors.compass.timestamp; 701 sensors.compass.timestamp = timestamp; 702 sensors.compass.status |= INV_NEW_DATA | INV_SENSOR_ON; 703 704 return INV_SUCCESS; 705} 706 707/** Record new temperature data for use when inv_execute_on_data() is called. 708 * @param[in] temp Temperature data in q16 format. 709 * @param[in] timestamp Monotonic time stamp; for Android it's in 710 * nanoseconds. 711* @return Returns INV_SUCCESS if successful or an error code if not. 712 */ 713inv_error_t inv_build_temp(const long temp, inv_time_t timestamp) 714{ 715#ifdef INV_PLAYBACK_DBG 716 if (inv_data_builder.debug_mode == RD_RECORD) { 717 int type = PLAYBACK_DBG_TYPE_TEMPERATURE; 718 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 719 fwrite(&temp, sizeof(temp), 1, inv_data_builder.file); 720 fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); 721 } 722#endif 723 sensors.temp.calibrated[0] = temp; 724 sensors.temp.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; 725 sensors.temp.timestamp_prev = sensors.temp.timestamp; 726 sensors.temp.timestamp = timestamp; 727 /* TODO: Apply scale, remove offset. */ 728 729 return INV_SUCCESS; 730} 731/** quaternion data 732* @param[in] quat Quaternion data. 2^30 = 1.0 or 2^14=1 for 16-bit data. 733* Real part first. Length 4. 734* @param[in] status number of axis, 16-bit or 32-bit 735* @param[in] timestamp 736* @param[in] timestamp Monotonic time stamp; for Android it's in 737* nanoseconds. 738* @param[out] executed Set to 1 if data processing was done. 739* @return Returns INV_SUCCESS if successful or an error code if not. 740*/ 741inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp) 742{ 743#ifdef INV_PLAYBACK_DBG 744 if (inv_data_builder.debug_mode == RD_RECORD) { 745 int type = PLAYBACK_DBG_TYPE_QUAT; 746 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 747 fwrite(quat, sizeof(quat[0]), 4, inv_data_builder.file); 748 fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); 749 } 750#endif 751 752 memcpy(sensors.quat.raw, quat, sizeof(sensors.quat.raw)); 753 sensors.quat.timestamp = timestamp; 754 sensors.quat.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; 755 sensors.quat.status |= (INV_BIAS_APPLIED & status); 756 757 return INV_SUCCESS; 758} 759 760/** This should be called when the accel has been turned off. This is so 761* that we will know if the data is contiguous. 762*/ 763void inv_accel_was_turned_off() 764{ 765 sensors.accel.status = 0; 766} 767 768/** This should be called when the compass has been turned off. This is so 769* that we will know if the data is contiguous. 770*/ 771void inv_compass_was_turned_off() 772{ 773 sensors.compass.status = 0; 774} 775 776/** This should be called when the quaternion data from the DMP has been turned off. This is so 777* that we will know if the data is contiguous. 778*/ 779void inv_quaternion_sensor_was_turned_off(void) 780{ 781 sensors.quat.status = 0; 782} 783 784/** This should be called when the gyro has been turned off. This is so 785* that we will know if the data is contiguous. 786*/ 787void inv_gyro_was_turned_off() 788{ 789 sensors.gyro.status = 0; 790} 791 792/** This should be called when the temperature sensor has been turned off. 793 * This is so that we will know if the data is contiguous. 794 */ 795void inv_temperature_was_turned_off() 796{ 797 sensors.temp.status = 0; 798} 799 800/** Registers to receive a callback when there is new sensor data. 801* @internal 802* @param[in] func Function pointer to receive callback when there is new sensor data 803* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority 804* numbers must be unique. 805* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be 806* a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW = 807* gyro data, INV_MAG_NEW = compass data. So passing in 808* INV_ACCEL_NEW | INV_MAG_NEW, a 809* callback would be generated if there was new magnetomer data OR new accel data. 810*/ 811inv_error_t inv_register_data_cb( 812 inv_error_t (*func)(struct inv_sensor_cal_t *data), 813 int priority, int sensor_type) 814{ 815 inv_error_t result = INV_SUCCESS; 816 int kk, nn; 817 818 // Make sure we haven't registered this function already 819 // Or used the same priority 820 for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { 821 if ((inv_data_builder.process[kk].func == func) || 822 (inv_data_builder.process[kk].priority == priority)) { 823 return INV_ERROR_INVALID_PARAMETER; //fixme give a warning 824 } 825 } 826 827 // Make sure we have not filled up our number of allowable callbacks 828 if (inv_data_builder.num_cb <= INV_MAX_DATA_CB - 1) { 829 kk = 0; 830 if (inv_data_builder.num_cb != 0) { 831 // set kk to be where this new callback goes in the array 832 while ((kk < inv_data_builder.num_cb) && 833 (inv_data_builder.process[kk].priority < priority)) { 834 kk++; 835 } 836 if (kk != inv_data_builder.num_cb) { 837 // We need to move the others 838 for (nn = inv_data_builder.num_cb; nn > kk; --nn) { 839 inv_data_builder.process[nn] = 840 inv_data_builder.process[nn - 1]; 841 } 842 } 843 } 844 // Add new callback 845 inv_data_builder.process[kk].func = func; 846 inv_data_builder.process[kk].priority = priority; 847 inv_data_builder.process[kk].data_required = sensor_type; 848 inv_data_builder.num_cb++; 849 } else { 850 MPL_LOGE("Unable to add feature callback as too many were already registered\n"); 851 result = INV_ERROR_MEMORY_EXAUSTED; 852 } 853 854 return result; 855} 856 857/** Unregisters the callback that happens when new sensor data is received. 858* @internal 859* @param[in] func Function pointer to receive callback when there is new sensor data 860* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority 861* numbers must be unique. 862* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be 863* a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW = 864* gyro data, INV_MAG_NEW = compass data. So passing in 865* INV_ACCEL_NEW | INV_MAG_NEW, a 866* callback would be generated if there was new magnetomer data OR new accel data. 867*/ 868inv_error_t inv_unregister_data_cb( 869 inv_error_t (*func)(struct inv_sensor_cal_t *data)) 870{ 871 int kk, nn; 872 873 for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { 874 if (inv_data_builder.process[kk].func == func) { 875 // Delete this callback 876 for (nn = kk + 1; nn < inv_data_builder.num_cb; ++nn) { 877 inv_data_builder.process[nn - 1] = 878 inv_data_builder.process[nn]; 879 } 880 inv_data_builder.num_cb--; 881 return INV_SUCCESS; 882 } 883 } 884 885 return INV_SUCCESS; // We did not find the callback 886} 887 888/** After at least one of inv_build_gyro(), inv_build_accel(), or 889* inv_build_compass() has been called, this function should be called. 890* It will process the data it has received and update all the internal states 891* and features that have been turned on. 892* @return Returns INV_SUCCESS if successful or an error code if not. 893*/ 894inv_error_t inv_execute_on_data(void) 895{ 896 inv_error_t result, first_error; 897 int kk; 898 int mode; 899 900#ifdef INV_PLAYBACK_DBG 901 if (inv_data_builder.debug_mode == RD_RECORD) { 902 int type = PLAYBACK_DBG_TYPE_EXECUTE; 903 fwrite(&type, sizeof(type), 1, inv_data_builder.file); 904 } 905#endif 906 // Determine what new data we have 907 mode = 0; 908 if (sensors.gyro.status & INV_NEW_DATA) 909 mode |= INV_GYRO_NEW; 910 if (sensors.accel.status & INV_NEW_DATA) 911 mode |= INV_ACCEL_NEW; 912 if (sensors.compass.status & INV_NEW_DATA) 913 mode |= INV_MAG_NEW; 914 if (sensors.temp.status & INV_NEW_DATA) 915 mode |= INV_TEMP_NEW; 916 if (sensors.quat.status & INV_QUAT_NEW) 917 mode |= INV_QUAT_NEW; 918 919 first_error = INV_SUCCESS; 920 921 for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { 922 if (mode & inv_data_builder.process[kk].data_required) { 923 result = inv_data_builder.process[kk].func(&sensors); 924 if (result && !first_error) { 925 first_error = result; 926 } 927 } 928 } 929 930 inv_set_contiguous(); 931 932 return first_error; 933} 934 935/** Cleans up status bits after running all the callbacks. It sets the contiguous flag. 936* 937*/ 938static void inv_set_contiguous(void) 939{ 940 inv_time_t current_time = 0; 941 if (sensors.gyro.status & INV_NEW_DATA) { 942 sensors.gyro.status |= INV_CONTIGUOUS; 943 current_time = sensors.gyro.timestamp; 944 } 945 if (sensors.accel.status & INV_NEW_DATA) { 946 sensors.accel.status |= INV_CONTIGUOUS; 947 current_time = MAX(current_time, sensors.accel.timestamp); 948 } 949 if (sensors.compass.status & INV_NEW_DATA) { 950 sensors.compass.status |= INV_CONTIGUOUS; 951 current_time = MAX(current_time, sensors.compass.timestamp); 952 } 953 if (sensors.temp.status & INV_NEW_DATA) { 954 sensors.temp.status |= INV_CONTIGUOUS; 955 current_time = MAX(current_time, sensors.temp.timestamp); 956 } 957 if (sensors.quat.status & INV_NEW_DATA) { 958 sensors.quat.status |= INV_CONTIGUOUS; 959 current_time = MAX(current_time, sensors.quat.timestamp); 960 } 961 962#if 0 963 /* See if sensors are still on. These should be turned off by inv_*_was_turned_off() 964 * type functions. This is just in case that breaks down. We make sure 965 * all the data is within 2 seconds of the newest piece of data*/ 966 if (inv_delta_time_ms(current_time, sensors.gyro.timestamp) >= 2000) 967 inv_gyro_was_turned_off(); 968 if (inv_delta_time_ms(current_time, sensors.accel.timestamp) >= 2000) 969 inv_accel_was_turned_off(); 970 if (inv_delta_time_ms(current_time, sensors.compass.timestamp) >= 2000) 971 inv_compass_was_turned_off(); 972 /* TODO: Temperature might not need to be read this quickly. */ 973 if (inv_delta_time_ms(current_time, sensors.temp.timestamp) >= 2000) 974 inv_temperature_was_turned_off(); 975#endif 976 977 /* clear bits */ 978 sensors.gyro.status &= ~INV_NEW_DATA; 979 sensors.accel.status &= ~INV_NEW_DATA; 980 sensors.compass.status &= ~INV_NEW_DATA; 981 sensors.temp.status &= ~INV_NEW_DATA; 982 sensors.quat.status &= ~INV_NEW_DATA; 983} 984 985/** Gets a whole set of accel data including data, accuracy and timestamp. 986 * @param[out] data Accel Data where 1g = 2^16 987 * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. 988 * @param[out] timestamp The timestamp of the data sample. 989*/ 990void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t *timestamp) 991{ 992 if (data != NULL) { 993 memcpy(data, sensors.accel.calibrated, sizeof(sensors.accel.calibrated)); 994 } 995 if (timestamp != NULL) { 996 *timestamp = sensors.accel.timestamp; 997 } 998 if (accuracy != NULL) { 999 *accuracy = sensors.accel.accuracy; 1000 } 1001} 1002 1003/** Gets a whole set of gyro data including data, accuracy and timestamp. 1004 * @param[out] data Gyro Data where 1 dps = 2^16 1005 * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. 1006 * @param[out] timestamp The timestamp of the data sample. 1007*/ 1008void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t *timestamp) 1009{ 1010 memcpy(data, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated)); 1011 if (timestamp != NULL) { 1012 *timestamp = sensors.gyro.timestamp; 1013 } 1014 if (accuracy != NULL) { 1015 *accuracy = sensors.gyro.accuracy; 1016 } 1017} 1018 1019/** Gets a whole set of gyro raw data including data, accuracy and timestamp. 1020 * @param[out] data Gyro Data where 1 dps = 2^16 1021 * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. 1022 * @param[out] timestamp The timestamp of the data sample. 1023*/ 1024void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp) 1025{ 1026 memcpy(data, sensors.gyro.raw_scaled, sizeof(sensors.gyro.raw_scaled)); 1027 if (timestamp != NULL) { 1028 *timestamp = sensors.gyro.timestamp; 1029 } 1030 if (accuracy != NULL) { 1031 *accuracy = sensors.gyro.accuracy; 1032 } 1033} 1034 1035/** Get's latest gyro data. 1036* @param[out] gyro Gyro Data, Length 3. 1 dps = 2^16. 1037*/ 1038void inv_get_gyro(long *gyro) 1039{ 1040 memcpy(gyro, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated)); 1041} 1042 1043/** Gets a whole set of compass data including data, accuracy and timestamp. 1044 * @param[out] data Compass Data where 1 uT = 2^16 1045 * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. 1046 * @param[out] timestamp The timestamp of the data sample. 1047*/ 1048void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t *timestamp) 1049{ 1050 memcpy(data, sensors.compass.calibrated, sizeof(sensors.compass.calibrated)); 1051 if (timestamp != NULL) { 1052 *timestamp = sensors.compass.timestamp; 1053 } 1054 if (accuracy != NULL) { 1055 if (inv_data_builder.compass_disturbance) 1056 *accuracy = 0; 1057 else 1058 *accuracy = sensors.compass.accuracy; 1059 } 1060} 1061 1062/** Gets a whole set of temperature data including data, accuracy and timestamp. 1063 * @param[out] data Temperature data where 1 degree C = 2^16 1064 * @param[out] accuracy 0 to 3, where 3 is most accurate. 1065 * @param[out] timestamp The timestamp of the data sample. 1066 */ 1067void inv_get_temp_set(long *data, int *accuracy, inv_time_t *timestamp) 1068{ 1069 data[0] = sensors.temp.calibrated[0]; 1070 if (timestamp) 1071 *timestamp = sensors.temp.timestamp; 1072 if (accuracy) 1073 *accuracy = sensors.temp.accuracy; 1074} 1075 1076/** Returns accuracy of gyro. 1077 * @return Accuracy of gyro with 0 being not accurate, and 3 being most accurate. 1078*/ 1079int inv_get_gyro_accuracy(void) 1080{ 1081 return sensors.gyro.accuracy; 1082} 1083 1084/** Returns accuracy of compass. 1085 * @return Accuracy of compass with 0 being not accurate, and 3 being most accurate. 1086*/ 1087int inv_get_mag_accuracy(void) 1088{ 1089 if (inv_data_builder.compass_disturbance) 1090 return 0; 1091 return sensors.compass.accuracy; 1092} 1093 1094/** Returns accuracy of accel. 1095 * @return Accuracy of accel with 0 being not accurate, and 3 being most accurate. 1096*/ 1097int inv_get_accel_accuracy(void) 1098{ 1099 return sensors.accel.accuracy; 1100} 1101 1102inv_error_t inv_get_gyro_orient(int *orient) 1103{ 1104 *orient = sensors.gyro.orientation; 1105 return 0; 1106} 1107 1108inv_error_t inv_get_accel_orient(int *orient) 1109{ 1110 *orient = sensors.accel.orientation; 1111 return 0; 1112} 1113 1114 1115/** 1116 * @} 1117 */ 1118