bool AP_InertialSensor_MPU6000::update( void )
{
int32_t sum[7];
uint16_t count;
float count_scale;
// wait for at least 1 sample
while (_count == 0) /* nop */;
// disable interrupts for mininum time
cli();
for (int i=0; i<7; i++) {
sum[i] = _sum[i];
_sum[i] = 0;
}
count = _count;
_count = 0;
sei();
count_scale = 1.0 / count;
_gyro.x = _gyro_scale * _gyro_data_sign[0] * sum[_gyro_data_index[0]] * count_scale;
_gyro.y = _gyro_scale * _gyro_data_sign[1] * sum[_gyro_data_index[1]] * count_scale;
_gyro.z = _gyro_scale * _gyro_data_sign[2] * sum[_gyro_data_index[2]] * count_scale;
_accel.x = _accel_scale * _accel_data_sign[0] * sum[_accel_data_index[0]] * count_scale;
_accel.y = _accel_scale * _accel_data_sign[1] * sum[_accel_data_index[1]] * count_scale;
_accel.z = _accel_scale * _accel_data_sign[2] * sum[_accel_data_index[2]] * count_scale;
_temp = _temp_to_celsius(sum[_temp_data_index] * count_scale);
return true;
}
bool AP_InertialSensor_MPU6000::update( void )
{
int32_t sum[7];
float count_scale;
Vector3f accel_scale = _accel_scale.get();
// wait for at least 1 sample
if (!wait_for_sample(1000)) {
return false;
}
// disable timer procs for mininum time
hal.scheduler->suspend_timer_procs();
/** ATOMIC SECTION w/r/t TIMER PROCESS */
{
for (int i=0; i<7; i++) {
sum[i] = _sum[i];
_sum[i] = 0;
}
_num_samples = _count;
_count = 0;
}
hal.scheduler->resume_timer_procs();
count_scale = 1.0f / _num_samples;
_gyro = Vector3f(_gyro_data_sign[0] * sum[_gyro_data_index[0]],
_gyro_data_sign[1] * sum[_gyro_data_index[1]],
_gyro_data_sign[2] * sum[_gyro_data_index[2]]);
_gyro.rotate(_board_orientation);
_gyro *= _gyro_scale * count_scale;
_gyro -= _gyro_offset;
_accel = Vector3f(_accel_data_sign[0] * sum[_accel_data_index[0]],
_accel_data_sign[1] * sum[_accel_data_index[1]],
_accel_data_sign[2] * sum[_accel_data_index[2]]);
_accel.rotate(_board_orientation);
_accel *= count_scale * MPU6000_ACCEL_SCALE_1G;
_accel.x *= accel_scale.x;
_accel.y *= accel_scale.y;
_accel.z *= accel_scale.z;
_accel -= _accel_offset;
_temp = _temp_to_celsius(sum[_temp_data_index] * count_scale);
if (_last_filter_hz != _mpu6000_filter) {
if (_spi_sem->take(10)) {
_set_filter_register(_mpu6000_filter, 0);
_spi_sem->give();
}
}
return true;
}
bool AP_InertialSensor_ITG3200::update( void )
{
int32_t sum[7];
float count_scale;
Vector3f accel_scale = _accel_scale.get();
// wait for at least 1 sample
wait_for_sample();
// disable interrupts for mininum time
hal.scheduler->suspend_timer_procs();
/** ATOMIC SECTION w/r/t TIMER PROCESS */
{
for (int i=0; i<7; i++) {
sum[i] = _sum[i];
_sum[i] = 0;
}
_num_samples = _count;
_count = 0;
}
hal.scheduler->resume_timer_procs();
count_scale = 1.0f / _num_samples;
_gyro = Vector3f(_gyro_data_sign[0] * sum[_gyro_data_index[0]],
_gyro_data_sign[1] * sum[_gyro_data_index[1]],
_gyro_data_sign[2] * sum[_gyro_data_index[2]]);
_gyro.rotate(_board_orientation);
_gyro *= _gyro_scale * count_scale;
_gyro -= _gyro_offset;
_accel = Vector3f(_accel_data_sign[0] * sum[_accel_data_index[0]],
_accel_data_sign[1] * sum[_accel_data_index[1]],
_accel_data_sign[2] * sum[_accel_data_index[2]]);
_accel.rotate(_board_orientation);
_accel *= count_scale * _accel_scale_1G;
_accel.x *= accel_scale.x;
_accel.y *= accel_scale.y;
_accel.z *= accel_scale.z;
_accel -= _accel_offset;
_temp = _temp_to_celsius(sum[_temp_data_index] * count_scale);
return true;
}
bool AP_InertialSensor_MPU6000_Ext::update( void )
{
// wait for at least 1 sample
if (!wait_for_sample(1000)) {
return false;
}
_previous_accel[0] = _accel[0];
// disable timer procs for mininum time
hal.scheduler->suspend_timer_procs();
_gyro[0] = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
_accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z);
_num_samples = _sum_count;
_accel_sum.zero();
_gyro_sum.zero();
_temp[0] = _temp_sum;
_temp_sum = 0;
_sum_count = 0;
hal.scheduler->resume_timer_procs();
_gyro[0].rotate(_board_orientation);
_gyro[0] *= _gyro_scale / _num_samples;
_gyro[0] -= _gyro_offset[0];
_accel[0].rotate(_board_orientation);
_accel[0] *= MPU6000_ACCEL_SCALE_1G / _num_samples;
Vector3f accel_scale = _accel_scale[0].get();
_accel[0].x *= accel_scale.x;
_accel[0].y *= accel_scale.y;
_accel[0].z *= accel_scale.z;
_accel[0] -= _accel_offset[0];
_temp[0] = _temp_to_celsius(_temp[0] / _num_samples);
if (_last_filter_hz != _mpu6000_filter) {
if (_spi_sem->take(10)) {
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
_set_filter_register(_mpu6000_filter, 0);
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
_error_count = 0;
_spi_sem->give();
}
}
return true;
}
bool AP_InertialSensor_VRBRAIN::update( void )
{
// wait for at least 1 sample
if (!wait_for_sample(100)) {
return false;
}
_previous_accel[0] = _accel[0];
// disable timer procs for mininum time
hal.scheduler->suspend_timer_procs();
_accel[0] = _accel_filtered;
_accel_samples = 0;
_gyro[0] = _gyro_filtered;
_gyro_samples = 0;
_temp[0] = _temp_filtered;
hal.scheduler->resume_timer_procs();
_accel[0].rotate(_board_orientation);
_accel[0] *= MPU6000_ACCEL_SCALE_1G;
Vector3f accel_scale = _accel_scale[0].get();
_accel[0].x *= accel_scale.x;
_accel[0].y *= accel_scale.y;
_accel[0].z *= accel_scale.z;
_accel[0] -= _accel_offset[0];
_gyro[0].rotate(_board_orientation);
_gyro[0] *= _gyro_scale;
_gyro[0] -= _gyro_offset[0];
_temp[0] = _temp_to_celsius(_temp[0]);
if (_last_filter_hz != _mpu6000_filter) {
_set_filter_frequency(_mpu6000_filter);
_last_filter_hz = _mpu6000_filter;
}
return true;
}
