Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor_L3G4200D: a sample is only available if gyro had a sample

This commit is contained in:
Andrew Tridgell 2013-10-08 12:31:20 +11:00
parent eef966c5fb
commit 6444b0bddd
1 changed files with 13 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View File

@ -324,18 +324,22 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
num_samples_available = fifo_status & L3G4200D_REG_FIFO_SRC_ENTRIES_MASK;
}
// read all the entries in one go, using AUTO_INCREMENT. This saves a lot of time on I2C setup
int16_t buffer[num_samples_available][3];
if (hal.i2c->readRegisters(L3G4200D_I2C_ADDRESS, L3G4200D_REG_XL | L3G4200D_REG_AUTO_INCREMENT,
sizeof(buffer), (uint8_t *)&buffer[0][0]) == 0) {
for (uint8_t i=0; i<num_samples_available; i++) {
_gyro_filtered = Vector3f(_gyro_filter_x.apply(buffer[i][0]),
_gyro_filter_y.apply(-buffer[i][1]),
_gyro_filter_z.apply(-buffer[i][2]));
if (num_samples_available > 0) {
// read all the entries in one go, using AUTO_INCREMENT. This saves a lot of time on I2C setup
int16_t buffer[num_samples_available][3];
if (hal.i2c->readRegisters(L3G4200D_I2C_ADDRESS, L3G4200D_REG_XL | L3G4200D_REG_AUTO_INCREMENT,
sizeof(buffer), (uint8_t *)&buffer[0][0]) == 0) {
for (uint8_t i=0; i<num_samples_available; i++) {
_gyro_filtered = Vector3f(_gyro_filter_x.apply(buffer[i][0]),
_gyro_filter_y.apply(-buffer[i][1]),
_gyro_filter_z.apply(-buffer[i][2]));
}
}
}
_sample_available = true;
if (num_samples_available > 0) {
_sample_available = true;
}
// give back i2c semaphore
i2c_sem->give();