mirror of https://github.com/ArduPilot/ardupilot
AP_InertialSensor: get rid of data_ready for MPU9250
when running off a 1kHz timer and reading at 1kHz it is better to double sample than it is to skip a sample, as skipping samples will throw the filter off, whereas a double sample will have minimal effect
This commit is contained in:
parent
a627cd2af2
commit
3c2ab31415
|
@ -174,7 +174,6 @@ extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250() :
|
AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250() :
|
||||||
AP_InertialSensor(),
|
AP_InertialSensor(),
|
||||||
_drdy_pin(NULL),
|
|
||||||
_initialised(false),
|
_initialised(false),
|
||||||
_mpu9250_product_id(AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE),
|
_mpu9250_product_id(AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE),
|
||||||
_last_filter_hz(-1),
|
_last_filter_hz(-1),
|
||||||
|
@ -198,11 +197,6 @@ uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate )
|
||||||
_spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250);
|
_spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250);
|
||||||
_spi_sem = _spi->get_semaphore();
|
_spi_sem = _spi->get_semaphore();
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLE
|
|
||||||
_drdy_pin = hal.gpio->channel(BBB_P8_7);
|
|
||||||
_drdy_pin->mode(HAL_GPIO_INPUT);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
hal.scheduler->suspend_timer_procs();
|
hal.scheduler->suspend_timer_procs();
|
||||||
|
|
||||||
uint8_t whoami = _register_read(MPUREG_WHOAMI);
|
uint8_t whoami = _register_read(MPUREG_WHOAMI);
|
||||||
|
@ -221,7 +215,8 @@ uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate )
|
||||||
if (!_spi_sem->take(100)) {
|
if (!_spi_sem->take(100)) {
|
||||||
hal.scheduler->panic(PSTR("MPU9250: Unable to get semaphore"));
|
hal.scheduler->panic(PSTR("MPU9250: Unable to get semaphore"));
|
||||||
}
|
}
|
||||||
if (_data_ready()) {
|
uint8_t status = _register_read(MPUREG_INT_STATUS);
|
||||||
|
if ((status & BIT_RAW_RDY_INT) != 0) {
|
||||||
_spi_sem->give();
|
_spi_sem->give();
|
||||||
break;
|
break;
|
||||||
} else {
|
} else {
|
||||||
|
@ -344,22 +339,6 @@ bool AP_InertialSensor_MPU9250::update( void )
|
||||||
|
|
||||||
/*================ HARDWARE FUNCTIONS ==================== */
|
/*================ HARDWARE FUNCTIONS ==================== */
|
||||||
|
|
||||||
/**
|
|
||||||
* Return true if the MPU9250 has new data available for reading.
|
|
||||||
*
|
|
||||||
* We use the data ready pin if it is available. Otherwise, read the
|
|
||||||
* status register.
|
|
||||||
*/
|
|
||||||
bool AP_InertialSensor_MPU9250::_data_ready()
|
|
||||||
{
|
|
||||||
if (_drdy_pin) {
|
|
||||||
return _drdy_pin->read() != 0;
|
|
||||||
}
|
|
||||||
uint8_t status = _register_read(MPUREG_INT_STATUS);
|
|
||||||
return (status & BIT_RAW_RDY_INT) != 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Timer process to poll for new data from the MPU9250.
|
* Timer process to poll for new data from the MPU9250.
|
||||||
*/
|
*/
|
||||||
|
@ -374,9 +353,7 @@ void AP_InertialSensor_MPU9250::_poll_data(void)
|
||||||
*/
|
*/
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
if (_data_ready()) {
|
|
||||||
_read_data_transaction();
|
_read_data_transaction();
|
||||||
}
|
|
||||||
_spi_sem->give();
|
_spi_sem->give();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -395,13 +372,6 @@ void AP_InertialSensor_MPU9250::_read_data_transaction()
|
||||||
|
|
||||||
_spi->transaction((const uint8_t *)&tx, (uint8_t *)&rx, sizeof(rx));
|
_spi->transaction((const uint8_t *)&tx, (uint8_t *)&rx, sizeof(rx));
|
||||||
|
|
||||||
if (_drdy_pin) {
|
|
||||||
if (_drdy_pin->read() != 0) {
|
|
||||||
// data ready should have gone low after a read
|
|
||||||
printf("MPU9250: DRDY didn't go low\n");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
|
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
|
||||||
|
|
||||||
_accel_filtered = Vector3f(_accel_filter_x.apply(int16_val(rx.v, 1)),
|
_accel_filtered = Vector3f(_accel_filter_x.apply(int16_val(rx.v, 1)),
|
||||||
|
|
|
@ -32,7 +32,6 @@ public:
|
||||||
|
|
||||||
private:
|
private:
|
||||||
uint16_t _init_sensor( Sample_rate sample_rate );
|
uint16_t _init_sensor( Sample_rate sample_rate );
|
||||||
AP_HAL::DigitalSource *_drdy_pin;
|
|
||||||
|
|
||||||
void _read_data_transaction();
|
void _read_data_transaction();
|
||||||
bool _data_ready();
|
bool _data_ready();
|
||||||
|
|
Loading…
Reference in New Issue