Ejemplos de _rotate_and_correct_gyro en C++ (Cpp)

Ejemplo n.º 1

Archivo: AP_InertialSensor_PX4.cpp Proyecto: MarkHarbison/ardupilot

void AP_InertialSensor_PX4::_new_gyro_sample(uint8_t i, gyro_report &gyro_report)
{
    Vector3f gyro = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
    uint8_t frontend_instance = _gyro_instance[i];

    // apply corrections
    _rotate_and_correct_gyro(frontend_instance, gyro);
    _notify_new_gyro_raw_sample(frontend_instance, gyro, gyro_report.timestamp);

    // save last timestamp
    _last_gyro_timestamp[i] = gyro_report.timestamp;

    // report error count
    _set_gyro_error_count(_gyro_instance[i], gyro_report.error_count);

#ifdef AP_INERTIALSENSOR_PX4_DEBUG
    // get time since last sample
    float dt = _gyro_sample_time[i];

    _gyro_dt_max[i] = max(_gyro_dt_max[i],dt);

    _gyro_meas_count[i] ++;

    if(_gyro_meas_count[i] >= 10000) {
        uint32_t tnow = AP_HAL::micros();

        ::printf("g%d %.2f Hz max %.8f s\n", frontend_instance, 10000.0f/((tnow-_gyro_meas_count_start_us[i])*1.0e-6f), _gyro_dt_max[i]);

        _gyro_meas_count_start_us[i] = tnow;
        _gyro_meas_count[i] = 0;
        _gyro_dt_max[i] = 0;
    }
#endif // AP_INERTIALSENSOR_PX4_DEBUG
}

Ejemplo n.º 2

Archivo: AP_InertialSensor_Backend.cpp Proyecto: IR-LOCK/ardupilot

/*
  rotate gyro vector and add the gyro offset
 */
void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &gyro, bool rotate_and_correct)
{
    _imu._gyro[instance] = gyro;
    _imu._gyro_healthy[instance] = true;

    if (rotate_and_correct) {
        _rotate_and_correct_gyro(instance, _imu._gyro[instance]);
    }
}

Ejemplo n.º 3

Archivo: AP_InertialSensor_QURT.cpp Proyecto: CUAir/ardupilot

void AP_InertialSensor_QURT::accumulate(void) 
{
    const float ACCEL_SCALE_1G = GRAVITY_MSS / 2048.0;
    const float GYRO_SCALE = 0.0174532 / 16.4;

    struct mpu9x50_data data;
    
    while (buf.pop(data)) {
        Vector3f accel(data.accel_raw[0]*ACCEL_SCALE_1G,
                       data.accel_raw[1]*ACCEL_SCALE_1G,
                       data.accel_raw[2]*ACCEL_SCALE_1G);
        Vector3f gyro(data.gyro_raw[0]*GYRO_SCALE,
                      data.gyro_raw[1]*GYRO_SCALE,
                      data.gyro_raw[2]*GYRO_SCALE);

        _rotate_and_correct_accel(accel_instance, accel);
        _rotate_and_correct_gyro(gyro_instance, gyro);
    
        _notify_new_gyro_raw_sample(gyro_instance, gyro, data.timestamp);
        _notify_new_accel_raw_sample(accel_instance, accel, data.timestamp);
    }
}

Ejemplo n.º 4

Archivo: AP_InertialSensor_Oilpan.cpp Proyecto: AurelienRoy/ardupilot

/*
  copy data from ADC to frontend
 */
bool AP_InertialSensor_Oilpan::update()
{
    float adc_values[6];

    apm1_adc.Ch6(_sensors, adc_values);

    // copy gyros to frontend
    Vector3f v;
    v(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_x,
      _sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_y,
      _sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_z);
    _rotate_and_correct_gyro(_gyro_instance, v);
    _publish_gyro(_gyro_instance, v);

    // copy accels to frontend
    v(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET),
      _sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET),
      _sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET));
    v *= OILPAN_ACCEL_SCALE_1G;
    _rotate_and_correct_accel(_accel_instance, v);
    _publish_accel(_accel_instance, v);

    return true;
}