/** Record new compass data for use when inv_execute_on_data() is called * @param[in] compass Compass data, if it was calibrated outside MPL, the units are uT scaled by 2^16. * Length 3. * @param[in] status Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 being most accurate. * The upper bit INV_CALIBRATED, is set if the data was calibrated outside MPL and it is * not set if the data being passed is raw. Raw data should be in device units, typically * in a 16-bit range. * @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds. * @return Returns INV_SUCCESS if successful or an error code if not. */ inv_error_t inv_build_compass(const long *compass, int status, inv_time_t timestamp) { #ifdef INV_PLAYBACK_DBG if (inv_data_builder.debug_mode == RD_RECORD) { int type = PLAYBACK_DBG_TYPE_COMPASS; fwrite(&type, sizeof(type), 1, inv_data_builder.file); fwrite(compass, sizeof(compass[0]), 3, inv_data_builder.file); fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); } #endif if ((status & INV_CALIBRATED) == 0) { sensors.compass.raw[0] = (short)compass[0]; sensors.compass.raw[1] = (short)compass[1]; sensors.compass.raw[2] = (short)compass[2]; inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias); sensors.compass.status |= INV_RAW_DATA; } else { sensors.compass.calibrated[0] = compass[0]; sensors.compass.calibrated[1] = compass[1]; sensors.compass.calibrated[2] = compass[2]; sensors.compass.status |= INV_CALIBRATED; sensors.compass.accuracy = status & 3; inv_data_builder.save.compass_accuracy = status & 3; } sensors.compass.timestamp_prev = sensors.compass.timestamp; sensors.compass.timestamp = timestamp; sensors.compass.status |= INV_NEW_DATA | INV_SENSOR_ON; return INV_SUCCESS; }
/** Sets the gyro bias * @param[in] bias Gyro bias in hardware units scaled by 2^16. In chip mounting frame. * Length 3. * @param[in] accuracy Accuracy of bias. 0 = least accurate, 3 = most accurate. */ void inv_set_gyro_bias(const long *bias, int accuracy) { if (bias != NULL) { if (memcmp(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias))) { memcpy(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias)); inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias); } } sensors.gyro.accuracy = accuracy; inv_data_builder.save.gyro_accuracy = accuracy; /* TODO: What should we do if there's no temperature data? */ if (sensors.temp.calibrated[0]) inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0]; else /* Set to 27 deg C for now until we've got a better solution. */ inv_data_builder.save.gyro_temp = 1769472L; inv_set_message(INV_MSG_NEW_GB_EVENT, INV_MSG_NEW_GB_EVENT, 0); /* TODO: this flag works around the synchronization problem seen with using the user-exposed message layer to signal the temperature compensation module that gyro biases were set. A better, cleaner method is certainly needed. */ inv_data_builder.save.gyro_bias_tc_set = true; }
void inv_set_compass_bias(const long *bias, int accuracy) { if (memcmp(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias))) { memcpy(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias)); inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias); } sensors.compass.accuracy = accuracy; inv_data_builder.save.compass_accuracy = accuracy; inv_set_message(INV_MSG_NEW_CB_EVENT, INV_MSG_NEW_CB_EVENT, 0); }
/** Sets the accel bias with control over which axis. * @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame * @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. * @param[in] mask Mask to select axis to apply bias set. */ void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask) { if (bias) { if (mask & 1){ inv_data_builder.save.accel_bias[0] = bias[0]; } if (mask & 2){ inv_data_builder.save.accel_bias[1] = bias[1]; } if (mask & 4){ inv_data_builder.save.accel_bias[2] = bias[2]; } inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); } sensors.accel.accuracy = accuracy; inv_data_builder.save.accel_accuracy = accuracy; }
/** Record new gyro data and calls inv_execute_on_data() if previous * sample has not been processed. * @param[in] gyro Data is in device units. Length 3. * @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds. * @return Returns INV_SUCCESS if successful or an error code if not. */ inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp) { #ifdef INV_PLAYBACK_DBG if (inv_data_builder.debug_mode == RD_RECORD) { int type = PLAYBACK_DBG_TYPE_GYRO; fwrite(&type, sizeof(type), 1, inv_data_builder.file); fwrite(gyro, sizeof(gyro[0]), 3, inv_data_builder.file); fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); } #endif memcpy(sensors.gyro.raw, gyro, 3 * sizeof(short)); sensors.gyro.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; sensors.gyro.timestamp_prev = sensors.gyro.timestamp; sensors.gyro.timestamp = timestamp; inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias); return INV_SUCCESS; }
/** Sets the gyro bias * @param[in] bias Gyro bias in hardware units scaled by 2^16. In chip mounting frame. * Length 3. * @param[in] accuracy Accuracy of bias. 0 = least accurate, 3 = most accurate. */ void inv_set_gyro_bias(const long *bias, int accuracy) { if (bias != NULL) { if (memcmp(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias))) { memcpy(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias)); inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias); } } sensors.gyro.accuracy = accuracy; inv_data_builder.save.gyro_accuracy = accuracy; /* TODO: What should we do if there's no temperature data? */ if (sensors.temp.calibrated[0]) inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0]; else /* Set to 27 deg C for now until we've got a better solution. */ inv_data_builder.save.gyro_temp = 1769472L; inv_set_message(INV_MSG_NEW_GB_EVENT, INV_MSG_NEW_GB_EVENT, 0); }