C++ (Cpp) inv_convert_to_body_with_scaleの例

コード例 #1

ファイル: data_builder.c プロジェクト: stkunkel/indoor-loc

/** 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.
* @param[in,out] sensor structure to modify
* @param[in] bias bias in the mounting frame, in hardware units scaled by
*                 2^16. Length 3.
*/
void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias)
{
    long raw32[3];

    // Convert raw to calibrated
    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;
}

コード例 #2

ファイル: datalogger_outputs.c プロジェクト: MIPS/hardware-invensense

/**
 *  Raw (uncompensated) angular velocity (degrees per second) in body frame.
 *  @param[out] values      raw angular velocity in dps.
 *  @param[out] timestamp   Time when sensor was sampled.
 */
void inv_get_sensor_type_gyro_raw_body_float(float *values,
        inv_time_t *timestamp)
{
    struct inv_single_sensor_t *pg = &dl_out.sc.gyro;
    long raw[3];
    long raw_body[3];

    raw[0] = (long) pg->raw[0] * (1L << 16);
    raw[1] = (long) pg->raw[1] * (1L << 16);
    raw[2] = (long) pg->raw[2] * (1L << 16);
    inv_convert_to_body_with_scale(pg->orientation, pg->sensitivity,
                                   raw, raw_body);
    if (values) {
        values[0] = inv_q16_to_float(raw_body[0]);
        values[1] = inv_q16_to_float(raw_body[1]);
        values[2] = inv_q16_to_float(raw_body[2]);
    }
    if (timestamp)
        *timestamp = pg->timestamp;
}