/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . -- Coded by Victor Mayoral Vilches -- The MPU9150 is a sensor composed by a MPU6050 with a AK8975 on the auxiliary bus. Please check the following links for datasheets and documentation: - http://www.invensense.com/mems/gyro/documents/PS-MPU-9150A-00v4_3.pdf - http://www.invensense.com/mems/gyro/documents/RM-MPU-9150A-00v4_2.pdf Note that this is an experimental driver. It is not used by any actively maintained board and should be considered untested and unmaintained */ #include #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX #include #include "AP_InertialSensor_MPU9150.h" #include #include #include const extern AP_HAL::HAL& hal; /////// /* Hardware registers needed by driver. */ struct gyro_reg_s { uint8_t who_am_i; uint8_t rate_div; uint8_t lpf; uint8_t prod_id; uint8_t user_ctrl; uint8_t fifo_en; uint8_t gyro_cfg; uint8_t accel_cfg; uint8_t motion_thr; uint8_t motion_dur; uint8_t fifo_count_h; uint8_t fifo_r_w; uint8_t raw_gyro; uint8_t raw_accel; uint8_t temp; uint8_t int_enable; uint8_t dmp_int_status; uint8_t int_status; uint8_t pwr_mgmt_1; uint8_t pwr_mgmt_2; uint8_t int_pin_cfg; uint8_t mem_r_w; uint8_t accel_offs; uint8_t i2c_mst; uint8_t bank_sel; uint8_t mem_start_addr; uint8_t prgm_start_h; uint8_t raw_compass; uint8_t yg_offs_tc; uint8_t s0_addr; uint8_t s0_reg; uint8_t s0_ctrl; uint8_t s1_addr; uint8_t s1_reg; uint8_t s1_ctrl; uint8_t s4_ctrl; uint8_t s0_do; uint8_t s1_do; uint8_t i2c_delay_ctrl; }; /* Information specific to a particular device. */ struct hw_s { uint8_t addr; uint16_t max_fifo; uint8_t num_reg; uint16_t temp_sens; int16_t temp_offset; uint16_t bank_size; uint16_t compass_fsr; }; /* Information for self-test. */ struct test_s { uint32_t gyro_sens; uint32_t accel_sens; uint8_t reg_rate_div; uint8_t reg_lpf; uint8_t reg_gyro_fsr; uint8_t reg_accel_fsr; uint16_t wait_ms; uint8_t packet_thresh; float min_dps; float max_dps; float max_gyro_var; float min_g; float max_g; float max_accel_var; }; /* Gyro driver state variables. */ struct gyro_state_s { const struct gyro_reg_s *reg; const struct hw_s *hw; const struct test_s *test; }; /* Filter configurations. The values correspond to the DLPF_CFG register. Note that the gyro and accel frequencies are slightly different. (DLPF_CFG register, RM-MPU-9150A00.pdf, pg. 13) */ enum lpf_e { INV_FILTER_256HZ_NOLPF2 = 0, INV_FILTER_188HZ = 1, INV_FILTER_98HZ = 2, INV_FILTER_42HZ = 3, INV_FILTER_20HZ = 4, INV_FILTER_10HZ = 5, INV_FILTER_5HZ = 6, INV_FILTER_2100HZ_NOLPF = 7, NUM_FILTER }; /* Full scale ranges. FS_SEL register. (RM-MPU-9150A00.pdf, pg. 14) */ enum gyro_fsr_e { INV_FSR_250DPS = 0, INV_FSR_500DPS = 1, INV_FSR_1000DPS = 2, INV_FSR_2000DPS = 3, NUM_GYRO_FSR }; /* Full scale ranges. AFS_SEL register. (RM-MPU-9150A00.pdf, pg. 15) */ enum accel_fsr_e { INV_FSR_2G = 0, INV_FSR_4G = 1, INV_FSR_8G = 2, INV_FSR_16G = 4, NUM_ACCEL_FSR }; /* Clock sources. */ enum clock_sel_e { INV_CLK_INTERNAL = 0, INV_CLK_PLL, NUM_CLK }; #define MPU9150_ADDRESS 0x68 #define INV_X_GYRO (0x40) #define INV_Y_GYRO (0x20) #define INV_Z_GYRO (0x10) #define INV_XYZ_GYRO (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO) #define INV_XYZ_ACCEL (0x08) #define INV_XYZ_COMPASS (0x01) #define MPU_INT_STATUS_DATA_READY (0x0001) #define MPU_INT_STATUS_DMP (0x0002) #define MPU_INT_STATUS_PLL_READY (0x0004) #define MPU_INT_STATUS_I2C_MST (0x0008) #define MPU_INT_STATUS_FIFO_OVERFLOW (0x0010) #define MPU_INT_STATUS_ZMOT (0x0020) #define MPU_INT_STATUS_MOT (0x0040) #define MPU_INT_STATUS_FREE_FALL (0x0080) #define MPU_INT_STATUS_DMP_0 (0x0100) #define MPU_INT_STATUS_DMP_1 (0x0200) #define MPU_INT_STATUS_DMP_2 (0x0400) #define MPU_INT_STATUS_DMP_3 (0x0800) #define MPU_INT_STATUS_DMP_4 (0x1000) #define MPU_INT_STATUS_DMP_5 (0x2000) #define BIT_I2C_MST_VDDIO (0x80) #define BIT_FIFO_EN (0x40) #define BIT_DMP_EN (0x80) #define BIT_FIFO_RST (0x04) #define BIT_DMP_RST (0x08) #define BIT_FIFO_OVERFLOW (0x10) #define BIT_DATA_RDY_EN (0x01) #define BIT_DMP_INT_EN (0x02) #define BIT_MOT_INT_EN (0x40) #define BITS_FSR (0x18) #define BITS_LPF (0x07) #define BITS_HPF (0x07) #define BITS_CLK (0x07) #define BIT_FIFO_SIZE_1024 (0x40) #define BIT_FIFO_SIZE_2048 (0x80) #define BIT_FIFO_SIZE_4096 (0xC0) #define BIT_RESET (0x80) #define BIT_SLEEP (0x40) #define BIT_S0_DELAY_EN (0x01) #define BIT_S2_DELAY_EN (0x04) #define BITS_SLAVE_LENGTH (0x0F) #define BIT_SLAVE_BYTE_SW (0x40) #define BIT_SLAVE_GROUP (0x10) #define BIT_SLAVE_EN (0x80) #define BIT_I2C_READ (0x80) #define BITS_I2C_MASTER_DLY (0x1F) #define BIT_AUX_IF_EN (0x20) #define BIT_ACTL (0x80) #define BIT_LATCH_EN (0x20) #define BIT_ANY_RD_CLR (0x10) #define BIT_BYPASS_EN (0x02) #define BITS_WOM_EN (0xC0) #define BIT_LPA_CYCLE (0x20) #define BIT_STBY_XA (0x20) #define BIT_STBY_YA (0x10) #define BIT_STBY_ZA (0x08) #define BIT_STBY_XG (0x04) #define BIT_STBY_YG (0x02) #define BIT_STBY_ZG (0x01) #define BIT_STBY_XYZA (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA) #define BIT_STBY_XYZG (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG) // AK8975_SECONDARY #define SUPPORTS_AK89xx_HIGH_SENS (0x00) #define AK89xx_FSR (9830) #define MAX_COMPASS_SAMPLE_RATE (100) // Gyroscope scale (uncertain where the 0.01745 value comes from) #define MPU9150_GYRO_SCALE_2000 (0.0174532f / 16.4f) #define MPU9150_GYRO_SCALE_1000 (0.0174532f / 32.8f) #define MPU9150_GYRO_SCALE_500 (0.0174532f / 65.5f) #define MPU9150_GYRO_SCALE_250 (0.0174532f / 131f) // Accelerometer scale adjustment #define MPU9150_ACCEL_SCALE_16G (GRAVITY_MSS / 2048.0f) #define MPU9150_ACCEL_SCALE_8G (GRAVITY_MSS / 4096.0f) #define MPU9150_ACCEL_SCALE_4G (GRAVITY_MSS / 8192.0f) #define MPU9150_ACCEL_SCALE_2G (GRAVITY_MSS / 16384.0f) const struct gyro_reg_s reg = { /* .who_am_i */ 0x75, /* .rate_div */ 0x19, /* .lpf */ 0x1A, /* .prod_id */ 0x0C, /* .user_ctrl */ 0x6A, /* .fifo_en */ 0x23, /* .gyro_cfg */ 0x1B, /* .accel_cfg */ 0x1C, /* .motion_thr */ 0x1F, /* .motion_dur */ 0x20, /* .fifo_count_h */ 0x72, /* .fifo_r_w */ 0x74, /* .raw_gyro */ 0x43, /* .raw_accel */ 0x3B, /* .temp */ 0x41, /* .int_enable */ 0x38, /* .dmp_int_status*/ 0x39, /* .int_status */ 0x3A, /* .pwr_mgmt_1 */ 0x6B, /* .pwr_mgmt_2 */ 0x6C, /* .int_pin_cfg */ 0x37, /* .mem_r_w */ 0x6F, /* .accel_offs */ 0x06, /* .i2c_mst */ 0x24, /* .bank_sel */ 0x6D, /* .mem_start_addr*/ 0x6E, /* .prgm_start_h */ 0x70, /* .raw_compass */ 0x49, /* .yg_offs_tc */ 0x01, /* .s0_addr */ 0x25, /* .s0_reg */ 0x26, /* .s0_ctrl */ 0x27, /* .s1_addr */ 0x28, /* .s1_reg */ 0x29, /* .s1_ctrl */ 0x2A, /* .s4_ctrl */ 0x34, /* .s0_do */ 0x63, /* .s1_do */ 0x64, /* .i2c_delay_ctrl*/ 0x67 }; const struct hw_s hw = { /* .addr */ 0x68, /* .max_fifo */ 1024, /* .num_reg */ 118, /* .temp_sens */ 340, /* .temp_offset */ -521, /* .bank_size */ 256, /* .compass_fsr */ AK89xx_FSR }; const struct test_s test = { /* .gyro_sens */ 32768/250, /* .accel_sens */ 32768/16, /* .reg_rate_div */ 0, /* 1kHz. */ /* .reg_lpf */ 1, /* 188Hz. */ /* .reg_gyro_fsr */ 0, /* 250dps. */ /* .reg_accel_fsr */ 0x18, /* 16g. */ /* .wait_ms */ 50, /* .packet_thresh */ 5, /* 5% */ /* .min_dps */ 10.f, /* .max_dps */ 105.f, /* .max_gyro_var */ 0.14f, /* .min_g */ 0.3f, /* .max_g */ 0.95f, /* .max_accel_var */ 0.14f }; static struct gyro_state_s st = { /* .reg */ ®, /* .hw */ &hw, /* .test */ &test }; /** * @brief Constructor */ AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150(AP_InertialSensor &imu) : AP_InertialSensor_Backend(imu) { } /* detect the sensor */ AP_InertialSensor_Backend *AP_InertialSensor_MPU9150::detect(AP_InertialSensor &_imu) { AP_InertialSensor_MPU9150 *sensor = new AP_InertialSensor_MPU9150(_imu); if (sensor == NULL) { return NULL; } if (!sensor->_init_sensor()) { delete sensor; return NULL; } return sensor; } /** * Init method */ bool AP_InertialSensor_MPU9150::_init_sensor(void) { // Sensors pushed to the FIFO. uint8_t sensors; // get pointer to i2c bus semaphore AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); // take i2c bus sempahore if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)){ return false; } // Init the sensor // Reset the device hal.i2c->writeRegister(st.hw->addr, st.reg->pwr_mgmt_1, BIT_RESET); hal.scheduler->delay(100); // Wake up the chip hal.i2c->writeRegister(st.hw->addr, st.reg->pwr_mgmt_1, 0x00); // Check product revision // This registers are not documented in the register map. uint8_t buff[6]; if (hal.i2c->readRegisters(st.hw->addr, st.reg->accel_offs, 6, buff) != 0) { hal.console->printf("AP_InertialSensor_MPU9150: couldn't read the registers to determine revision"); goto failed; } // uint8_t rev; // rev = ((buff[5] & 0x01) << 2) | ((buff[3] & 0x01) << 1) | // (buff[1] & 0x01); // Do not do the checking, for some reason the MPU-9150 returns 0 // if (rev) { // if (rev == 1){ // /* Congrats, these parts are better. */ // ; // } // else if (rev == 2){ // ; // } // else { // AP_HAL::panic("AP_InertialSensor_MPU9150: Unsupported software product rev.\n"); // goto failed; // } // } else { // AP_HAL::panic("Product ID read as 0 indicates device is either incompatible or an MPU3050.\n"); // goto failed; // } // Set gyro full-scale range [250, 500, 1000, 2000] if (mpu_set_gyro_fsr(2000)){ hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_gyro_fsr.\n"); goto failed; } // Set the accel full-scale range if (mpu_set_accel_fsr(2)){ hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_accel_fsr.\n"); goto failed; } // Set digital low pass filter to 256Hz (DLPF disabled) if (mpu_set_lpf(256)) { hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_lpf.\n"); goto failed; } // Set sampling rate (value must be between 4Hz and 1KHz) if (mpu_set_sample_rate(800)){ hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_sample_rate.\n"); goto failed; } // Select which sensors are pushed to FIFO. sensors = INV_XYZ_ACCEL| INV_XYZ_GYRO; if (mpu_configure_fifo(sensors)){ hal.console->printf("AP_InertialSensor_MPU9150: mpu_configure_fifo.\n"); goto failed; } // For now the compass is not used. // TODO adjust the functions to the ArduPilot API // setup_compass(); // if (mpu_set_compass_sample_rate(10, 400)) // return -1; mpu_set_sensors(sensors); // give back i2c semaphore i2c_sem->give(); _gyro_instance = _imu.register_gyro(800); _accel_instance = _imu.register_accel(800); // start the timer process to read samples hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9150::_accumulate, void)); return true; failed: // give back i2c semaphore i2c_sem->give(); return false; } /** * @brief Set the gyro full-scale range. * @param[in] fsr Desired full-scale range. * @return 0 if successful. */ int16_t AP_InertialSensor_MPU9150::mpu_set_gyro_fsr(uint16_t fsr) { uint8_t data; switch (fsr) { case 250: data = INV_FSR_250DPS << 3; break; case 500: data = INV_FSR_500DPS << 3; break; case 1000: data = INV_FSR_1000DPS << 3; break; case 2000: data = INV_FSR_2000DPS << 3; break; default: return -1; } hal.i2c->writeRegister(st.hw->addr, st.reg->gyro_cfg, data); return 0; } /** * @brief Set the accel full-scale range. * @param[in] fsr Desired full-scale range. * @return 0 if successful. */ int16_t AP_InertialSensor_MPU9150::mpu_set_accel_fsr(uint8_t fsr) { uint8_t data; switch (fsr) { case 2: data = INV_FSR_2G << 3; break; case 4: data = INV_FSR_4G << 3; break; case 8: data = INV_FSR_8G << 3; break; case 16: data = INV_FSR_16G << 3; break; default: return -1; } hal.i2c->writeRegister(st.hw->addr, st.reg->accel_cfg, data); return 0; } /** * @brief Set digital low pass filter. * The following LPF settings are supported: 188, 98, 42, 20, 10, 5. * @param[in] lpf Desired LPF setting. * @return 0 if successful. */ int16_t AP_InertialSensor_MPU9150::mpu_set_lpf(uint16_t lpf) { uint8_t data; if (lpf == 0) { data = INV_FILTER_256HZ_NOLPF2; } else if (lpf >= 256){ data = INV_FILTER_256HZ_NOLPF2; } else if (lpf >= 188){ data = INV_FILTER_188HZ; } else if (lpf >= 98){ data = INV_FILTER_98HZ; } else if (lpf >= 42){ data = INV_FILTER_42HZ; } else if (lpf >= 20){ data = INV_FILTER_20HZ; } else if (lpf >= 10){ data = INV_FILTER_10HZ; } else { data = INV_FILTER_5HZ; } hal.i2c->writeRegister(st.hw->addr, st.reg->lpf, data); return 0; } /** * @brief Set sampling rate. * Sampling rate must be between 4Hz and 1kHz. * @param[in] rate Desired sampling rate (Hz). * @return 0 if successful. */ int16_t AP_InertialSensor_MPU9150::mpu_set_sample_rate(uint16_t rate) { uint8_t data; // uint16_t sample_rate; if (rate < 4){ rate = 4; } else if (rate > 1000){ rate = 1000; } data = 1000 / rate - 1; hal.i2c->writeRegister(st.hw->addr, st.reg->rate_div, data); // sample_rate = 1000 / (1 + data); // mpu_set_compass_sample_rate(min(sample_rate, MAX_COMPASS_SAMPLE_RATE), rate); return 0; } /** * @brief Set compass sampling rate. * The compass on the auxiliary I2C bus is read by the MPU hardware at a * maximum of 100Hz. The actual rate can be set to a fraction of the gyro * sampling rate. * * \n WARNING: The new rate may be different than what was requested. Call * mpu_get_compass_sample_rate to check the actual setting. * @param[in] rate Desired compass sampling rate (Hz). * @return 0 if successful. */ int16_t AP_InertialSensor_MPU9150::mpu_set_compass_sample_rate(uint16_t rate, uint16_t chip_sample_rate) { uint8_t div; if (!rate || rate > MAX_COMPASS_SAMPLE_RATE){ return -1; } div = chip_sample_rate / rate - 1; hal.i2c->writeRegister(st.hw->addr, st.reg->s4_ctrl, div); return 0; } /** * @brief Select which sensors are pushed to FIFO. * @e sensors can contain a combination of the following flags: * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO * \n INV_XYZ_GYRO * \n INV_XYZ_ACCEL * @param[in] sensors Mask of sensors to push to FIFO. * @return 0 if successful. */ int16_t AP_InertialSensor_MPU9150::mpu_configure_fifo(uint8_t sensors) { int16_t result = 0; /* Compass data isn't going into the FIFO. Stop trying. */ sensors &= ~INV_XYZ_COMPASS; // Enable or disable the interrupts // set_int_enable(1); set_int_enable(0); if (sensors) { if (mpu_reset_fifo(sensors)) { return -1; } } return result; } /** * @brief Enable/disable data ready interrupt. * If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready * interrupt is used. * @param[in] enable 1 to enable interrupt. * @return 0 if successful. */ int16_t AP_InertialSensor_MPU9150::set_int_enable(uint8_t enable) { uint8_t tmp; if (enable){ tmp = BIT_DATA_RDY_EN; } else { tmp = 0x00; } hal.i2c->writeRegister(st.hw->addr, st.reg->int_enable, tmp); return 0; } /** * @brief Reset FIFO read/write pointers. * @return 0 if successful. */ int16_t AP_InertialSensor_MPU9150::mpu_reset_fifo(uint8_t sensors) { uint8_t data; data = 0; hal.i2c->writeRegister(st.hw->addr,st.reg->int_enable, data); hal.i2c->writeRegister(st.hw->addr,st.reg->fifo_en, data); hal.i2c->writeRegister(st.hw->addr,st.reg->user_ctrl, data); data = BIT_FIFO_RST; hal.i2c->writeRegister(st.hw->addr,st.reg->user_ctrl, data); data = BIT_FIFO_EN; // data = BIT_FIFO_EN | BIT_AUX_IF_EN; hal.i2c->writeRegister(st.hw->addr,st.reg->user_ctrl, data); hal.scheduler->delay(50); // interrupts for the DMP // data = BIT_DATA_RDY_EN; data = 0; hal.i2c->writeRegister(st.hw->addr,st.reg->int_enable, data); // enable FIFO hal.i2c->writeRegister(st.hw->addr,st.reg->fifo_en, sensors); return 0; } #if 0 /* This initialization is similar to the one in ak8975.c. TODO: Use the ArduPilot APIs (write, read, ...), remove the st.chip_cfg cache vars */ static int AP_InertialSensor_MPU9150::setup_compass(void) { uint8_t data[4], akm_addr; mpu_set_bypass(1); /* Find compass. Possible addresses range from 0x0C to 0x0F. */ for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) { int result; result = i2c_read(akm_addr, AKM_REG_WHOAMI, 1, data); if (!result && (data[0] == AKM_WHOAMI)) break; } if (akm_addr > 0x0F) { /* TODO: Handle this case in all compass-related functions. */ log_e("Compass not found.\n"); return -1; } st.chip_cfg.compass_addr = akm_addr; data[0] = AKM_POWER_DOWN; if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) return -1; delay_ms(1); data[0] = AKM_FUSE_ROM_ACCESS; if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) return -1; delay_ms(1); /* Get sensitivity adjustment data from fuse ROM. */ if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ASAX, 3, data)) return -1; st.chip_cfg.mag_sens_adj[0] = (long)data[0] + 128; st.chip_cfg.mag_sens_adj[1] = (long)data[1] + 128; st.chip_cfg.mag_sens_adj[2] = (long)data[2] + 128; data[0] = AKM_POWER_DOWN; if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) return -1; delay_ms(1); mpu_set_bypass(0); /* Set up master mode, master clock, and ES bit. */ data[0] = 0x40; if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data)) return -1; /* Slave 0 reads from AKM data registers. */ data[0] = BIT_I2C_READ | st.chip_cfg.compass_addr; if (i2c_write(st.hw->addr, st.reg->s0_addr, 1, data)) return -1; /* Compass reads start at this register. */ data[0] = AKM_REG_ST1; if (i2c_write(st.hw->addr, st.reg->s0_reg, 1, data)) return -1; /* Enable slave 0, 8-byte reads. */ data[0] = BIT_SLAVE_EN | 8; if (i2c_write(st.hw->addr, st.reg->s0_ctrl, 1, data)) return -1; /* Slave 1 changes AKM measurement mode. */ data[0] = st.chip_cfg.compass_addr; if (i2c_write(st.hw->addr, st.reg->s1_addr, 1, data)) return -1; /* AKM measurement mode register. */ data[0] = AKM_REG_CNTL; if (i2c_write(st.hw->addr, st.reg->s1_reg, 1, data)) return -1; /* Enable slave 1, 1-byte writes. */ data[0] = BIT_SLAVE_EN | 1; if (i2c_write(st.hw->addr, st.reg->s1_ctrl, 1, data)) return -1; /* Set slave 1 data. */ data[0] = AKM_SINGLE_MEASUREMENT; if (i2c_write(st.hw->addr, st.reg->s1_do, 1, data)) return -1; /* Trigger slave 0 and slave 1 actions at each sample. */ data[0] = 0x03; if (i2c_write(st.hw->addr, st.reg->i2c_delay_ctrl, 1, data)) return -1; /* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. */ data[0] = BIT_I2C_MST_VDDIO; if (i2c_write(st.hw->addr, st.reg->yg_offs_tc, 1, data)) return -1; return 0; } #endif /** * @brief Turn specific sensors on/off. * @e sensors can contain a combination of the following flags: * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO * \n INV_XYZ_GYRO * \n INV_XYZ_ACCEL * \n INV_XYZ_COMPASS * @param[in] sensors Mask of sensors to wake. * @return 0 if successful. */ int16_t AP_InertialSensor_MPU9150::mpu_set_sensors(uint8_t sensors) { uint8_t data; // uint8_t user_ctrl; if (sensors & INV_XYZ_GYRO){ data = INV_CLK_PLL; } else if (sensors){ data = 0; } else { data = BIT_SLEEP; } hal.i2c->writeRegister(st.hw->addr,st.reg->pwr_mgmt_1, data); data = 0; if (!(sensors & INV_X_GYRO)){ data |= BIT_STBY_XG; } if (!(sensors & INV_Y_GYRO)){ data |= BIT_STBY_YG; } if (!(sensors & INV_Z_GYRO)){ data |= BIT_STBY_ZG; } if (!(sensors & INV_XYZ_ACCEL)){ data |= BIT_STBY_XYZA; } hal.i2c->writeRegister(st.hw->addr,st.reg->pwr_mgmt_2, data); #if 0 if (sensors && (sensors != INV_XYZ_ACCEL)){ /* Latched interrupts only used in LP accel mode. */ mpu_set_int_latched(0); } #endif // // handle the compass, not implemented for now // #ifdef AK89xx_SECONDARY // #ifdef AK89xx_BYPASS // if (sensors & INV_XYZ_COMPASS) // mpu_set_bypass(1); // else // mpu_set_bypass(0); // #else // if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl)) // return -1; // /* Handle AKM power management. */ // if (sensors & INV_XYZ_COMPASS) { // data = AKM_SINGLE_MEASUREMENT; // user_ctrl |= BIT_AUX_IF_EN; // } else { // data = AKM_POWER_DOWN; // user_ctrl &= ~BIT_AUX_IF_EN; // } // if (st.chip_cfg.dmp_on) // user_ctrl |= BIT_DMP_EN; // else // user_ctrl &= ~BIT_DMP_EN; // if (i2c_write(st.hw->addr, st.reg->s1_do, 1, &data)) // return -1; // /* Enable/disable I2C master mode. */ // if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl)) // return -1; // #endif hal.scheduler->delay(50); return 0; } #if 0 /** * TODO: Remove the st.chip_cfg cache variables * * @brief Enable latched interrupts. * Any MPU register will clear the interrupt. * @param[in] enable 1 to enable, 0 to disable. * @return 0 if successful. */ int16_t AP_InertialSensor_MPU9150::mpu_set_int_latched(uint8_t enable) { uint8_t tmp; if (st.chip_cfg.latched_int == enable){ return 0; } if (enable){ tmp = BIT_LATCH_EN | BIT_ANY_RD_CLR; } else { tmp = 0; } if (st.chip_cfg.bypass_mode){ tmp |= BIT_BYPASS_EN; } if (st.chip_cfg.active_low_int){ tmp |= BIT_ACTL; } hal.i2c->writeRegister(st.hw->addr,st.reg->int_pin_cfg, tmp); st.chip_cfg.latched_int = enable; return 0; } #endif /** * @brief Get one packet from the FIFO. * If @e sensors does not contain a particular sensor, disregard the data * returned to that pointer. * \n @e sensors can contain a combination of the following flags: * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO * \n INV_XYZ_GYRO * \n INV_XYZ_ACCEL * \n If the FIFO has no new data, @e sensors will be zero. * \n If the FIFO is disabled, @e sensors will be zero and this function will * return a non-zero error code. * @param[out] gyro Gyro data in hardware units. * @param[out] accel Accel data in hardware units. * @param[out] timestamp Timestamp in milliseconds. * @param[out] sensors Mask of sensors read from FIFO. * @param[out] more Number of remaining packets. * @return 0 if successful. */ int16_t AP_InertialSensor_MPU9150::mpu_read_fifo(int16_t *gyro, int16_t *accel, uint32_t *timestamp, uint8_t *sensors, uint8_t *more) { /* Assumes maximum packet size is gyro (6) + accel (6). */ uint8_t data[12]; uint8_t packet_size = 0; uint16_t fifo_count, index = 0; uint8_t sensors_aux = INV_XYZ_ACCEL| INV_XYZ_GYRO; sensors[0] = 0; // We assume we want gyro and accel values packet_size = 12; // fifo_count_h register contains the number of samples in the FIFO hal.i2c->readRegisters(st.hw->addr, st.reg->fifo_count_h, 2, data); fifo_count = (data[0] << 8) | data[1]; if (fifo_count < packet_size){ return 0; } // hal.console->printf(PTR("FIFO count: %hd\n", fifo_count)); if (fifo_count > (st.hw->max_fifo >> 1)) { /* FIFO is 50% full, better check overflow bit. */ hal.i2c->readRegister(st.hw->addr, st.reg->int_status, data); if (data[0] & BIT_FIFO_OVERFLOW) { mpu_reset_fifo(sensors_aux); return -2; } } *timestamp = AP_HAL::millis(); // read the data hal.i2c->readRegisters(st.hw->addr, st.reg->fifo_r_w, packet_size, data); more[0] = fifo_count / packet_size - 1; sensors[0] = 0; if (index != packet_size) { accel[0] = (data[index+0] << 8) | data[index+1]; accel[1] = (data[index+2] << 8) | data[index+3]; accel[2] = (data[index+4] << 8) | data[index+5]; sensors[0] |= INV_XYZ_ACCEL; index += 6; } if (index != packet_size) { gyro[0] = (data[index+0] << 8) | data[index+1]; sensors[0] |= INV_X_GYRO; index += 2; } if (index != packet_size) { gyro[1] = (data[index+0] << 8) | data[index+1]; sensors[0] |= INV_Y_GYRO; index += 2; } if (index != packet_size) { gyro[2] = (data[index+0] << 8) | data[index+1]; sensors[0] |= INV_Z_GYRO; index += 2; } return 0; } /** * @brief Accumulate values from accels and gyros. * * This method is called periodically by the scheduler. */ void AP_InertialSensor_MPU9150::_accumulate(void) { // get pointer to i2c bus semaphore AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); // take i2c bus sempahore if (!i2c_sem->take_nonblocking()){ return; } // Read accelerometer FIFO to find out how many samples are available /* Assumes maximum packet size is gyro (6) + accel (6). */ uint8_t data[12]; uint8_t packet_size = 12; uint16_t fifo_count, index = 0; int16_t accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z; // fifo_count_h register contains the number of samples in the FIFO hal.i2c->readRegisters(st.hw->addr, st.reg->fifo_count_h, 2, data); fifo_count = (data[0] << 8) | data[1]; if (fifo_count < packet_size){ // give back i2c semaphore i2c_sem->give(); return; } // hal.console->printf(PTR("FIFO count: %hd\n", fifo_count)); if (fifo_count > (st.hw->max_fifo >> 1)) { /* FIFO is 50% full, better check overflow bit. */ hal.i2c->readRegister(st.hw->addr, st.reg->int_status, data); if (data[0] & BIT_FIFO_OVERFLOW) { mpu_reset_fifo(INV_XYZ_ACCEL| INV_XYZ_GYRO); i2c_sem->give(); return; } } // read the samples for (uint16_t i=0; i< fifo_count; i++) { Vector3f accel, gyro; // read the data // TODO check whether it's possible to read all the packages in a single call hal.i2c->readRegisters(st.hw->addr, st.reg->fifo_r_w, packet_size, data); // TODO, remove all the checking since it's being configured this way. if (index != packet_size) { accel_x = (int16_t) (data[index+0] << 8) | data[index+1]; accel_y = (int16_t) (data[index+2] << 8) | data[index+3]; accel_z = (int16_t) (data[index+4] << 8) | data[index+5]; index += 6; } if (index != packet_size) { gyro_x = (int16_t) (data[index+0] << 8) | data[index+1]; index += 2; } if (index != packet_size) { gyro_y = (int16_t) (data[index+0] << 8) | data[index+1]; index += 2; } if (index != packet_size) { gyro_z = (int16_t) (data[index+0] << 8) | data[index+1]; index += 2; } // reset the index index = 0; // TODO Revisit why AP_InertialSensor_L3G4200D uses a minus sign in the y and z component. Maybe this // is because the sensor is placed in the bottom side of the board? accel = Vector3f(accel_x, accel_y, accel_z); accel *= MPU9150_ACCEL_SCALE_2G; _rotate_and_correct_accel(_accel_instance, accel); _notify_new_accel_raw_sample(_accel_instance, accel); gyro = Vector3f(gyro_x, gyro_y, gyro_z); gyro *= MPU9150_GYRO_SCALE_2000; _rotate_and_correct_gyro(_gyro_instance, gyro); _notify_new_gyro_raw_sample(_gyro_instance, gyro); } // give back i2c semaphore i2c_sem->give(); } bool AP_InertialSensor_MPU9150::update(void) { update_gyro(_gyro_instance); update_accel(_accel_instance); return true; } #endif // CONFIG_HAL_BOARD