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