bool AP_InertialSensor_VRBRAIN::update(void)
{
if (!wait_for_sample(100)) {
return false;
}
// get the latest sample from the sensor drivers
_get_sample();
for (uint8_t k=0; k<_num_accel_instances; k++) {
_previous_accel[k] = _accel[k];
_accel[k] = _accel_in[k];
_accel[k].rotate(_board_orientation);
_accel[k].x *= _accel_scale[k].get().x;
_accel[k].y *= _accel_scale[k].get().y;
_accel[k].z *= _accel_scale[k].get().z;
_accel[k] -= _accel_offset[k];
}
for (uint8_t k=0; k<_num_gyro_instances; k++) {
_gyro[k] = _gyro_in[k];
_gyro[k].rotate(_board_orientation);
_gyro[k] -= _gyro_offset[k];
}
if (_last_filter_hz != _mpu6000_filter) {
_set_filter_frequency(_mpu6000_filter);
_last_filter_hz = _mpu6000_filter;
}
_have_sample_available = false;
return true;
}
bool AP_InertialSensor_PX4::update(void)
{
Vector3f accel_scale = _accel_scale.get();
// get the latest sample from the sensor drivers
_get_sample();
_previous_accel = _accel;
_accel = _accel_in;
_gyro = _gyro_in;
// add offsets and rotation
_accel.rotate(_board_orientation);
_accel.x *= accel_scale.x;
_accel.y *= accel_scale.y;
_accel.z *= accel_scale.z;
_accel -= _accel_offset;
_gyro.rotate(_board_orientation);
_gyro -= _gyro_offset;
if (_last_filter_hz != _mpu6000_filter) {
_set_filter_frequency(_mpu6000_filter);
_last_filter_hz = _mpu6000_filter;
}
_have_sample_available = false;
return true;
}
// 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 filtered data to the frontend
*/
bool AP_InertialSensor_L3G4200D::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 *= ADXL345_ACCELEROMETER_SCALE_M_S;
_rotate_and_offset_accel(_accel_instance, accel);
// Adjust for chip scaling to get radians/sec
gyro *= L3G4200D_GYRO_SCALE_R_S;
_rotate_and_offset_gyro(_gyro_instance, gyro);
if (_last_filter_hz != _imu.get_filter()) {
_set_filter_frequency(_imu.get_filter());
_last_filter_hz = _imu.get_filter();
}
return true;
}
bool AP_InertialSensor_PX4::update(void)
{
// get the latest sample from the sensor drivers
_get_sample();
for (uint8_t k=0; k<_num_accel_instances; k++) {
Vector3f accel = _accel_in[k];
// calling _rotate_and_offset_accel sets the sensor healthy,
// so we only want to do this if we have new data from it
if (_last_accel_timestamp[k] != _last_accel_update_timestamp[k]) {
_rotate_and_offset_accel(_accel_instance[k], accel);
_last_accel_update_timestamp[k] = _last_accel_timestamp[k];
}
}
for (uint8_t k=0; k<_num_gyro_instances; k++) {
Vector3f gyro = _gyro_in[k];
// calling _rotate_and_offset_accel sets the sensor healthy,
// so we only want to do this if we have new data from it
if (_last_gyro_timestamp[k] != _last_gyro_update_timestamp[k]) {
_rotate_and_offset_gyro(_gyro_instance[k], gyro);
_last_gyro_update_timestamp[k] = _last_gyro_timestamp[k];
}
}
if (_last_filter_hz != _imu.get_filter()) {
_set_filter_frequency(_imu.get_filter());
_last_filter_hz = _imu.get_filter();
}
return true;
}
uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
{
// assumes max 2 instances
_accel_fd[0] = open(ACCEL_DEVICE_PATH, O_RDONLY);
_accel_fd[1] = open(ACCEL_DEVICE_PATH "1", O_RDONLY);
_accel_fd[2] = open(ACCEL_DEVICE_PATH "2", O_RDONLY);
_gyro_fd[0] = open(GYRO_DEVICE_PATH, O_RDONLY);
_gyro_fd[1] = open(GYRO_DEVICE_PATH "1", O_RDONLY);
_gyro_fd[2] = open(GYRO_DEVICE_PATH "2", O_RDONLY);
_num_accel_instances = 0;
_num_gyro_instances = 0;
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_accel_fd[i] >= 0) {
_num_accel_instances = i+1;
}
if (_gyro_fd[i] >= 0) {
_num_gyro_instances = i+1;
}
}
if (_num_accel_instances == 0) {
hal.scheduler->panic("Unable to open accel device " ACCEL_DEVICE_PATH);
}
if (_num_gyro_instances == 0) {
hal.scheduler->panic("Unable to open gyro device " GYRO_DEVICE_PATH);
}
switch (sample_rate) {
case RATE_50HZ:
_default_filter_hz = 15;
_sample_time_usec = 20000;
break;
case RATE_100HZ:
_default_filter_hz = 30;
_sample_time_usec = 10000;
break;
case RATE_200HZ:
_default_filter_hz = 30;
_sample_time_usec = 5000;
break;
case RATE_400HZ:
default:
_default_filter_hz = 30;
_sample_time_usec = 2500;
break;
}
_set_filter_frequency(_mpu6000_filter);
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
return AP_PRODUCT_ID_PX4_V2;
#else
return AP_PRODUCT_ID_PX4;
#endif
}
uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
{
switch (sample_rate) {
case RATE_50HZ:
_default_filter_hz = 15;
_sample_time_usec = 20000;
break;
case RATE_100HZ:
_default_filter_hz = 30;
_sample_time_usec = 10000;
break;
case RATE_200HZ:
default:
_default_filter_hz = 30;
_sample_time_usec = 5000;
break;
}
_delta_time = _sample_time_usec * 1.0e-6f;
// init accelerometers
_accel_fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (_accel_fd < 0) {
hal.scheduler->panic("Unable to open accel device " ACCEL_DEVICE_PATH);
}
_gyro_fd = open(GYRO_DEVICE_PATH, O_RDONLY);
if (_gyro_fd < 0) {
hal.scheduler->panic("Unable to open gyro device " GYRO_DEVICE_PATH);
}
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
uint32_t driver_rate = 1000;
#else
uint32_t driver_rate = 800;
#endif
/*
* set the accel and gyro sampling rate.
*/
ioctl(_accel_fd, ACCELIOCSSAMPLERATE, driver_rate);
ioctl(_accel_fd, SENSORIOCSPOLLRATE, driver_rate);
ioctl(_gyro_fd, GYROIOCSSAMPLERATE, driver_rate);
ioctl(_gyro_fd, SENSORIOCSPOLLRATE, driver_rate);
_set_filter_frequency(_mpu6000_filter);
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
return AP_PRODUCT_ID_PX4_V2;
#else
return AP_PRODUCT_ID_PX4;
#endif
}
// This takes about 20us to run
bool AP_InertialSensor_Flymaple::update(void)
{
while (sample_available() == false) {
hal.scheduler->delay(1);
}
Vector3f accel_scale = _accel_scale.get();
// Not really needed since Flymaple _accumulate runs in the main thread
hal.scheduler->suspend_timer_procs();
// base the time on the gyro timestamp, as that is what is
// multiplied by time to integrate in DCM
_delta_time = (_last_gyro_timestamp - _last_update_usec) * 1.0e-6f;
_last_update_usec = _last_gyro_timestamp;
_accel = _accel_filtered;
_accel_samples = 0;
_gyro = _gyro_filtered;
_gyro_samples = 0;
hal.scheduler->resume_timer_procs();
// add offsets and rotation
_accel.rotate(_board_orientation);
// Adjust for chip scaling to get m/s/s
_accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
// Now the calibration scale factor
_accel.x *= accel_scale.x;
_accel.y *= accel_scale.y;
_accel.z *= accel_scale.z;
_accel -= _accel_offset;
_gyro.rotate(_board_orientation);
// Adjust for chip scaling to get radians/sec
_gyro *= FLYMAPLE_GYRO_SCALE_R_S;
_gyro -= _gyro_offset;
if (_last_filter_hz != _mpu6000_filter) {
_set_filter_frequency(_mpu6000_filter);
_last_filter_hz = _mpu6000_filter;
}
return true;
}
uint16_t AP_InertialSensor_VRBRAIN::_init_sensor( Sample_rate sample_rate )
{
// assumes max 2 instances
_accel_fd[0] = open(ACCEL_DEVICE_PATH, O_RDONLY);
_accel_fd[1] = open(ACCEL_DEVICE_PATH "1", O_RDONLY);
_gyro_fd[0] = open(GYRO_DEVICE_PATH, O_RDONLY);
_gyro_fd[1] = open(GYRO_DEVICE_PATH "1", O_RDONLY);
if (_accel_fd[0] < 0) {
hal.scheduler->panic("Unable to open accel device " ACCEL_DEVICE_PATH);
}
if (_gyro_fd[0] < 0) {
hal.scheduler->panic("Unable to open gyro device " GYRO_DEVICE_PATH);
}
_num_accel_instances = _accel_fd[1] >= 0?2:1;
_num_gyro_instances = _gyro_fd[1] >= 0?2:1;
switch (sample_rate) {
case RATE_50HZ:
_default_filter_hz = 15;
_sample_time_usec = 20000;
break;
case RATE_100HZ:
_default_filter_hz = 30;
_sample_time_usec = 10000;
break;
case RATE_200HZ:
_default_filter_hz = 30;
_sample_time_usec = 5000;
break;
case RATE_400HZ:
default:
_default_filter_hz = 30;
_sample_time_usec = 2500;
break;
}
_set_filter_frequency(_mpu6000_filter);
return AP_PRODUCT_ID_VRBRAIN;
}
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;
}
bool AP_InertialSensor_PX4::_init_sensor(void)
{
// assumes max 3 instances
_accel_fd[0] = open(ACCEL_BASE_DEVICE_PATH "0", O_RDONLY);
_accel_fd[1] = open(ACCEL_BASE_DEVICE_PATH "1", O_RDONLY);
_accel_fd[2] = open(ACCEL_BASE_DEVICE_PATH "2", O_RDONLY);
_gyro_fd[0] = open(GYRO_BASE_DEVICE_PATH "0", O_RDONLY);
_gyro_fd[1] = open(GYRO_BASE_DEVICE_PATH "1", O_RDONLY);
_gyro_fd[2] = open(GYRO_BASE_DEVICE_PATH "2", O_RDONLY);
_num_accel_instances = 0;
_num_gyro_instances = 0;
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_accel_fd[i] >= 0) {
_num_accel_instances = i+1;
_accel_instance[i] = _imu.register_accel();
}
if (_gyro_fd[i] >= 0) {
_num_gyro_instances = i+1;
_gyro_instance[i] = _imu.register_gyro();
}
}
if (_num_accel_instances == 0) {
return false;
}
if (_num_gyro_instances == 0) {
return false;
}
_default_filter_hz = _default_filter();
_set_filter_frequency(_imu.get_filter());
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
_product_id = AP_PRODUCT_ID_VRBRAIN;
#else
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
_product_id = AP_PRODUCT_ID_PX4_V2;
#else
_product_id = AP_PRODUCT_ID_PX4;
#endif
#endif
return true;
}
uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate )
{
// Sensors are raw sampled at 800Hz.
// Here we figure the divider to get the rate that update should be called
switch (sample_rate) {
case RATE_50HZ:
_sample_divider = raw_sample_rate_hz / 50;
_default_filter_hz = 10;
break;
case RATE_100HZ:
_sample_divider = raw_sample_rate_hz / 100;
_default_filter_hz = 20;
break;
case RATE_200HZ:
default:
_sample_divider = raw_sample_rate_hz / 200;
_default_filter_hz = 20;
break;
}
// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
// take i2c bus sempahore
if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER))
return false;
// Init the accelerometer
uint8_t data;
hal.i2c->readRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DEVID, &data);
if (data != FLYMAPLE_ACCELEROMETER_XL345_DEVID)
hal.scheduler->panic(PSTR("AP_InertialSensor_Flymaple: could not find ADXL345 accelerometer sensor"));
hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL, 0x00);
hal.scheduler->delay(5);
hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL, 0xff);
hal.scheduler->delay(5);
// Measure mode:
hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL, 0x08);
hal.scheduler->delay(5);
// Full resolution, 8g:
// Caution, this must agree with FLYMAPLE_ACCELEROMETER_SCALE_1G
// In full resoution mode, the scale factor need not change
hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATA_FORMAT, 0x08);
hal.scheduler->delay(5);
// Normal power, 800Hz Output Data Rate, 400Hz bandwidth:
hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_BW_RATE, 0x0d);
hal.scheduler->delay(5);
// Power up default is FIFO bypass mode. FIFO is not used by the chip
// Init the Gyro
// Expect to read the same as the Gyro I2C adress:
hal.i2c->readRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_WHO_AM_I, &data);
if (data != FLYMAPLE_GYRO_ADDRESS)
hal.scheduler->panic(PSTR("AP_InertialSensor_Flymaple: could not find ITG-3200 accelerometer sensor"));
hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_PWR_MGM, 0x00);
hal.scheduler->delay(1);
// Sample rate divider: with 8kHz internal clock (see FLYMAPLE_GYRO_DLPF_FS),
// get 500Hz sample rate, 2 samples
hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_SMPLRT_DIV, 0x0f);
hal.scheduler->delay(1);
// 2000 degrees/sec, 256Hz LPF, 8kHz internal sample rate
// This is the least amount of filtering we can configure for this device
hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_DLPF_FS, 0x18);
hal.scheduler->delay(1);
// No interrupts
hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_INT_CFG, 0x00);
hal.scheduler->delay(1);
// Set up the filter desired
_set_filter_frequency(_mpu6000_filter);
// give back i2c semaphore
i2c_sem->give();
return AP_PRODUCT_ID_FLYMAPLE;
}
bool AP_InertialSensor_L3G4200D::_init_sensor(void)
{
_default_filter_hz = _default_filter();
// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
// take i2c bus sempahore
if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER))
return false;
// Init the accelerometer
uint8_t data = 0;
hal.i2c->readRegister(ADXL345_ACCELEROMETER_ADDRESS, ADXL345_ACCELEROMETER_ADXLREG_DEVID, &data);
if (data != ADXL345_ACCELEROMETER_XL345_DEVID) {
hal.scheduler->panic(PSTR("AP_InertialSensor_L3G4200D: could not find ADXL345 accelerometer sensor"));
}
hal.i2c->writeRegister(ADXL345_ACCELEROMETER_ADDRESS, ADXL345_ACCELEROMETER_ADXLREG_POWER_CTL, 0x00);
hal.scheduler->delay(5);
hal.i2c->writeRegister(ADXL345_ACCELEROMETER_ADDRESS, ADXL345_ACCELEROMETER_ADXLREG_POWER_CTL, 0xff);
hal.scheduler->delay(5);
// Measure mode:
hal.i2c->writeRegister(ADXL345_ACCELEROMETER_ADDRESS, ADXL345_ACCELEROMETER_ADXLREG_POWER_CTL, 0x08);
hal.scheduler->delay(5);
// Full resolution, 8g:
// Caution, this must agree with ADXL345_ACCELEROMETER_SCALE_1G
// In full resoution mode, the scale factor need not change
hal.i2c->writeRegister(ADXL345_ACCELEROMETER_ADDRESS, ADXL345_ACCELEROMETER_ADXLREG_DATA_FORMAT, 0x08);
hal.scheduler->delay(5);
// Normal power, 800Hz Output Data Rate, 400Hz bandwidth:
hal.i2c->writeRegister(ADXL345_ACCELEROMETER_ADDRESS, ADXL345_ACCELEROMETER_ADXLREG_BW_RATE, 0x0d);
hal.scheduler->delay(5);
// enable FIFO in stream mode. This will allow us to read the accelerometers much less frequently
hal.i2c->writeRegister(ADXL345_ACCELEROMETER_ADDRESS,
ADXL345_ACCELEROMETER_ADXLREG_FIFO_CTL,
ADXL345_ACCELEROMETER_ADXLREG_FIFO_CTL_STREAM);
// Init the Gyro
// Expect to read the right 'WHO_AM_I' value
hal.i2c->readRegister(L3G4200D_I2C_ADDRESS, L3G4200D_REG_WHO_AM_I, &data);
if (data != L3G4200D_REG_WHO_AM_I_VALUE)
hal.scheduler->panic(PSTR("AP_InertialSensor_L3G4200D: could not find L3G4200D gyro sensor"));
// setup for 800Hz sampling with 110Hz filter
hal.i2c->writeRegister(L3G4200D_I2C_ADDRESS,
L3G4200D_REG_CTRL_REG1,
L3G4200D_REG_CTRL_REG1_DRBW_800_110 |
L3G4200D_REG_CTRL_REG1_PD |
L3G4200D_REG_CTRL_REG1_XYZ_ENABLE);
hal.scheduler->delay(1);
hal.i2c->writeRegister(L3G4200D_I2C_ADDRESS,
L3G4200D_REG_CTRL_REG1,
L3G4200D_REG_CTRL_REG1_DRBW_800_110 |
L3G4200D_REG_CTRL_REG1_PD |
L3G4200D_REG_CTRL_REG1_XYZ_ENABLE);
hal.scheduler->delay(1);
hal.i2c->writeRegister(L3G4200D_I2C_ADDRESS,
L3G4200D_REG_CTRL_REG1,
L3G4200D_REG_CTRL_REG1_DRBW_800_110 |
L3G4200D_REG_CTRL_REG1_PD |
L3G4200D_REG_CTRL_REG1_XYZ_ENABLE);
hal.scheduler->delay(1);
// setup for 2000 degrees/sec full range
hal.i2c->writeRegister(L3G4200D_I2C_ADDRESS,
L3G4200D_REG_CTRL_REG4,
L3G4200D_REG_CTRL_REG4_FS_2000);
hal.scheduler->delay(1);
// enable FIFO
hal.i2c->writeRegister(L3G4200D_I2C_ADDRESS,
L3G4200D_REG_CTRL_REG5,
L3G4200D_REG_CTRL_REG5_FIFO_EN);
hal.scheduler->delay(1);
// enable FIFO in stream mode. This will allow us to read the gyros much less frequently
hal.i2c->writeRegister(L3G4200D_I2C_ADDRESS,
L3G4200D_REG_FIFO_CTL,
L3G4200D_REG_FIFO_CTL_STREAM);
hal.scheduler->delay(1);
// Set up the filter desired
_set_filter_frequency(_imu.get_filter());
// give back i2c semaphore
i2c_sem->give();
// start the timer process to read samples
hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_L3G4200D::_accumulate));
_gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel();
_product_id = AP_PRODUCT_ID_L3G4200D;
return true;
}
