Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: use new scheduler API

Browse Source
This commit is contained in:
Andrew Tridgell 2013-09-28 16:29:24 +10:00
parent 7ec242146d
commit 03036c632d
2 changed files with 23 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -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_HAL::TimedProc>(&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;
}
}

24
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -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;