diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp index 636438d591..cb9619b456 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp @@ -13,22 +13,11 @@ along with this program. If not, see . */ /* - This is an INS driver for the combination L3G4200D gyro and ADXL345 accelerometer. - This combination is available as a cheap 10DOF sensor on ebay - - This sensor driver is an example only - it should not be used in any - serious autopilot as the latencies on I2C prevent good timing at - high sample rates. It is useful when doing an initial port of - ardupilot to a board where only I2C is available, and a cheap sensor - can be used. - -Datasheets: - ADXL345 Accelerometer http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf - L3G4200D gyro http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00265057.pdf -*/ + IMU driver for Robsense PhenixPro Devkit board including i3g4250d and iis328dq + */ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RST_ZYNQ #include "AP_InertialSensor_RST.h" #include @@ -39,150 +28,151 @@ Datasheets: const extern AP_HAL::HAL &hal; -#define ADDR_INCREMENT (1<<6) +#define ADDR_INCREMENT (1<<6) /************************************iis328dq register addresses *******************************************/ -#define ACCEL_WHO_AM_I 0x0F -#define ACCEL_WHO_I_AM 0x32 +#define ACCEL_WHO_AM_I 0x0F +#define ACCEL_WHO_I_AM 0x32 -#define ACCEL_CTRL_REG1 0x20 +#define ACCEL_CTRL_REG1 0x20 /* keep lowpass low to avoid noise issues */ -#define RATE_50HZ_LP_37HZ (0<<4) | (0<<3) -#define RATE_100HZ_LP_74HZ (0<<4) | (1<<3) -#define RATE_400HZ_LP_292HZ (1<<4) | (0<<3) -#define RATE_1000HZ_LP_780HZ (1<<4) | (1<<3) - -#define ACCEL_CTRL_REG2 0x21 -#define ACCEL_CTRL_REG3 0x22 -#define ACCEL_CTRL_REG4 0x23 - -#define ACCEL_CTRL_REG5 0x24 -#define ACCEL_HP_FILTER_RESETE 0x25 -#define ACCEL_OUT_REFERENCE 0x26 -#define ACCEL_STATUS_REG 0x27 -#define ACCEL_OUT_X_L 0x28 -#define ACCEL_OUT_X_H 0x29 -#define ACCEL_OUT_Y_L 0x2A -#define ACCEL_OUT_Y_H 0x2B -#define ACCEL_OUT_Z_L 0x2C -#define ACCEL_OUT_Z_H 0x2D -#define ACCEL_INT1_CFG 0x30 -#define ACCEL_INT1_SRC 0x31 -#define ACCEL_INT1_TSH 0x32 -#define ACCEL_INT1_DURATION 0x33 -#define ACCEL_INT2_CFG 0x34 -#define ACCEL_INT2_SRC 0x35 -#define ACCEL_INT2_TSH 0x36 -#define ACCEL_INT2_DURATION 0x37 +#define RATE_50HZ_LP_37HZ (0<<4) | (0<<3) +#define RATE_100HZ_LP_74HZ (0<<4) | (1<<3) +#define RATE_400HZ_LP_292HZ (1<<4) | (0<<3) +#define RATE_1000HZ_LP_780HZ (1<<4) | (1<<3) + +#define ACCEL_CTRL_REG2 0x21 +#define ACCEL_CTRL_REG3 0x22 +#define ACCEL_CTRL_REG4 0x23 + +#define ACCEL_CTRL_REG5 0x24 +#define ACCEL_HP_FILTER_RESETE 0x25 +#define ACCEL_OUT_REFERENCE 0x26 +#define ACCEL_STATUS_REG 0x27 +#define ACCEL_OUT_X_L 0x28 +#define ACCEL_OUT_X_H 0x29 +#define ACCEL_OUT_Y_L 0x2A +#define ACCEL_OUT_Y_H 0x2B +#define ACCEL_OUT_Z_L 0x2C +#define ACCEL_OUT_Z_H 0x2D +#define ACCEL_INT1_CFG 0x30 +#define ACCEL_INT1_SRC 0x31 +#define ACCEL_INT1_TSH 0x32 +#define ACCEL_INT1_DURATION 0x33 +#define ACCEL_INT2_CFG 0x34 +#define ACCEL_INT2_SRC 0x35 +#define ACCEL_INT2_TSH 0x36 +#define ACCEL_INT2_DURATION 0x37 /* Internal configuration values */ -#define ACCEL_REG1_POWER_NORMAL ((0<<7) | (0<<6) | (1<<5)) -#define ACCEL_REG1_Z_ENABLE (1<<2) -#define ACCEL_REG1_Y_ENABLE (1<<1) -#define ACCEL_REG1_X_ENABLE (1<<0) - -#define ACCEL_REG4_BDU (1<<7) -#define ACCEL_REG4_BLE (1<<6) -#define ACCEL_REG4_FULL_SCALE_BITS ((1<<5) | (1<<4)) -#define ACCEL_REG4_FULL_SCALE_2G ((0<<5) | (0<<4)) -#define ACCEL_REG4_FULL_SCALE_4G ((0<<5) | (1<<4)) -#define ACCEL_REG4_FULL_SCALE_8G ((1<<5) | (1<<4)) - -#define ACCEL_STATUS_ZYXOR (1<<7) -#define ACCEL_STATUS_ZOR (1<<6) -#define ACCEL_STATUS_YOR (1<<5) -#define ACCEL_STATUS_XOR (1<<4) -#define ACCEL_STATUS_ZYXDA (1<<3) -#define ACCEL_STATUS_ZDA (1<<2) -#define ACCEL_STATUS_YDA (1<<1) -#define ACCEL_STATUS_XDA (1<<0) +#define ACCEL_REG1_POWER_NORMAL ((0<<7) | (0<<6) | (1<<5)) +#define ACCEL_REG1_Z_ENABLE (1<<2) +#define ACCEL_REG1_Y_ENABLE (1<<1) +#define ACCEL_REG1_X_ENABLE (1<<0) + +#define ACCEL_REG4_BDU (1<<7) +#define ACCEL_REG4_BLE (1<<6) +#define ACCEL_REG4_FULL_SCALE_BITS ((1<<5) | (1<<4)) +#define ACCEL_REG4_FULL_SCALE_2G ((0<<5) | (0<<4)) +#define ACCEL_REG4_FULL_SCALE_4G ((0<<5) | (1<<4)) +#define ACCEL_REG4_FULL_SCALE_8G ((1<<5) | (1<<4)) + +#define ACCEL_STATUS_ZYXOR (1<<7) +#define ACCEL_STATUS_ZOR (1<<6) +#define ACCEL_STATUS_YOR (1<<5) +#define ACCEL_STATUS_XOR (1<<4) +#define ACCEL_STATUS_ZYXDA (1<<3) +#define ACCEL_STATUS_ZDA (1<<2) +#define ACCEL_STATUS_YDA (1<<1) +#define ACCEL_STATUS_XDA (1<<0) #define ACCEL_DEFAULT_RANGE_G 8 -#define ACCEL_DEFAULT_RATE 1000 +#define ACCEL_DEFAULT_RATE 1000 #define ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 780 #define ACCEL_ONE_G 9.80665f /************************************i3g4250d register addresses *******************************************/ -#define GYRO_WHO_AM_I 0x0F -#define GYRO_WHO_I_AM 0xD3 +#define GYRO_WHO_AM_I 0x0F +#define GYRO_WHO_I_AM 0xD3 -#define GYRO_CTRL_REG1 0x20 +#define GYRO_CTRL_REG1 0x20 /* keep lowpass low to avoid noise issues */ -#define RATE_100HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) -#define RATE_200HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4)) -#define RATE_200HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4)) -#define RATE_200HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) -#define RATE_400HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) -#define RATE_400HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4)) -#define RATE_400HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) -#define RATE_400HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) -#define RATE_800HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4)) -#define RATE_800HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4)) -#define RATE_800HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4)) -#define RATE_800HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) - -#define GYRO_CTRL_REG2 0x21 -#define GYRO_CTRL_REG3 0x22 -#define GYRO_CTRL_REG4 0x23 -#define RANGE_250DPS (0<<4) -#define RANGE_500DPS (1<<4) -#define RANGE_2000DPS (3<<4) - -#define GYRO_CTRL_REG5 0x24 -#define GYRO_REFERENCE 0x25 -#define GYRO_OUT_TEMP 0x26 -#define GYRO_STATUS_REG 0x27 -#define GYRO_OUT_X_L 0x28 -#define GYRO_OUT_X_H 0x29 -#define GYRO_OUT_Y_L 0x2A -#define GYRO_OUT_Y_H 0x2B -#define GYRO_OUT_Z_L 0x2C -#define GYRO_OUT_Z_H 0x2D -#define GYRO_FIFO_CTRL_REG 0x2E -#define GYRO_FIFO_SRC_REG 0x2F -#define GYRO_INT1_CFG 0x30 -#define GYRO_INT1_SRC 0x31 -#define GYRO_INT1_TSH_XH 0x32 -#define GYRO_INT1_TSH_XL 0x33 -#define GYRO_INT1_TSH_YH 0x34 -#define GYRO_INT1_TSH_YL 0x35 -#define GYRO_INT1_TSH_ZH 0x36 -#define GYRO_INT1_TSH_ZL 0x37 -#define GYRO_INT1_DURATION 0x38 -#define GYRO_LOW_ODR 0x39 +#define RATE_100HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) +#define RATE_200HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define RATE_200HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4)) +#define RATE_200HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) +#define RATE_400HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define RATE_400HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4)) +#define RATE_400HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define RATE_400HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) +#define RATE_800HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4)) +#define RATE_800HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4)) +#define RATE_800HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4)) +#define RATE_800HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) + +#define GYRO_CTRL_REG2 0x21 +#define GYRO_CTRL_REG3 0x22 +#define GYRO_CTRL_REG4 0x23 +#define RANGE_250DPS (0<<4) +#define RANGE_500DPS (1<<4) +#define RANGE_2000DPS (3<<4) + +#define GYRO_CTRL_REG5 0x24 +#define GYRO_REFERENCE 0x25 +#define GYRO_OUT_TEMP 0x26 +#define GYRO_STATUS_REG 0x27 +#define GYRO_OUT_X_L 0x28 +#define GYRO_OUT_X_H 0x29 +#define GYRO_OUT_Y_L 0x2A +#define GYRO_OUT_Y_H 0x2B +#define GYRO_OUT_Z_L 0x2C +#define GYRO_OUT_Z_H 0x2D +#define GYRO_FIFO_CTRL_REG 0x2E +#define GYRO_FIFO_SRC_REG 0x2F +#define GYRO_INT1_CFG 0x30 +#define GYRO_INT1_SRC 0x31 +#define GYRO_INT1_TSH_XH 0x32 +#define GYRO_INT1_TSH_XL 0x33 +#define GYRO_INT1_TSH_YH 0x34 +#define GYRO_INT1_TSH_YL 0x35 +#define GYRO_INT1_TSH_ZH 0x36 +#define GYRO_INT1_TSH_ZL 0x37 +#define GYRO_INT1_DURATION 0x38 +#define GYRO_LOW_ODR 0x39 /* Internal configuration values */ -#define GYRO_REG1_POWER_NORMAL (1<<3) -#define GYRO_REG1_Z_ENABLE (1<<2) -#define GYRO_REG1_Y_ENABLE (1<<1) -#define GYRO_REG1_X_ENABLE (1<<0) - -#define GYRO_REG4_BLE (1<<6) - -#define GYRO_REG5_FIFO_ENABLE (1<<6) -#define GYRO_REG5_REBOOT_MEMORY (1<<7) - -#define GYRO_STATUS_ZYXOR (1<<7) -#define GYRO_STATUS_ZOR (1<<6) -#define GYRO_STATUS_YOR (1<<5) -#define GYRO_STATUS_XOR (1<<4) -#define GYRO_STATUS_ZYXDA (1<<3) -#define GYRO_STATUS_ZDA (1<<2) -#define GYRO_STATUS_YDA (1<<1) -#define GYRO_STATUS_XDA (1<<0) - -#define GYRO_FIFO_CTRL_BYPASS_MODE (0<<5) -#define GYRO_FIFO_CTRL_FIFO_MODE (1<<5) -#define GYRO_FIFO_CTRL_STREAM_MODE (1<<6) -#define GYRO_FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5) +#define GYRO_REG1_POWER_NORMAL (1<<3) +#define GYRO_REG1_Z_ENABLE (1<<2) +#define GYRO_REG1_Y_ENABLE (1<<1) +#define GYRO_REG1_X_ENABLE (1<<0) + +#define GYRO_REG4_BLE (1<<6) + +#define GYRO_REG5_FIFO_ENABLE (1<<6) +#define GYRO_REG5_REBOOT_MEMORY (1<<7) + +#define GYRO_STATUS_ZYXOR (1<<7) +#define GYRO_STATUS_ZOR (1<<6) +#define GYRO_STATUS_YOR (1<<5) +#define GYRO_STATUS_XOR (1<<4) +#define GYRO_STATUS_ZYXDA (1<<3) +#define GYRO_STATUS_ZDA (1<<2) +#define GYRO_STATUS_YDA (1<<1) +#define GYRO_STATUS_XDA (1<<0) + +#define GYRO_FIFO_CTRL_BYPASS_MODE (0<<5) +#define GYRO_FIFO_CTRL_FIFO_MODE (1<<5) +#define GYRO_FIFO_CTRL_STREAM_MODE (1<<6) +#define GYRO_FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5) #define GYRO_FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7) -#define GYRO_DEFAULT_RATE 800 //data output frequency +//data output frequency +#define GYRO_DEFAULT_RATE 800 #define GYRO_DEFAULT_RANGE_DPS 2000 -#define GYRO_DEFAULT_FILTER_FREQ 35 -#define GYRO_TEMP_OFFSET_CELSIUS 40 +#define GYRO_DEFAULT_FILTER_FREQ 35 +#define GYRO_TEMP_OFFSET_CELSIUS 40 // constructor @@ -230,10 +220,9 @@ AP_InertialSensor_Backend *AP_InertialSensor_RST::probe(AP_InertialSensor &imu, */ bool AP_InertialSensor_RST::_init_gyro(void) { + uint8_t whoami; - _gyro_spi_sem = _dev_gyro->get_semaphore(); - - if (!_gyro_spi_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + if (!_dev_gyro->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { return false; } @@ -242,10 +231,10 @@ bool AP_InertialSensor_RST::_init_gyro(void) _dev_gyro->set_speed(AP_HAL::Device::SPEED_HIGH); - _dev_gyro->read_registers(GYRO_WHO_AM_I, &_gyro_whoami, sizeof(_gyro_whoami)); - if (_gyro_whoami != GYRO_WHO_I_AM) { - hal.console->printf("RST: unexpected gyro WHOAMI 0x%x\n", (unsigned)_gyro_whoami); - printf("RST: unexpected gyro WHOAMI 0x%x\n", (unsigned)_gyro_whoami); + _dev_gyro->read_registers(GYRO_WHO_AM_I, &whoami, sizeof(whoami)); + if (whoami != GYRO_WHO_I_AM) { + hal.console->printf("RST: unexpected gyro WHOAMI 0x%x\n", (unsigned)whoami); + printf("RST: unexpected gyro WHOAMI 0x%x\n", (unsigned)whoami); goto fail_whoami; } @@ -256,32 +245,37 @@ bool AP_InertialSensor_RST::_init_gyro(void) hal.scheduler->delay(100); - _dev_gyro->write_register(GYRO_CTRL_REG1, - GYRO_REG1_POWER_NORMAL | GYRO_REG1_Z_ENABLE | GYRO_REG1_X_ENABLE | GYRO_REG1_Y_ENABLE | RATE_800HZ_LP_50HZ); + _dev_gyro->write_register(GYRO_CTRL_REG1, + GYRO_REG1_POWER_NORMAL | + GYRO_REG1_Z_ENABLE | GYRO_REG1_X_ENABLE | GYRO_REG1_Y_ENABLE | + RATE_800HZ_LP_50HZ); + + /* disable high-pass filters */ + _dev_gyro->write_register(GYRO_CTRL_REG2, 0); + + /* DRDY disable */ + _dev_gyro->write_register(GYRO_CTRL_REG3, 0x0); + _dev_gyro->write_register(GYRO_CTRL_REG4, RANGE_2000DPS); - _dev_gyro->write_register(GYRO_CTRL_REG2, 0); /* disable high-pass filters */ - _dev_gyro->write_register(GYRO_CTRL_REG3, 0x0); /* DRDY disable */ - _dev_gyro->write_register(GYRO_CTRL_REG4, RANGE_2000DPS); - _dev_gyro->write_register(GYRO_CTRL_REG5, GYRO_REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ + /* disable wake-on-interrupt */ + _dev_gyro->write_register(GYRO_CTRL_REG5, GYRO_REG5_FIFO_ENABLE); - /* disable FIFO. This makes things simpler and ensures we - * aren't getting stale data. It means we must run the hrt - * callback fast enough to not miss data. */ - _dev_gyro->write_register(GYRO_FIFO_CTRL_REG, GYRO_FIFO_CTRL_BYPASS_MODE); + /* disable FIFO. This makes things simpler and ensures we + * aren't getting stale data. It means we must run the hrt + * callback fast enough to not miss data. */ + _dev_gyro->write_register(GYRO_FIFO_CTRL_REG, GYRO_FIFO_CTRL_BYPASS_MODE); - _gyro_scale = 70e-3f / 180.0f * M_PI; + _gyro_scale = 70e-3f / 180.0f * M_PI; hal.scheduler->delay(100); - _gyro_spi_sem->give(); + _dev_gyro->get_semaphore()->give(); return true; fail_whoami: - _gyro_spi_sem->give(); + _dev_gyro->get_semaphore()->give(); return false; - - } /* @@ -289,9 +283,9 @@ fail_whoami: */ bool AP_InertialSensor_RST::_init_accel(void) { - _accel_spi_sem = _dev_accel->get_semaphore(); + uint8_t whoami; - if (!_accel_spi_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + if (!_dev_accel->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { return false; } @@ -299,27 +293,34 @@ bool AP_InertialSensor_RST::_init_accel(void) _dev_accel->set_read_flag(0x80); - _dev_accel->read_registers(ACCEL_WHO_AM_I, &_accel_whoami, sizeof(_accel_whoami)); - if (_accel_whoami != ACCEL_WHO_I_AM) { - hal.console->printf("RST: unexpected accel WHOAMI 0x%x\n", (unsigned)_accel_whoami); - printf("RST: unexpected accel WHOAMI 0x%x\n", (unsigned)_accel_whoami); + _dev_accel->read_registers(ACCEL_WHO_AM_I, &whoami, sizeof(whoami)); + if (whoami != ACCEL_WHO_I_AM) { + hal.console->printf("RST: unexpected accel WHOAMI 0x%x\n", (unsigned)whoami); + printf("RST: unexpected accel WHOAMI 0x%x\n", (unsigned)whoami); goto fail_whoami; } - _dev_accel->write_register(ACCEL_CTRL_REG1, - ACCEL_REG1_POWER_NORMAL | ACCEL_REG1_Z_ENABLE | ACCEL_REG1_Y_ENABLE | ACCEL_REG1_X_ENABLE | RATE_1000HZ_LP_780HZ); - _dev_accel->write_register(ACCEL_CTRL_REG2, 0); /* disable high-pass filters */ - _dev_accel->write_register(ACCEL_CTRL_REG3, 0x02); /* DRDY enable */ - _dev_accel->write_register(ACCEL_CTRL_REG4, ACCEL_REG4_BDU | ACCEL_REG4_FULL_SCALE_8G); + _dev_accel->write_register(ACCEL_CTRL_REG1, + ACCEL_REG1_POWER_NORMAL | + ACCEL_REG1_Z_ENABLE | ACCEL_REG1_Y_ENABLE | ACCEL_REG1_X_ENABLE | + RATE_1000HZ_LP_780HZ); + + /* disable high-pass filters */ + _dev_accel->write_register(ACCEL_CTRL_REG2, 0); + + /* DRDY enable */ + _dev_accel->write_register(ACCEL_CTRL_REG3, 0x02); + _dev_accel->write_register(ACCEL_CTRL_REG4, + ACCEL_REG4_BDU | ACCEL_REG4_FULL_SCALE_8G); _accel_scale = 0.244e-3f * ACCEL_ONE_G; - _accel_spi_sem->give(); + _dev_accel->get_semaphore()->give(); return true; fail_whoami: - _accel_spi_sem->give(); + _dev_accel->get_semaphore()->give(); return false; @@ -327,8 +328,7 @@ fail_whoami: bool AP_InertialSensor_RST::_init_sensor(void) { - if(!_init_gyro() || !_init_accel()) - { + if (!_init_gyro() || !_init_accel()) { return false; } @@ -368,64 +368,35 @@ void AP_InertialSensor_RST::gyro_measure(void) Vector3f gyro; uint8_t status = 0; int16_t raw_data[3]; - static uint64_t start = 0; - uint64_t now = 0; - static int count = 0; _dev_gyro->read_registers(GYRO_STATUS_REG, &status, sizeof(status)); - if((status & GYRO_STATUS_ZYXDA) == 0) + if ((status & GYRO_STATUS_ZYXDA) == 0) { return; - - count++; - - now = AP_HAL::micros64(); - - if(now - start >= 1000000) - { - //printf("gyro count = %d\n", count); - count = 0; - start = now; } - if (_dev_gyro->read_registers(GYRO_OUT_X_L | ADDR_INCREMENT, (uint8_t *)raw_data, sizeof(raw_data))) - { + if (_dev_gyro->read_registers(GYRO_OUT_X_L | ADDR_INCREMENT, (uint8_t *)raw_data, sizeof(raw_data))) { gyro = Vector3f(raw_data[0], raw_data[1], raw_data[2]); gyro *= _gyro_scale; _rotate_and_correct_gyro(_gyro_instance, gyro); _notify_new_gyro_raw_sample(_gyro_instance, gyro); } - } -// Accumulate values from accels +// Accumulate values from accels void AP_InertialSensor_RST::accel_measure(void) { Vector3f accel; uint8_t status = 0; int16_t raw_data[3]; - static uint64_t start = 0; - uint64_t now = 0; - static int count = 0; _dev_accel->read_registers(ACCEL_STATUS_REG, &status, sizeof(status)); - if((status & ACCEL_STATUS_ZYXDA) == 0) + if ((status & ACCEL_STATUS_ZYXDA) == 0) { return; - - count++; - - now = AP_HAL::micros64(); - - if(now - start >= 1000000) - { - //printf("accel count = %d\n", count); - count = 0; - start = now; } - if (_dev_accel->read_registers(ACCEL_OUT_X_L | ADDR_INCREMENT, (uint8_t *)raw_data, sizeof(raw_data))) - { + if (_dev_accel->read_registers(ACCEL_OUT_X_L | ADDR_INCREMENT, (uint8_t *)raw_data, sizeof(raw_data))) { accel = Vector3f(raw_data[0], raw_data[1], raw_data[2]); accel *= _accel_scale; _rotate_and_correct_accel(_accel_instance, accel); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_RST.h b/libraries/AP_InertialSensor/AP_InertialSensor_RST.h index 3cc7f3327a..90d76d3e39 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_RST.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_RST.h @@ -43,13 +43,6 @@ private: AP_HAL::OwnPtr _dev_gyro;//i3g4250d AP_HAL::OwnPtr _dev_accel;//iis328dq - AP_HAL::Semaphore *_gyro_spi_sem; - AP_HAL::Semaphore *_accel_spi_sem; - - // gyro whoami - uint8_t _gyro_whoami; - // accel whoami - uint8_t _accel_whoami; float _gyro_scale; float _accel_scale;