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