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

InertialSensor: simplify the data_ready interrupt handling

Browse Source 
this avoids using the defer process code, and fixes a nasty bug that
caused the APM to lockup on reset
This commit is contained in:
Andrew Tridgell 2012-12-21 20:01:19 +11:00
parent c5ba33d39a
commit 9548e7e79e
2 changed files with 18 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -191,6 +191,9 @@ static volatile uint32_t _delta_time_start_micros = 0;
// time latest sample was collected // time latest sample was collected
static volatile uint32_t _last_sample_time_micros = 0; static volatile uint32_t _last_sample_time_micros = 0;
// data ready interrupt fired
volatile bool AP_InertialSensor_MPU6000::_data_ready;
// DMP related static variables // DMP related static variables
bool AP_InertialSensor_MPU6000::_dmp_initialised = false; bool AP_InertialSensor_MPU6000::_dmp_initialised = false;
// high byte of number of elements in fifo buffer // high byte of number of elements in fifo buffer
@ -247,7 +250,7 @@ bool AP_InertialSensor_MPU6000::update( void )
// wait for at least 1 sample // wait for at least 1 sample
uint32_t tstart = hal.scheduler->micros(); uint32_t tstart = hal.scheduler->micros();
while (_count == 0) { while (num_samples_available() == 0) {
if (hal.scheduler->micros() - tstart > 50000) { if (hal.scheduler->micros() - tstart > 50000) {
hal.scheduler->panic( hal.scheduler->panic(
PSTR("PANIC: AP_InertialSensor_MPU6000::update " PSTR("PANIC: AP_InertialSensor_MPU6000::update "
@ -303,7 +306,7 @@ float AP_InertialSensor_MPU6000::temperature() {
* these other devices must perform their spi reads after being called by the * these other devices must perform their spi reads after being called by the
* AP_TimerProcess. * AP_TimerProcess.
*/ */
void AP_InertialSensor_MPU6000::read(uint32_t) void AP_InertialSensor_MPU6000::read_data(void)
{ {
static int semfail_ctr = 0; static int semfail_ctr = 0;
if (_spi_sem) { if (_spi_sem) {
@ -380,11 +383,9 @@ void AP_InertialSensor_MPU6000::register_write(uint8_t reg, uint8_t val)
// MPU6000 new data interrupt on INT6 // MPU6000 new data interrupt on INT6
void AP_InertialSensor_MPU6000::data_interrupt(void) void AP_InertialSensor_MPU6000::data_interrupt(void)
{ {
_data_ready = true;
// record time that data was available // record time that data was available
_last_sample_time_micros = hal.scheduler->micros(); _last_sample_time_micros = hal.scheduler->micros();
// queue our read process to run after any currently running timed
// processes complete
hal.scheduler->defer_timer_process( AP_InertialSensor_MPU6000::read );
} }
void AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate) void AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate)
@ -506,6 +507,14 @@ float AP_InertialSensor_MPU6000::get_gyro_drift_rate(void)
// get number of samples read from the sensors // get number of samples read from the sensors
uint16_t AP_InertialSensor_MPU6000::num_samples_available() uint16_t AP_InertialSensor_MPU6000::num_samples_available()
{ {
if (_data_ready) {
// a data interrupt has occurred - read the data.
// Note that doing it this way means we doing the read out of
// interrupt context, called from the main loop. This avoids
// all possible conflicts with the DataFlash SPI bus
read_data();
_data_ready = false;
}
return _count; return _count;
} }

5
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -49,7 +49,7 @@ protected:
private: private:
static void read(uint32_t); static void read_data();
static void data_interrupt(void); static void data_interrupt(void);
static uint8_t register_read( uint8_t reg ); static uint8_t register_read( uint8_t reg );
static void register_write( uint8_t reg, uint8_t val ); static void register_write( uint8_t reg, uint8_t val );
@ -58,6 +58,9 @@ private:
static AP_HAL::SPIDeviceDriver *_spi; static AP_HAL::SPIDeviceDriver *_spi;
static AP_HAL::Semaphore *_spi_sem; static AP_HAL::Semaphore *_spi_sem;
// set to true when data interrupt from MPU6k fires
static volatile bool _data_ready;
float _temp; float _temp;
float _temp_to_celsius( uint16_t ); float _temp_to_celsius( uint16_t );