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 34 typedef inv_error_t (*inv_process_cb_func)(struct inv_sensor_cal_t *data); 35 36 struct process_t { 37 inv_process_cb_func func; 38 int priority; 39 int data_required; 40 }; 41 42 struct inv_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 60 struct 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 72 void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias); 73 static void inv_set_contiguous(void); 74 75 static struct inv_data_builder_t inv_data_builder; 76 static 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 */ 86 void 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 */ 96 void 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 */ 105 static 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 */ 120 static 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 */ 128 inv_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 */ 143 long 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 */ 153 long 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 */ 163 long 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 */ 174 void 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 */ 190 void 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 */ 207 void 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 */ 226 void 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 */ 245 void 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 261 void inv_get_gyro_sample_rate_ms(long *sample_rate_ms) 262 { 263 *sample_rate_ms = sensors.gyro.sample_rate_ms; 264 } 265 266 void inv_get_accel_sample_rate_ms(long *sample_rate_ms) 267 { 268 *sample_rate_ms = sensors.accel.sample_rate_ms; 269 } 270 271 void 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 */ 279 void 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 */ 295 void 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 */ 303 void 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 */ 311 void 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 */ 319 int 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 */ 327 int 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 */ 335 int 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 */ 344 inv_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 */ 376 void 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 */ 399 void 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 412 void 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 */ 425 void 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 */ 449 void 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 456 void 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 */ 470 void inv_set_compass_disturbance(int dist) 471 { 472 inv_data_builder.compass_disturbance = dist; 473 } 474 475 int 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 */ 482 void 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 */ 498 void 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 */ 510 void 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 */ 536 void 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 */ 566 void 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 */ 579 void 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 */ 604 inv_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 */ 643 inv_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 */ 674 inv_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 */ 713 inv_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 */ 741 inv_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 */ 763 void 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 */ 771 void 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 */ 779 void 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 */ 787 void 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 */ 795 void 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 */ 811 inv_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 */ 868 inv_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 */ 894 inv_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 */ 938 static 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 */ 990 void 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 */ 1008 void 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 */ 1024 void 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 */ 1038 void 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 */ 1048 void 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 */ 1067 void 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 */ 1079 int 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 */ 1087 int 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 */ 1097 int inv_get_accel_accuracy(void) 1098 { 1099 return sensors.accel.accuracy; 1100 } 1101 1102 inv_error_t inv_get_gyro_orient(int *orient) 1103 { 1104 *orient = sensors.gyro.orientation; 1105 return 0; 1106 } 1107 1108 inv_error_t inv_get_accel_orient(int *orient) 1109 { 1110 *orient = sensors.accel.orientation; 1111 return 0; 1112 } 1113 1114 1115 /** 1116 * @} 1117 */ 1118
