// This takes about 20us to run
bool AP_InertialSensor_Flymaple::update(void)
{
Vector3f accel, gyro;
hal.scheduler->suspend_timer_procs();
accel = _accel_filtered;
gyro = _gyro_filtered;
_have_gyro_sample = false;
_have_accel_sample = false;
hal.scheduler->resume_timer_procs();
// Adjust for chip scaling to get m/s/s
accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
_publish_accel(_accel_instance, accel);
// Adjust for chip scaling to get radians/sec
gyro *= FLYMAPLE_GYRO_SCALE_R_S;
_publish_gyro(_gyro_instance, gyro);
if (_last_filter_hz != _accel_filter_cutoff()) {
_set_filter_frequency(_accel_filter_cutoff());
_last_filter_hz = _accel_filter_cutoff();
}
return true;
}
/*
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;
}
/*
process any
*/
bool AP_InertialSensor_MPU6000::update( void )
{
#if !MPU6000_FAST_SAMPLING
if (_sum_count < _sample_count) {
// we don't have enough samples yet
return false;
}
#endif
// we have a full set of samples
uint16_t num_samples;
Vector3f accel, gyro;
hal.scheduler->suspend_timer_procs();
#if MPU6000_FAST_SAMPLING
gyro = _gyro_filtered;
accel = _accel_filtered;
num_samples = 1;
#else
gyro(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
accel(_accel_sum.x, _accel_sum.y, _accel_sum.z);
num_samples = _sum_count;
_accel_sum.zero();
_gyro_sum.zero();
#endif
_sum_count = 0;
hal.scheduler->resume_timer_procs();
gyro *= _gyro_scale / num_samples;
accel *= MPU6000_ACCEL_SCALE_1G / num_samples;
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
accel.rotate(ROTATION_PITCH_180_YAW_90);
gyro.rotate(ROTATION_PITCH_180_YAW_90);
#endif
_publish_accel(_accel_instance, accel);
_publish_gyro(_gyro_instance, gyro);
#if MPU6000_FAST_SAMPLING
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
_accel_filter.set_cutoff_frequency(1000, _accel_filter_cutoff());
_last_accel_filter_hz = _accel_filter_cutoff();
}
if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
_gyro_filter.set_cutoff_frequency(1000, _gyro_filter_cutoff());
_last_gyro_filter_hz = _gyro_filter_cutoff();
}
#else
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
if (_spi_sem->take(10)) {
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
_set_filter_register(_accel_filter_cutoff());
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
_spi_sem->give();
_last_accel_filter_hz = _accel_filter_cutoff();
}
}
#endif
return true;
}
