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_PX4::_get_sample(void)
{
for (uint8_t i=0; i<_num_accel_instances; i++) {
struct accel_report accel_report;
while (_accel_fd[i] != -1 &&
::read(_accel_fd[i], &accel_report, sizeof(accel_report)) == sizeof(accel_report) &&
accel_report.timestamp != _last_accel_timestamp[i]) {
_accel_in[i] = Vector3f(accel_report.x, accel_report.y, accel_report.z);
_last_accel_timestamp[i] = accel_report.timestamp;
_set_accel_error_count(_accel_instance[i], accel_report.error_count);
}
}
for (uint8_t i=0; i<_num_gyro_instances; i++) {
struct gyro_report gyro_report;
while (_gyro_fd[i] != -1 &&
::read(_gyro_fd[i], &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report) &&
gyro_report.timestamp != _last_gyro_timestamp[i]) {
_gyro_in[i] = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
_last_gyro_timestamp[i] = gyro_report.timestamp;
_set_gyro_error_count(_gyro_instance[i], gyro_report.error_count);
}
}
_last_get_sample_timestamp = hal.scheduler->micros64();
}
