From 03036c632d227422d24ff558872fb376df509da3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 28 Sep 2013 16:29:24 +1000 Subject: [PATCH] AP_InertialSensor: use new scheduler API --- .../AP_InertialSensor_MPU6000.cpp | 28 +++++++------------ .../AP_InertialSensor_MPU6000.h | 24 ++++++++-------- 2 files changed, 23 insertions(+), 29 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index d7f1e34f17..add6e484fd 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -169,17 +169,6 @@ const int8_t AP_InertialSensor_MPU6000::_accel_data_sign[3] = { 1, 1, -1 }; const uint8_t AP_InertialSensor_MPU6000::_temp_data_index = 3; -int16_t AP_InertialSensor_MPU6000::_mpu6000_product_id = AP_PRODUCT_ID_NONE; -AP_HAL::DigitalSource *AP_InertialSensor_MPU6000::_drdy_pin = NULL; - -// time we start collecting sample (reset on update) -// time latest sample was collected -static volatile uint32_t _last_sample_time_micros = 0; - -/* Static SPI device driver */ -AP_HAL::SPIDeviceDriver* AP_InertialSensor_MPU6000::_spi = NULL; -AP_HAL::Semaphore* AP_InertialSensor_MPU6000::_spi_sem = NULL; - /* * RM-MPU-6000A-00.pdf, page 31, section 4.23 lists LSB sensitivity of * accel as 4096 LSB/mg at scale factor of +/- 8g (AFS_SEL==2) @@ -188,10 +177,13 @@ AP_HAL::Semaphore* AP_InertialSensor_MPU6000::_spi_sem = NULL; * variants however */ -AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000() : AP_InertialSensor() +AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000() : + AP_InertialSensor(), + _mpu6000_product_id(AP_PRODUCT_ID_NONE), + _drdy_pin(NULL), + _temp(0), + _initialised(false) { - _temp = 0; - _initialised = false; } uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate ) @@ -236,7 +228,7 @@ uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate ) _read_data_transaction(); // start the timer process to read samples - hal.scheduler->register_timer_process(_poll_data); + hal.scheduler->register_timer_process(reinterpret_cast(&AP_InertialSensor_MPU6000::_poll_data), this); #if MPU6000_DEBUG _dump_registers(); @@ -362,7 +354,7 @@ bool AP_InertialSensor_MPU6000::_data_ready() /** * Timer process to poll for new data from the MPU6000. */ -void AP_InertialSensor_MPU6000::_poll_data(uint32_t now) +void AP_InertialSensor_MPU6000::_poll_data(void) { if (_data_ready()) { if (hal.scheduler->in_timerprocess()) { @@ -602,7 +594,7 @@ float AP_InertialSensor_MPU6000::get_gyro_drift_rate(void) // return true if a sample is available bool AP_InertialSensor_MPU6000::sample_available() { - _poll_data(0); + _poll_data(); return (_count >> _sample_shift) > 0; } @@ -629,4 +621,4 @@ float AP_InertialSensor_MPU6000::get_delta_time() { // the sensor runs at 200Hz return 0.005 * _num_samples; -} \ No newline at end of file +} diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index 79e07dde11..96b9333d9e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -35,19 +35,19 @@ protected: private: - static void _read_data_from_timerprocess(); - static void _read_data_transaction(); - static bool _data_ready(); - static void _poll_data(uint32_t now); - static AP_HAL::DigitalSource *_drdy_pin; - static uint8_t _register_read( uint8_t reg ); - static bool _register_read_from_timerprocess( uint8_t reg, uint8_t *val ); - static void register_write( uint8_t reg, uint8_t val ); + void _read_data_from_timerprocess(); + void _read_data_transaction(); + bool _data_ready(); + void _poll_data(void); + AP_HAL::DigitalSource *_drdy_pin; + uint8_t _register_read( uint8_t reg ); + bool _register_read_from_timerprocess( uint8_t reg, uint8_t *val ); + void register_write( uint8_t reg, uint8_t val ); void wait_for_sample(); bool hardware_init(Sample_rate sample_rate); - static AP_HAL::SPIDeviceDriver *_spi; - static AP_HAL::Semaphore *_spi_sem; + AP_HAL::SPIDeviceDriver *_spi; + AP_HAL::Semaphore *_spi_sem; uint16_t _num_samples; @@ -65,9 +65,11 @@ private: static const uint8_t _temp_data_index; + uint32_t _last_sample_time_micros; + // ensure we can't initialise twice bool _initialised; - static int16_t _mpu6000_product_id; + int16_t _mpu6000_product_id; // how many hardware samples before we report a sample to the caller uint8_t _sample_shift;