/** 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);
}
