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