void AP_InertialSensor_PX4::_new_accel_sample(uint8_t i, accel_report &accel_report) { Vector3f accel = Vector3f(accel_report.x, accel_report.y, accel_report.z); uint8_t frontend_instance = _accel_instance[i]; // apply corrections _rotate_and_correct_accel(frontend_instance, accel); _notify_new_accel_raw_sample(frontend_instance, accel, accel_report.timestamp); // save last timestamp _last_accel_timestamp[i] = accel_report.timestamp; // report error count _set_accel_error_count(frontend_instance, accel_report.error_count); // publish a temperature (for logging purposed only) _publish_temperature(frontend_instance, accel_report.temperature); #ifdef AP_INERTIALSENSOR_PX4_DEBUG // get time since last sample float dt = _accel_sample_time[i]; _accel_dt_max[i] = max(_accel_dt_max[i],dt); _accel_meas_count[i] ++; if(_accel_meas_count[i] >= 10000) { uint32_t tnow = AP_HAL::micros(); ::printf("a%d %.2f Hz max %.8f s\n", frontend_instance, 10000.0f/((tnow-_accel_meas_count_start_us[i])*1.0e-6f),_accel_dt_max[i]); _accel_meas_count_start_us[i] = tnow; _accel_meas_count[i] = 0; _accel_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(); }