/* * This program is free software */ #include #include "AP_InertialSensor_LSM9DS1.h" #include #include extern const AP_HAL::HAL& hal; #define WHO_AM_I 0x68 #define WHO_AM_I_M 0x3D #define LSM9DS1_DRY_XG_PIN -1 /* * Accelerometer and Gyroscope registers */ #define LSM9DS1XG_ACT_THS 0x04 # define LSM9DS1XG_ACT_THS_SLEEP_ON (0x1 << 7) #define LSM9DS1XG_ACT_DUR 0x05 #define LSM9DS1XG_INT_GEN_CFG_XL 0x06 # define LSM9DS1XG_INT_GEN_CFG_XL_AOI_XL (0x1 << 7) # define LSM9DS1XG_INT_GEN_CFG_XL_6D (0x1 << 6) # define LSM9DS1XG_INT_GEN_CFG_XL_ZHIE_XL (0x1 << 5) # define LSM9DS1XG_INT_GEN_CFG_XL_ZLIE_XL (0x1 << 4) # define LSM9DS1XG_INT_GEN_CFG_XL_YHIE_XL (0x1 << 3) # define LSM9DS1XG_INT_GEN_CFG_XL_YLIE_XL (0x1 << 2) # define LSM9DS1XG_INT_GEN_CFG_XL_XHIE_XL (0x1 << 1) # define LSM9DS1XG_INT_GEN_CFG_XL_XLIE_XL (0x1 << 0) #define LSM9DS1XG_INT_GEN_THS_X_XL 0x07 #define LSM9DS1XG_INT_GEN_THS_Y_XL 0x08 #define LSM9DS1XG_INT_GEN_THS_Z_XL 0x09 #define LSM9DS1XG_INT_GEN_DUR_XL 0x0A # define LSM9DS1XG_INT_GEN_DUR_XL_WAIT_XL (0x1 << 7) #define LSM9DS1XG_REFERENCE_G 0x0B #define LSM9DS1XG_INT1_CTRL 0x0C # define LSM9DS1XG_INT1_CTRL_INT1_IG_G (0x1 << 7) # define LSM9DS1XG_INT1_CTRL_INT_IG_XL (0x1 << 6) # define LSM9DS1XG_INT1_CTRL_INT_FSS5 (0x1 << 5) # define LSM9DS1XG_INT1_CTRL_INT_OVR (0x1 << 4) # define LSM9DS1XG_INT1_CTRL_INT_FTH (0x1 << 3) # define LSM9DS1XG_INT1_CTRL_INT_Boot (0x1 << 2) # define LSM9DS1XG_INT1_CTRL_INT_DRDY_G (0x1 << 1) # define LSM9DS1XG_INT1_CTRL_INT_DRDY_XL (0x1 << 0) #define LSM9DS1XG_INT2_CTRL 0x0D # define LSM9DS1XG_INT2_CTRL_INT2_INACT (0x1 << 7) # define LSM9DS1XG_INT2_CTRL_INT2_FSS5 (0x1 << 5) # define LSM9DS1XG_INT2_CTRL_INT2_OVR (0x1 << 4) # define LSM9DS1XG_INT2_CTRL_INT2_FTH (0x1 << 3) # define LSM9DS1XG_INT2_CTRL_INT2_DRDY_TEMP (0x1 << 2) # define LSM9DS1XG_INT2_CTRL_INT2_DRDY_G (0x1 << 1) # define LSM9DS1XG_INT2_CTRL_INT2_DRDY_XL (0x1 << 0) #define LSM9DS1XG_WHO_AM_I 0x0F #define LSM9DS1XG_CTRL_REG1_G 0x10 # define LSM9DS1XG_CTRL_REG1_G_ODR_G_14900mHz (0x1 << 5) # define LSM9DS1XG_CTRL_REG1_G_ODR_G_59500mHz (0x2 << 5) # define LSM9DS1XG_CTRL_REG1_G_ODR_G_119Hz (0x3 << 5) # define LSM9DS1XG_CTRL_REG1_G_ODR_G_238Hz (0x4 << 5) # define LSM9DS1XG_CTRL_REG1_G_ODR_G_476Hz (0x5 << 5) # define LSM9DS1XG_CTRL_REG1_G_ODR_G_952Hz (0x6 << 5) # define LSM9DS1XG_CTRL_REG1_FS_G_245DPS (0x0 << 3) # define LSM9DS1XG_CTRL_REG1_FS_G_500DPS (0x1 << 3) # define LSM9DS1XG_CTRL_REG1_FS_G_2000DPS (0x3 << 3) #define LSM9DS1XG_CTRL_REG2_G 0x11 # define LSM9DS1XG_CTRL_REG2_G_INT_SEL_00 (0x0 << 2) # define LSM9DS1XG_CTRL_REG2_G_INT_SEL_01 (0x1 << 2) # define LSM9DS1XG_CTRL_REG2_G_INT_SEL_10 (0x2 << 2) # define LSM9DS1XG_CTRL_REG2_G_INT_SEL_11 (0x3 << 2) # define LSM9DS1XG_CTRL_REG2_G_OUT_SEL_00 (0x0 << 0) # define LSM9DS1XG_CTRL_REG2_G_OUT_SEL_01 (0x1 << 0) # define LSM9DS1XG_CTRL_REG2_G_OUT_SEL_10 (0x2 << 0) # define LSM9DS1XG_CTRL_REG2_G_OUT_SEL_11 (0x3 << 0) #define LSM9DS1XG_CTRL_REG3_G 0x12 # define LSM9DS1XG_CTRL_REG3_G_LP_MODE (0x1 << 7) # define LSM9DS1XG_CTRL_REG3_G_HP_EN (0x1 << 6) #define LSM9DS1XG_ORIENT_CFG_G 0x13 #define LSM9DS1XG_INT_GEN_SRC_G 0x14 # define LSM9DS1XG_INT_GEN_SRC_G_IA_G (0x1 << 6) # define LSM9DS1XG_INT_GEN_SRC_G_ZH_G (0x1 << 5) # define LSM9DS1XG_INT_GEN_SRC_G_ZL_G (0x1 << 4) # define LSM9DS1XG_INT_GEN_SRC_G_YH_G (0x1 << 3) # define LSM9DS1XG_INT_GEN_SRC_G_YL_G (0x1 << 2) # define LSM9DS1XG_INT_GEN_SRC_G_XH_G (0x1 << 1) # define LSM9DS1XG_INT_GEN_SRC_G_XL_G (0x1 << 0) #define LSM9DS1XG_OUT_TEMP_L 0x15 #define LSM9DS1XG_OUT_TEMP_H 0x16 #define LSM9DS1XG_STATUS_REG 0x17 # define LSM9DS1XG_STATUS_REG_IG_XL (0x1 << 6) # define LSM9DS1XG_STATUS_REG_IG_G (0x1 << 5) # define LSM9DS1XG_STATUS_REG_INACT (0x1 << 4) # define LSM9DS1XG_STATUS_REG_BOOT_STATUS (0x1 << 3) # define LSM9DS1XG_STATUS_REG_TDA (0x1 << 2) # define LSM9DS1XG_STATUS_REG_GDA (0x1 << 1) # define LSM9DS1XG_STATUS_REG_XLDA (0x1 << 0) #define LSM9DS1XG_OUT_X_L_G 0x18 #define LSM9DS1XG_OUT_X_H_G 0x19 #define LSM9DS1XG_OUT_Y_L_G 0x1A #define LSM9DS1XG_OUT_Y_H_G 0x1B #define LSM9DS1XG_OUT_Z_L_G 0x1C #define LSM9DS1XG_OUT_Z_H_G 0x1D #define LSM9DS1XG_CTRL_REG4 0x1E # define LSM9DS1XG_CTRL_REG4_Zen_G (0x1 << 5) # define LSM9DS1XG_CTRL_REG4_Yen_G (0x1 << 4) # define LSM9DS1XG_CTRL_REG4_Xen_G (0x1 << 3) # define LSM9DS1XG_CTRL_REG4_LIR_XL1 (0x1 << 1) # define LSM9DS1XG_CTRL_REG4_4D_XL1 (0x1 << 0) #define LSM9DS1XG_CTRL_REG5_XL 0x1F # define LSM9DS1XG_CTRL_REG5_XL_Zen_XL (0x1 << 5) # define LSM9DS1XG_CTRL_REG5_XL_Yen_XL (0x1 << 4) # define LSM9DS1XG_CTRL_REG5_XL_Xen_XL (0x1 << 3) #define LSM9DS1XG_CTRL_REG6_XL 0x20 # define LSM9DS1XG_CTRL_REG6_XL_ODR_XL_10Hz (0x1 << 5) # define LSM9DS1XG_CTRL_REG6_XL_ODR_XL_50Hz (0x2 << 5) # define LSM9DS1XG_CTRL_REG6_XL_ODR_XL_119Hz (0x3 << 5) # define LSM9DS1XG_CTRL_REG6_XL_ODR_XL_238Hz (0x4 << 5) # define LSM9DS1XG_CTRL_REG6_XL_ODR_XL_476Hz (0x5 << 5) # define LSM9DS1XG_CTRL_REG6_XL_ODR_XL_952Hz (0x6 << 5) # define LSM9DS1XG_CTRL_REG6_XL_FS_XL_2G (0x0 << 3) # define LSM9DS1XG_CTRL_REG6_XL_FS_XL_16G (0x1 << 3) # define LSM9DS1XG_CTRL_REG6_XL_FS_XL_4G (0x2 << 3) # define LSM9DS1XG_CTRL_REG6_XL_FS_XL_8G (0x3 << 3) # define LSM9DS1XG_CTRL_REG6_XL_BW_SCAL_ODR (0x1 << 2) # define LSM9DS1XG_CTRL_REG6_XL_BW_XL_408Hz (0x0 << 0) # define LSM9DS1XG_CTRL_REG6_XL_BW_XL_211Hz (0x1 << 0) # define LSM9DS1XG_CTRL_REG6_XL_BW_XL_105Hz (0x2 << 0) # define LSM9DS1XG_CTRL_REG6_XL_BW_XL_50Hz (0x3 << 0) #define LSM9DS1XG_CTRL_REG7_XL 0x21 # define LSM9DS1XG_CTRL_REG7_XL_FDS (0x1 << 2) # define LSM9DS1XG_CTRL_REG7_XL_HPIS1 (0x1 << 0) #define LSM9DS1XG_CTRL_REG8 0x22 # define LSM9DS1XG_CTRL_REG8_BOOT (0x1 << 7) # define LSM9DS1XG_CTRL_REG8_BDU (0x1 << 6) # define LSM9DS1XG_CTRL_REG8_H_LACTIVE (0x1 << 5) # define LSM9DS1XG_CTRL_REG8_PP_OD (0x1 << 4) # define LSM9DS1XG_CTRL_REG8_SIM (0x1 << 3) # define LSM9DS1XG_CTRL_REG8_IF_ADD_INC (0x1 << 2) # define LSM9DS1XG_CTRL_REG8_BLE (0x1 << 1) # define LSM9DS1XG_CTRL_REG8_SW_RESET (0x1 << 0) #define LSM9DS1XG_CTRL_REG9 0x23 # define LSM9DS1XG_CTRL_REG9_SLEEP_G (0x1 << 6) # define LSM9DS1XG_CTRL_REG9_FIFO_TEMP_EN (0x1 << 4) # define LSM9DS1XG_CTRL_REG9_DRDY_mask_bit (0x1 << 3) # define LSM9DS1XG_CTRL_REG9_I2C_DISABLE (0x1 << 2) # define LSM9DS1XG_CTRL_REG9_FIFO_EN (0x1 << 1) # define LSM9DS1XG_CTRL_REG9_STOP_ON_FTH (0x1 << 0) #define LSM9DS1XG_CTRL_REG10 0x24 # define LSM9DS1XG_CTRL_REG10_ST_G (0x1 << 2) # define LSM9DS1XG_CTRL_REG10_ST_XL (0x1 << 0) #define LSM9DS1XG_INT_GEN_SRC_XL 0x26 # define LSM9DS1XG_INT_GEN_SRC_XL_IA_XL (0x1 << 6) # define LSM9DS1XG_INT_GEN_SRC_XL_ZH_XL (0x1 << 5) # define LSM9DS1XG_INT_GEN_SRC_XL_ZL_XL (0x1 << 4) # define LSM9DS1XG_INT_GEN_SRC_XL_YH_XL (0x1 << 3) # define LSM9DS1XG_INT_GEN_SRC_XL_YL_XL (0x1 << 2) # define LSM9DS1XG_INT_GEN_SRC_XL_XH_XL (0x1 << 1) # define LSM9DS1XG_INT_GEN_SRC_XL_XL_XL (0x1 << 0) //#define LSM9DS1XG_STATUS_REG 0x27 //repeat #define LSM9DS1XG_OUT_X_L_XL 0x28 #define LSM9DS1XG_OUT_X_H_XL 0x29 #define LSM9DS1XG_OUT_Y_L_XL 0x2A #define LSM9DS1XG_OUT_Y_H_XL 0x2B #define LSM9DS1XG_OUT_Z_L_XL 0x2C #define LSM9DS1XG_OUT_Z_H_XL 0x2D #define LSM9DS1XG_FIFO_CTRL 0x2E # define LSM9DS1XG_FIFO_CTRL_FMODE_BYPASS (0x0 << 5) # define LSM9DS1XG_FIFO_CTRL_FMODE_FIFO (0x1 << 5) # define LSM9DS1XG_FIFO_CTRL_FMODE_STREAM (0x3 << 5) # define LSM9DS1XG_FIFO_CTRL_FMODE_B_TO_S (0x4 << 5) # define LSM9DS1XG_FIFO_CTRL_FMODE_CON (0x5 << 5) #define LSM9DS1XG_FIFO_SRC 0x2F # define LSM9DS1XG_FIFO_SRC_FTH (0x1 << 7) # define LSM9DS1XG_FIFO_SRC_OVRN (0x1 << 6) # define LSM9DS1XG_FIFO_SRC_UNREAD_SAMPLES 0x3F #define LSM9DS1XG_INT_GEN_CFG_G 0x30 # define LSM9DS1XG_INT_GEN_CFG_G_AOI_G (0x1 << 7) # define LSM9DS1XG_INT_GEN_CFG_G_LIR_G (0x1 << 6) # define LSM9DS1XG_INT_GEN_CFG_G_ZHIE_G (0x1 << 5) # define LSM9DS1XG_INT_GEN_CFG_G_ZLIE_G (0x1 << 4) # define LSM9DS1XG_INT_GEN_CFG_G_YHIE_G (0x1 << 3) # define LSM9DS1XG_INT_GEN_CFG_G_YLIE_G (0x1 << 2) # define LSM9DS1XG_INT_GEN_CFG_G_XHIE_G (0x1 << 1) # define LSM9DS1XG_INT_GEN_CFG_G_XLIE_G (0x1 << 0) #define LSM9DS1XG_INT_GEN_THS_XH_G 0x31 # define LSM9DS1XG_INT_GEN_THS_XH_G_DCRM_G (0x1 << 7) #define LSM9DS1XG_INT_GEN_THS_XL_G 0x32 #define LSM9DS1XG_INT_GEN_THS_YH_G 0x33 #define LSM9DS1XG_INT_GEN_THS_YL_G 0x34 #define LSM9DS1XG_INT_GEN_THS_ZH_G 0x35 #define LSM9DS1XG_INT_GEN_THS_ZL_G 0x36 #define LSM9DS1XG_INT_GEN_DUR_G 0x37 # define LSM9DS1XG_INT_GEN_DUR_G_WAIT_G (0x1 << 7) AP_InertialSensor_LSM9DS1::AP_InertialSensor_LSM9DS1(AP_InertialSensor &imu, AP_HAL::OwnPtr dev, int drdy_pin_num_xg, enum Rotation rotation) : AP_InertialSensor_Backend(imu) , _dev(std::move(dev)) , _drdy_pin_num_xg(drdy_pin_num_xg) , _rotation(rotation) { } AP_InertialSensor_Backend *AP_InertialSensor_LSM9DS1::probe(AP_InertialSensor &_imu, AP_HAL::OwnPtr dev, enum Rotation rotation) { if (!dev) { return nullptr; } AP_InertialSensor_LSM9DS1 *sensor = NEW_NOTHROW AP_InertialSensor_LSM9DS1(_imu,std::move(dev), LSM9DS1_DRY_XG_PIN, rotation); if (!sensor || !sensor->_init_sensor()) { delete sensor; return nullptr; } return sensor; } bool AP_InertialSensor_LSM9DS1::_init_sensor() { _spi_sem = _dev->get_semaphore(); if (_drdy_pin_num_xg >= 0) { _drdy_pin_xg = hal.gpio->channel(_drdy_pin_num_xg); if (_drdy_pin_xg == nullptr) { AP_HAL::panic("LSM9DS1: null accel data-ready GPIO channel\n"); } _drdy_pin_xg->mode(HAL_GPIO_INPUT); } bool success = _hardware_init(); #if LSM9DS1_DEBUG _dump_registers(); #endif return success; } bool AP_InertialSensor_LSM9DS1::_hardware_init() { _spi_sem->take_blocking(); uint8_t tries, whoami; // set flag for reading registers _dev->set_read_flag(0x80); whoami = _register_read(LSM9DS1XG_WHO_AM_I); if (whoami != WHO_AM_I) { DEV_PRINTF("LSM9DS1: unexpected acc/gyro WHOAMI 0x%x\n", whoami); goto fail_whoami; } _fifo_reset(); for (tries = 0; tries < 5; tries++) { _dev->set_speed(AP_HAL::Device::SPEED_LOW); _gyro_init(); _accel_init(); _dev->set_speed(AP_HAL::Device::SPEED_HIGH); hal.scheduler->delay(10); int samples = _register_read(LSM9DS1XG_FIFO_SRC); //if samples == 0 -> FIFO empty if (samples > 1) { break; } #if LSM9DS1_DEBUG _dump_registers(); #endif } if (tries == 5) { DEV_PRINTF("Failed to boot LSM9DS1 5 times\n\n"); goto fail_tries; } _spi_sem->give(); return true; fail_tries: fail_whoami: _spi_sem->give(); return false; } void AP_InertialSensor_LSM9DS1::_fifo_reset() { _dev->set_speed(AP_HAL::Device::SPEED_LOW); //Disable FIFO int reg_9 = _register_read(LSM9DS1XG_CTRL_REG9); _dev->write_register(LSM9DS1XG_CTRL_REG9, reg_9 & ~0x02); //Enable Bypass for reset FIFO _dev->write_register(LSM9DS1XG_FIFO_CTRL, LSM9DS1XG_FIFO_CTRL_FMODE_BYPASS); //Enable FIFO and Disable I2C _dev->write_register(LSM9DS1XG_CTRL_REG9,LSM9DS1XG_CTRL_REG9_FIFO_EN | LSM9DS1XG_CTRL_REG9_I2C_DISABLE); //Enable block data update, allow auto-increment during multiple byte read _dev->write_register(LSM9DS1XG_CTRL_REG8, LSM9DS1XG_CTRL_REG8_BDU | LSM9DS1XG_CTRL_REG8_IF_ADD_INC); hal.scheduler->delay_microseconds(1); //Enable FIFO stream mode and set watermark at 32 samples _dev->write_register(LSM9DS1XG_FIFO_CTRL, 0x1F | LSM9DS1XG_FIFO_CTRL_FMODE_FIFO); hal.scheduler->delay_microseconds(1); _dev->set_speed(AP_HAL::Device::SPEED_HIGH); notify_accel_fifo_reset(accel_instance); notify_gyro_fifo_reset(gyro_instance); } /* start the sensor going */ void AP_InertialSensor_LSM9DS1::start(void) { if (!_imu.register_gyro(gyro_instance, 952, _dev->get_bus_id_devtype(DEVTYPE_GYR_LSM9DS1)) || !_imu.register_accel(accel_instance, 952, _dev->get_bus_id_devtype(DEVTYPE_ACC_LSM9DS1))) { return; } set_accel_orientation(accel_instance, _rotation); set_gyro_orientation(gyro_instance, _rotation); _set_accel_max_abs_offset(accel_instance, 5.0f); /* start the timer process to read samples */ _dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_LSM9DS1::_poll_data, void)); } uint8_t AP_InertialSensor_LSM9DS1::_register_read(uint8_t reg) { uint8_t val = 0; _dev->read_registers(reg, &val, 1); return val; } void AP_InertialSensor_LSM9DS1::_register_write(uint8_t reg, uint8_t val, bool checked) { _dev->write_register(reg, val, checked); } void AP_InertialSensor_LSM9DS1::_gyro_init() { _register_write(LSM9DS1XG_CTRL_REG1_G, LSM9DS1XG_CTRL_REG1_G_ODR_G_952Hz | LSM9DS1XG_CTRL_REG1_FS_G_2000DPS); hal.scheduler->delay(1); _register_write(LSM9DS1XG_CTRL_REG4, LSM9DS1XG_CTRL_REG4_Zen_G | LSM9DS1XG_CTRL_REG4_Yen_G | LSM9DS1XG_CTRL_REG4_Xen_G); _set_gyro_scale(); hal.scheduler->delay(1); } void AP_InertialSensor_LSM9DS1::_accel_init() { _register_write(LSM9DS1XG_CTRL_REG6_XL, LSM9DS1XG_CTRL_REG6_XL_ODR_XL_952Hz | LSM9DS1XG_CTRL_REG6_XL_FS_XL_16G); hal.scheduler->delay(1); _register_write(LSM9DS1XG_CTRL_REG5_XL, LSM9DS1XG_CTRL_REG5_XL_Zen_XL | LSM9DS1XG_CTRL_REG5_XL_Yen_XL | LSM9DS1XG_CTRL_REG5_XL_Xen_XL); _set_accel_scale(A_SCALE_16G); hal.scheduler->delay(1); } void AP_InertialSensor_LSM9DS1::_set_gyro_scale() { /* scale value from datasheet 2000 mdps/digit */ _gyro_scale = 70; /* convert mdps/digit to dps/digit */ _gyro_scale /= 1000; /* convert dps/digit to (rad/s)/digit */ _gyro_scale *= DEG_TO_RAD; } void AP_InertialSensor_LSM9DS1::_set_accel_scale(accel_scale scale) { /* * Possible accelerometer scales (and their register bit settings) are: * 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100). Here's a bit of an * algorithm to calculate g/(ADC tick) based on that 3-bit value: */ _accel_scale = (((float) scale + 1.0f) * 2.0f) / 32768.0f; if (scale == A_SCALE_16G) { /* the datasheet shows an exception for +-16G */ _accel_scale = 0.000732f; } /* convert to G/LSB to (m/s/s)/LSB */ _accel_scale *= GRAVITY_MSS; } /** * Timer process to poll for new data from the LSM9DS1. */ void AP_InertialSensor_LSM9DS1::_poll_data() { uint16_t samples = _register_read(LSM9DS1XG_FIFO_SRC); samples = samples & LSM9DS1XG_FIFO_SRC_UNREAD_SAMPLES; if (samples > 1) { _read_data_transaction_g(samples); _read_data_transaction_x(samples); } if (samples == 32) { _fifo_reset(); } // check next register value for correctness AP_HAL::Device::checkreg reg; if (!_dev->check_next_register(reg)) { log_register_change(_dev->get_bus_id(), reg); _inc_accel_error_count(accel_instance); } } void AP_InertialSensor_LSM9DS1::_read_data_transaction_x(uint16_t samples) { struct sensor_raw_data raw_data = { }; int32_t _accel_bias[3] = {0, 0, 0}; const uint8_t _reg = LSM9DS1XG_OUT_X_L_XL | 0x80; // Read the accel data stored in the FIFO for (int i = 0; i < samples; i++) { struct sensor_raw_data raw_data_temp = { }; if (!_dev->transfer(&_reg, 1, (uint8_t *) &raw_data_temp, sizeof(raw_data_temp))) { DEV_PRINTF("LSM9DS1: error reading accelerometer\n"); return; } _accel_bias[0] += (int32_t) raw_data_temp.x; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases _accel_bias[1] += (int32_t) raw_data_temp.y; _accel_bias[2] += (int32_t) raw_data_temp.z; } raw_data.x = _accel_bias[0] / samples; // average the data raw_data.y = _accel_bias[1] / samples; raw_data.z = _accel_bias[2] / samples; Vector3f accel_data(raw_data.x, raw_data.y, -raw_data.z); accel_data *= _accel_scale; _rotate_and_correct_accel(accel_instance, accel_data); _notify_new_accel_raw_sample(accel_instance, accel_data); } /* * read from the data registers and update filtered data */ void AP_InertialSensor_LSM9DS1::_read_data_transaction_g(uint16_t samples) { struct sensor_raw_data raw_data = { }; int32_t _gyro_bias[3] = {0, 0, 0}; const uint8_t _reg = LSM9DS1XG_OUT_X_L_G | 0x80; // Read the gyro data stored in the FIFO for (int i = 0; i < samples; i++) { struct sensor_raw_data raw_data_temp = { }; if (!_dev->transfer(&_reg, 1, (uint8_t *) &raw_data_temp, sizeof(raw_data_temp))) { DEV_PRINTF("LSM9DS1: error reading gyroscope\n"); return; } _gyro_bias[0] += (int32_t) raw_data_temp.x; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases _gyro_bias[1] += (int32_t) raw_data_temp.y; _gyro_bias[2] += (int32_t) raw_data_temp.z; } raw_data.x = _gyro_bias[0] / samples; // average the data raw_data.y = _gyro_bias[1] / samples; raw_data.z = _gyro_bias[2] / samples; Vector3f gyro_data(raw_data.x, raw_data.y, -raw_data.z); gyro_data *= _gyro_scale; _rotate_and_correct_gyro(gyro_instance, gyro_data); _notify_new_gyro_raw_sample(gyro_instance, gyro_data); } bool AP_InertialSensor_LSM9DS1::update() { update_gyro(gyro_instance); update_accel(accel_instance); return true; } #if LSM9DS1_DEBUG /* * dump all config registers - used for debug */ void AP_InertialSensor_LSM9DS1::_dump_registers(void) { hal.console->println("LSM9DS1 registers:"); const uint8_t first = LSM9DS1XG_ACT_THS; const uint8_t last = LSM9DS1XG_INT_GEN_DUR_G; for (uint8_t reg=first; reg<=last; reg++) { uint8_t v = _register_read(reg); hal.console->printf("%02x:%02x ", reg, v); if ((reg - (first-1)) % 16 == 0) { hal.console->println(); } } hal.console->println(); } #endif