/* Takes raw data stored in the sensor, removes bias, and converts it to calibrated data in the body frame. Also store raw data for body frame.  */
void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias)
/* Takes raw data stored in the sensor, removes bias, and converts it to calibrated data in the body frame. Also store raw data for body frame.  */
void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias)
{
    long raw32[3];
    raw32[0] = (long)sensor->raw[0] << 15;
    raw32[1] = (long)sensor->raw[1] << 15;
    raw32[2] = (long)sensor->raw[2] << 15;
    inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_scaled);
    raw32[0] -= bias[0] >> 1;
    raw32[1] -= bias[1] >> 1;
    raw32[2] -= bias[2] >> 1;
    inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->calibrated);
    sensor->status |= INV_CALIBRATED;
}