/**
* Timer process to poll for new data from the MPU6000.
*/
void AP_InertialSensor_MPU6000::_poll_data(void)
{
if (hal.scheduler->in_timerprocess()) {
if (!_spi_sem->take_nonblocking()) {
/*
the semaphore being busy is an expected condition when the
mainline code is calling wait_for_sample() which will
grab the semaphore. We return now and rely on the mainline
code grabbing the latest sample.
*/
return;
}
if (_data_ready()) {
_last_sample_time_micros = hal.scheduler->micros();
_read_data_transaction();
}
_spi_sem->give();
} else {
/* Synchronous read - take semaphore */
if (_spi_sem->take(10)) {
if (_data_ready()) {
_last_sample_time_micros = hal.scheduler->micros();
_read_data_transaction();
}
_spi_sem->give();
} else {
hal.scheduler->panic(
PSTR("PANIC: AP_InertialSensor_MPU6000::_poll_data "
"failed to take SPI semaphore synchronously"));
}
}
}
uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate )
{
if (_initialised) return _mpu6000_product_id;
_initialised = true;
_spi = hal.spi->device(AP_HAL::SPIDevice_MPU6000);
_spi_sem = _spi->get_semaphore();
/* Pin 70 defined especially to hook
up PE6 to the hal.gpio abstraction.
(It is not a valid pin under Arduino.) */
_drdy_pin = hal.gpio->channel(70);
hal.scheduler->suspend_timer_procs();
uint8_t tries = 0;
do {
bool success = _hardware_init(sample_rate);
if (success) {
hal.scheduler->delay(5+2);
if (!_spi_sem->take(100)) {
hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore"));
}
if (_data_ready()) {
_spi_sem->give();
break;
} else {
hal.console->println_P(
PSTR("MPU6000 startup failed: no data ready"));
}
_spi_sem->give();
}
if (tries++ > 5) {
hal.scheduler->panic(PSTR("PANIC: failed to boot MPU6000 5 times"));
}
} while (1);
hal.scheduler->resume_timer_procs();
/* read the first lot of data.
* _read_data_transaction requires the spi semaphore to be taken by
* its caller. */
_last_sample_time_micros = hal.scheduler->micros();
hal.scheduler->delay(10);
if (_spi_sem->take(100)) {
_read_data_transaction();
_spi_sem->give();
}
// start the timer process to read samples
hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU6000::_poll_data));
#if MPU6000_DEBUG
_dump_registers();
#endif
return _mpu6000_product_id;
}
/**
* Timer process to poll for new data from the MPU6000.
*/
void AP_InertialSensor_MPU6000::_poll_data(void)
{
if (!_spi_sem->take_nonblocking()) {
return;
}
if (_data_ready()) {
_read_data_transaction();
}
_spi_sem->give();
}
/**
* Timer process to poll for new data from the MPU9250.
*/
void AP_InertialSensor_MPU9250::_poll_data(void)
{
if (!_spi_sem->take_nonblocking()) {
/*
the semaphore being busy is an expected condition when the
mainline code is calling wait_for_sample() which will
grab the semaphore. We return now and rely on the mainline
code grabbing the latest sample.
*/
return;
}
_read_data_transaction();
_spi_sem->give();
}
void AP_InertialSensor_MPU6000_I2C::_read_data_from_timerprocess()
{
if (!_i2c_sem->take_nonblocking()) {
/*
the semaphore being busy is an expected condition when the
mainline code is calling sample_available() which will
grab the semaphore. We return now and rely on the mainline
code grabbing the latest sample.
*/
return;
}
_read_data_transaction();
_i2c_sem->give();
}
uint16_t AP_InertialSensor_MPU6000_I2C::_init_sensor(Sample_rate sample_rate )
{
if (_initialised) return _mpu6000_product_id;
_initialised = true;
_i2c_sem = hal.i2c->get_semaphore();
hal.scheduler->suspend_timer_procs();
uint8_t tries = 0;
do {
bool success = hardware_init(sample_rate);
if (success) {
break;
} else {
hal.scheduler->delay(50); // delay for 50ms
hal.console->println_P(
PSTR("MPU6000 init failed"));
}
if (tries++ > 5) {
hal.scheduler->panic(PSTR("PANIC: failed to boot MPU6000 5 times"));
}
} while (1);
hal.scheduler->resume_timer_procs();
/* read the first lot of data.
* _read_data_transaction requires the spi semaphore to be taken by
* its caller. */
_ins_timer = hal.scheduler->micros();
_read_data_transaction();
hal.gpio->pinMode(46, GPIO_OUTPUT); // Debug output
hal.gpio->write(46,0);
hal.gpio->pinMode(45, GPIO_OUTPUT); // Debug output
hal.gpio->write(45,0);
// start the timer process to read samples
hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU6000_I2C::_poll_data));
return _mpu6000_product_id;
}
/*================ HARDWARE FUNCTIONS ==================== */
void AP_InertialSensor_ITG3200::_read_data_from_timerprocess()
{
static uint8_t semfail_ctr = 0;
bool got = _i2c_sem->take_nonblocking();
if (!got) {
semfail_ctr++;
if (semfail_ctr > 100) {
hal.scheduler->panic(PSTR("PANIC: failed to take I2C semaphore "
"100 times in AP_InertialSensor_ITG3200::"
"_read_data_from_timerprocess"));
}
return;
} else {
semfail_ctr = 0;
}
_read_data_transaction();
_i2c_sem->give();
}
/**
* Timer process to poll for new data from the MPU6000.
*/
void AP_InertialSensor_MPU6000::_poll_data(void)
{
if (_data_ready()) {
if (hal.scheduler->in_timerprocess()) {
_read_data_from_timerprocess();
} else {
/* Synchronous read - take semaphore */
bool got = _spi_sem->take(10);
if (got) {
_last_sample_time_micros = hal.scheduler->micros();
_read_data_transaction();
_spi_sem->give();
} else {
hal.scheduler->panic(
PSTR("PANIC: AP_InertialSensor_MPU6000::_poll_data "
"failed to take SPI semaphore synchronously"));
}
}
}
}
void AP_InertialSensor_ITG3200::_poll_data(uint32_t now)
{
if (now - _ins_timer > _micros_per_sample) {
_ins_timer = now;
if (hal.scheduler->in_timerprocess()) {
_read_data_from_timerprocess();
} else {
/* Synchronous read - take semaphore */
bool got = _i2c_sem->take(10);
if (got) {
_read_data_transaction();
_i2c_sem->give();
} else {
hal.scheduler->panic(
PSTR("PANIC: AP_InertialSensor_ITG3200::_poll_data "
"failed to take I2C semaphore synchronously"));
}
}
}
}
void AP_InertialSensor_MPU6000_I2C::_poll_data(void)
{
hal.gpio->write(46,1);
uint32_t now = hal.scheduler->micros();
if ( (now - _ins_timer > _micros_per_sample) || (_sens_stage == 1)) {
// hal.gpio->write(46,1);
if (hal.scheduler->in_timerprocess()) {
bool _stage = _sens_stage;
_read_data_from_timerprocess();
if (_stage != _sens_stage && _stage == 0) {
_ins_timer = now;
}
} else {
/* Synchronous read - take semaphore */
bool got = _i2c_sem->take(10);
if (got) {
if (_sens_stage == 0) {
_ins_timer = now;
}
_read_data_transaction();
_i2c_sem->give();
} else {
hal.scheduler->panic(
PSTR("PANIC: AP_InertialSensor_MPU6000::_poll_data "
"failed to take I2C semaphore synchronously"));
}
}
}
hal.gpio->write(46,0);
}
uint16_t AP_InertialSensor_L3GD20::_init_sensor( Sample_rate sample_rate )
{
if (_initialised) return _L3GD20_product_id;
_initialised = true;
_spi = hal.spi->device(AP_HAL::SPIDevice_L3GD20);
_spi_sem = _spi->get_semaphore();
#ifdef L3GD20_DRDY_PIN
_drdy_pin = hal.gpio->channel(L3GD20_DRDY_PIN);
_drdy_pin->mode(HAL_GPIO_INPUT);
#endif
hal.scheduler->suspend_timer_procs();
// Test WHOAMI
uint8_t whoami = _register_read(ADDR_WHO_AM_I);
if (whoami != WHO_I_AM) {
// TODO: we should probably accept multiple chip
// revisions. This is the one on the PXF
hal.console->printf("L3GD20: unexpected WHOAMI 0x%x\n", (unsigned)whoami);
hal.scheduler->panic(PSTR("L3GD20: bad WHOAMI"));
}
uint8_t tries = 0;
do {
bool success = _hardware_init(sample_rate);
if (success) {
hal.scheduler->delay(5+2);
if (!_spi_sem->take(100)) {
hal.scheduler->panic(PSTR("L3GD20: Unable to get semaphore"));
}
if (_data_ready()) {
_spi_sem->give();
break;
} else {
hal.console->println_P(
PSTR("L3GD20 startup failed: no data ready"));
}
_spi_sem->give();
}
if (tries++ > 5) {
hal.scheduler->panic(PSTR("PANIC: failed to boot L3GD20 5 times"));
}
} while (1);
hal.scheduler->resume_timer_procs();
/* read the first lot of data.
* _read_data_transaction requires the spi semaphore to be taken by
* its caller. */
_last_sample_time_micros = hal.scheduler->micros();
hal.scheduler->delay(10);
if (_spi_sem->take(100)) {
_read_data_transaction();
_spi_sem->give();
}
// start the timer process to read samples
hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_L3GD20::_poll_data));
#if L3GD20_DEBUG
_dump_registers();
#endif
return _L3GD20_product_id;
}
/*
initialise the sensor
*/
uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate )
{
if (_initialised) return _mpu9250_product_id;
_initialised = true;
_spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250);
_spi_sem = _spi->get_semaphore();
// we need to suspend timers to prevent other SPI drivers grabbing
// the bus while we do the long initialisation
hal.scheduler->suspend_timer_procs();
uint8_t whoami = _register_read(MPUREG_WHOAMI);
if (whoami != 0x71) {
// TODO: we should probably accept multiple chip
// revisions. This is the one on the PXF
hal.console->printf("MPU9250: unexpected WHOAMI 0x%x\n", (unsigned)whoami);
hal.scheduler->panic("MPU9250: bad WHOAMI");
}
uint8_t tries = 0;
do {
bool success = _hardware_init(sample_rate);
if (success) {
hal.scheduler->delay(10);
if (!_spi_sem->take(100)) {
hal.scheduler->panic(PSTR("MPU9250: Unable to get semaphore"));
}
uint8_t status = _register_read(MPUREG_INT_STATUS);
if ((status & BIT_RAW_RDY_INT) != 0) {
_spi_sem->give();
break;
} else {
hal.console->println_P(
PSTR("MPU9250 startup failed: no data ready"));
}
_spi_sem->give();
}
if (tries++ > 5) {
hal.scheduler->panic(PSTR("PANIC: failed to boot MPU9250 5 times"));
}
} while (1);
hal.scheduler->resume_timer_procs();
/* read the first lot of data.
* _read_data_transaction requires the spi semaphore to be taken by
* its caller. */
hal.scheduler->delay(10);
if (_spi_sem->take(100)) {
_read_data_transaction();
_spi_sem->give();
}
// start the timer process to read samples
hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU9250::_poll_data));
#if MPU9250_DEBUG
_dump_registers();
#endif
return _mpu9250_product_id;
}
