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