/* rotate accel vector, scale and add the accel offset */ void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f &accel, bool rotate_and_correct) { _imu._accel[instance] = accel; _imu._accel_healthy[instance] = true; if (rotate_and_correct) { _rotate_and_correct_accel(instance, _imu._accel[instance]); } }
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 }
/* 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; }