aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: Improvements to Flymaple sensors

Browse Source 
Fix a bug that caused hang after 71 minutes. Use I2CDriver semaphore.
Remove test for in_accumulate: not needed.
This commit is contained in:
Mike McCauley 2013-10-03 17:40:24 +10:00 committed by Andrew Tridgell
parent 39f9b7bbc9
commit c90c1b9998
2 changed files with 19 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
View File

@ -33,7 +33,6 @@ Vector3f AP_InertialSensor_Flymaple::_accel_filtered;
uint32_t AP_InertialSensor_Flymaple::_accel_samples; uint32_t AP_InertialSensor_Flymaple::_accel_samples;
Vector3f AP_InertialSensor_Flymaple::_gyro_filtered; Vector3f AP_InertialSensor_Flymaple::_gyro_filtered;
uint32_t AP_InertialSensor_Flymaple::_gyro_samples; uint32_t AP_InertialSensor_Flymaple::_gyro_samples;
volatile bool AP_InertialSensor_Flymaple::_in_accumulate;
uint64_t AP_InertialSensor_Flymaple::_last_accel_timestamp; uint64_t AP_InertialSensor_Flymaple::_last_accel_timestamp;
uint64_t AP_InertialSensor_Flymaple::_last_gyro_timestamp; uint64_t AP_InertialSensor_Flymaple::_last_gyro_timestamp;
LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_x(800, 10); LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_x(800, 10);
@ -98,6 +97,13 @@ uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate )
break; 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 // Init the accelerometer
uint8_t data; uint8_t data;
hal.i2c->readRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DEVID, &data); hal.i2c->readRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DEVID, &data);
@ -142,6 +148,9 @@ uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate )
// Set up the filter desired // Set up the filter desired
_set_filter_frequency(_mpu6000_filter); _set_filter_frequency(_mpu6000_filter);
// give back i2c semaphore
i2c_sem->give();
return AP_PRODUCT_ID_FLYMAPLE; return AP_PRODUCT_ID_FLYMAPLE;
} }
@ -239,18 +248,19 @@ float AP_InertialSensor_Flymaple::get_gyro_drift_rate(void)
// within the mainline and thropttle the reads here to suit the sensors // within the mainline and thropttle the reads here to suit the sensors
void AP_InertialSensor_Flymaple::_accumulate(void) void AP_InertialSensor_Flymaple::_accumulate(void)
{ {
if (_in_accumulate) { // get pointer to i2c bus semaphore
return; // Dont be re-entrant (how can this occur?) AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
}
_in_accumulate = true;
// take i2c bus sempahore
if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER))
return;
// Read accelerometer // Read accelerometer
// ADXL345 is in the default FIFO bypass mode, so the FIFO is not used // ADXL345 is in the default FIFO bypass mode, so the FIFO is not used
uint8_t buffer[6]; uint8_t buffer[6];
uint64_t now = hal.scheduler->micros(); uint64_t now = hal.scheduler->micros();
// This takes about 500us at 200kHz I2C speed // This takes about 500us at 200kHz I2C speed
if (now > (_last_accel_timestamp + raw_sample_interval_us) if ((now - _last_accel_timestamp) >= raw_sample_interval_us
&& hal.i2c->readRegisters(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0, 6, buffer) == 0) && hal.i2c->readRegisters(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0, 6, buffer) == 0)
{ {
// The order is a bit wierd here since the standard we have adopted for Flymaple // The order is a bit wierd here since the standard we have adopted for Flymaple
@ -270,7 +280,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
// Read gyro // Read gyro
now = hal.scheduler->micros(); now = hal.scheduler->micros();
// This takes about 500us at 200kHz I2C speed // This takes about 500us at 200kHz I2C speed
if (now > (_last_gyro_timestamp + raw_sample_interval_us) if ((now - _last_gyro_timestamp) >= raw_sample_interval_us
&& hal.i2c->readRegisters(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_GYROX_H, 6, buffer) == 0) && hal.i2c->readRegisters(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_GYROX_H, 6, buffer) == 0)
{ {
// See above re order of samples in buffer // See above re order of samples in buffer
@ -284,8 +294,8 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
_last_gyro_timestamp = now; _last_gyro_timestamp = now;
} }
_in_accumulate = false; // give back i2c semaphore
i2c_sem->give();
} }
bool AP_InertialSensor_Flymaple::sample_available(void) bool AP_InertialSensor_Flymaple::sample_available(void)

1
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h
View File

@ -33,7 +33,6 @@ private:
static uint32_t _accel_samples; static uint32_t _accel_samples;
static Vector3f _gyro_filtered; static Vector3f _gyro_filtered;
static uint32_t _gyro_samples; static uint32_t _gyro_samples;
static volatile bool _in_accumulate;
static uint64_t _last_accel_timestamp; static uint64_t _last_accel_timestamp;
static uint64_t _last_gyro_timestamp; static uint64_t _last_gyro_timestamp;
uint8_t _sample_divider; uint8_t _sample_divider;