/* set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro For ICM20948 accel and gyro samplerates are both set to the same value. */ void MPU9250::_set_sample_rate(unsigned desired_sample_rate_hz) { uint8_t div = 1; if (desired_sample_rate_hz == 0) { desired_sample_rate_hz = MPU9250_GYRO_DEFAULT_RATE; } switch (_whoami) { case MPU_WHOAMI_9250: case MPU_WHOAMI_6500: div = 1000 / desired_sample_rate_hz; break; case ICM_WHOAMI_20948: div = 1100 / desired_sample_rate_hz; break; } if (div > 200) { div = 200; } if (div < 1) { div = 1; } switch (_whoami) { case MPU_WHOAMI_9250: case MPU_WHOAMI_6500: write_checked_reg(MPUREG_SMPLRT_DIV, div - 1); _sample_rate = 1000 / div; break; case ICM_WHOAMI_20948: write_checked_reg(ICMREG_20948_GYRO_SMPLRT_DIV, div - 1); // There's also an MSB for this allowing much higher dividers for the accelerometer. // For 1 < div < 200 the LSB is sufficient. write_checked_reg(ICMREG_20948_ACCEL_SMPLRT_DIV_2, div - 1); _sample_rate = 1100 / div; break; } }
/* set the DLPF filter frequency. This affects both accel and gyro. */ void MPU9250::_set_dlpf_filter(uint16_t frequency_hz) { uint8_t filter; switch (_whoami) { case MPU_WHOAMI_9250: case MPU_WHOAMI_6500: /* choose next highest filter frequency available */ if (frequency_hz == 0) { _dlpf_freq = 0; filter = BITS_DLPF_CFG_3600HZ; } else if (frequency_hz <= 5) { _dlpf_freq = 5; filter = BITS_DLPF_CFG_5HZ; } else if (frequency_hz <= 10) { _dlpf_freq = 10; filter = BITS_DLPF_CFG_10HZ; } else if (frequency_hz <= 20) { _dlpf_freq = 20; filter = BITS_DLPF_CFG_20HZ; } else if (frequency_hz <= 41) { _dlpf_freq = 41; filter = BITS_DLPF_CFG_41HZ; } else if (frequency_hz <= 92) { _dlpf_freq = 92; filter = BITS_DLPF_CFG_92HZ; } else if (frequency_hz <= 184) { _dlpf_freq = 184; filter = BITS_DLPF_CFG_184HZ; } else if (frequency_hz <= 250) { _dlpf_freq = 250; filter = BITS_DLPF_CFG_250HZ; } else { _dlpf_freq = 0; filter = BITS_DLPF_CFG_3600HZ; } write_checked_reg(MPUREG_CONFIG, filter); break; } }
int BMI055_accel::reset() { write_reg(BMI055_ACC_SOFTRESET, BMI055_SOFT_RESET);//Soft-reset up_udelay(5000); write_checked_reg(BMI055_ACC_BW, BMI055_ACCEL_BW_1000); //Write accel bandwidth write_checked_reg(BMI055_ACC_RANGE, BMI055_ACCEL_RANGE_2_G);//Write range write_checked_reg(BMI055_ACC_INT_EN_1, BMI055_ACC_DRDY_INT_EN); //Enable DRDY interrupt write_checked_reg(BMI055_ACC_INT_MAP_1, BMI055_ACC_DRDY_INT1); //Map DRDY interrupt on pin INT1 set_accel_range(BMI055_ACCEL_DEFAULT_RANGE_G);//set accel range accel_set_sample_rate(BMI055_ACCEL_DEFAULT_RATE);//set accel ODR //Enable Accelerometer in normal mode write_reg(BMI055_ACC_PMU_LPW, BMI055_ACCEL_NORMAL); up_udelay(1000); uint8_t retries = 10; while (retries--) { bool all_ok = true; for (uint8_t i = 0; i < BMI055_ACCEL_NUM_CHECKED_REGISTERS; i++) { if (read_reg(_checked_registers[i]) != _checked_values[i]) { write_reg(_checked_registers[i], _checked_values[i]); all_ok = false; } } if (all_ok) { break; } } _accel_reads = 0; return OK; }
int BMI055_gyro::reset() { write_reg(BMI055_GYR_SOFTRESET, BMI055_SOFT_RESET);//Soft-reset usleep(5000); write_checked_reg(BMI055_GYR_BW, 0); // Write Gyro Bandwidth write_checked_reg(BMI055_GYR_RANGE, 0);// Write Gyro range write_checked_reg(BMI055_GYR_INT_EN_0, BMI055_GYR_DRDY_INT_EN); //Enable DRDY interrupt write_checked_reg(BMI055_GYR_INT_MAP_1, BMI055_GYR_DRDY_INT1); //Map DRDY interrupt on pin INT1 set_gyro_range(BMI055_GYRO_DEFAULT_RANGE_DPS);// set Gyro range gyro_set_sample_rate(BMI055_GYRO_DEFAULT_RATE);// set Gyro ODR //Enable Gyroscope in normal mode write_reg(BMI055_GYR_LPM1, BMI055_GYRO_NORMAL); up_udelay(1000); uint8_t retries = 10; while (retries--) { bool all_ok = true; for (uint8_t i = 0; i < BMI055_GYRO_NUM_CHECKED_REGISTERS; i++) { if (read_reg(_checked_registers[i]) != _checked_values[i]) { write_reg(_checked_registers[i], _checked_values[i]); all_ok = false; } } if (all_ok) { break; } } _gyro_reads = 0; return OK; }
void FXOS8700CQ::reset() { /* enable accel set it To Standby */ write_checked_reg(FXOS8700CQ_CTRL_REG1, 0); write_checked_reg(FXOS8700CQ_XYZ_DATA_CFG, 0); /* Use hybird mode to read Accel and Mag */ write_checked_reg(FXOS8700CQ_M_CTRL_REG1, M_CTRL_REG1_HMS_AM | M_CTRL_REG1_OS(7)); /* Use the hybird auto increment mode to read all the data at the same time */ write_checked_reg(FXOS8700CQ_M_CTRL_REG2, CTRL_REG2_AUTO_INC); accel_set_range(FXOS8700C_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(FXOS8700C_ACCEL_DEFAULT_RATE); accel_set_driver_lowpass_filter((float)FXOS8700C_ACCEL_DEFAULT_RATE, (float)FXOS8700C_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); // we setup the anti-alias on-chip filter as 50Hz. We believe // this operates in the analog domain, and is critical for // anti-aliasing. The 2 pole software filter is designed to // operate in conjunction with this on-chip filter accel_set_onchip_lowpass_filter_bandwidth(FXOS8700C_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); mag_set_range(FXOS8700C_MAG_DEFAULT_RANGE_GA); /* enable set it To Standby mode at 800 Hz which becomes 400 Hz due to hybird mode */ write_checked_reg(FXOS8700CQ_CTRL_REG1, CTRL_REG1_DR(0) | CTRL_REG1_ACTIVE); _accel_read = 0; _mag_read = 0; }
int FXAS21002C::set_samplerate(unsigned frequency) { uint8_t bits = CTRL_REG1_READY | CTRL_REG1_ACTIVE; if (frequency == 0 || frequency == GYRO_SAMPLERATE_DEFAULT) { frequency = FXAS21002C_DEFAULT_RATE; } if (frequency <= 13) { _current_rate = 13; bits |= CTRL_REG_DR_12_5; } else if (frequency <= 25) { _current_rate = 25; bits |= CTRL_REG_DR_25HZ; } else if (frequency <= 50) { _current_rate = 50; bits |= CTRL_REG_DR_50HZ; } else if (frequency <= 100) { _current_rate = 100; bits |= CTRL_REG_DR_100HZ; } else if (frequency <= 200) { _current_rate = 200; bits |= CTRL_REG_DR_200HZ; } else if (frequency <= 400) { _current_rate = 400; bits |= CTRL_REG_DR_400HZ; } else if (frequency <= 800) { _current_rate = 800; bits |= CTRL_REG_DR_800HZ; } else { return -EINVAL; } write_checked_reg(FXAS21002C_CTRL_REG1, bits); return OK; }
/* set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro */ void MPU9250::_set_sample_rate(unsigned desired_sample_rate_hz) { if (desired_sample_rate_hz == 0 || desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { desired_sample_rate_hz = MPU9250_GYRO_DEFAULT_RATE; } uint8_t div = 1000 / desired_sample_rate_hz; if (div > 200) { div = 200; } if (div < 1) { div = 1; } write_checked_reg(MPUREG_SMPLRT_DIV, div - 1); _sample_rate = 1000 / div; }
int MPU9250::set_accel_range(unsigned max_g_in) { uint8_t afs_sel; float lsb_per_g; float max_accel_g; if (max_g_in > 8) { // 16g - AFS_SEL = 3 afs_sel = 3; lsb_per_g = 2048; max_accel_g = 16; } else if (max_g_in > 4) { // 8g - AFS_SEL = 2 afs_sel = 2; lsb_per_g = 4096; max_accel_g = 8; } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 afs_sel = 1; lsb_per_g = 8192; max_accel_g = 4; } else { // 2g - AFS_SEL = 0 afs_sel = 0; lsb_per_g = 16384; max_accel_g = 2; } switch (_whoami) { case MPU_WHOAMI_9250: case MPU_WHOAMI_6500: write_checked_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3); break; case ICM_WHOAMI_20948: modify_checked_reg(ICMREG_20948_ACCEL_CONFIG, ICM_BITS_ACCEL_FS_SEL_MASK, afs_sel << 1); break; } _accel_range_scale = (CONSTANTS_ONE_G / lsb_per_g); _accel_range_m_s2 = max_accel_g * CONSTANTS_ONE_G; return OK; }
void L3GD20::disable_i2c(void) { uint8_t retries = 10; while (retries--) { // add retries uint8_t a = read_reg(0x05); write_reg(0x05, (0x20 | a)); if (read_reg(0x05) == (a | 0x20)) { // this sets the I2C_DIS bit on the // L3GD20H. The l3gd20 datasheet doesn't // mention this register, but it does seem to // accept it. write_checked_reg(ADDR_LOW_ODR, 0x08); return; } } debug("FAILED TO DISABLE I2C"); }
int IIS328DQ::set_samplerate(unsigned frequency, unsigned bandwidth) { uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; if (frequency == 0 || frequency == IIS328DQ_DEFAULT_RATE) { frequency = 1000; } /* * Use limits good for H or non-H models. Rates are slightly different * for L3G4200D part but register settings are the same. */ if (frequency <= 50) { _accel_samplerate =50; _accel_onchip_filter_bandwidth = 37; bits |= RATE_50HZ_LP_37HZ; } else if (frequency <= 100) { _accel_samplerate = 100; _accel_onchip_filter_bandwidth = 70; bits |= RATE_100HZ_LP_74HZ; } else if (frequency <= 400) { _accel_samplerate = 400; _accel_onchip_filter_bandwidth = 292; bits |= RATE_400HZ_LP_292HZ; } else if (frequency <= 1000) { _accel_samplerate = 1000; _accel_onchip_filter_bandwidth = 780; bits |= RATE_1000HZ_LP_780HZ; } else { return -EINVAL; } write_checked_reg(ADDR_CTRL_REG1, bits); return OK; }
void IIS328DQ::disable_i2c(void) { #if 0 uint8_t retries = 10; while (retries--) { // add retries uint8_t a = read_reg(0x05); write_reg(0x05, (0x20 | a)); if (read_reg(0x05) == (a | 0x20)) { // this sets the I2C_DIS bit on the // IIS328DQ. The l3gd20 datasheet doesn't // mention this register, but it does seem to // accept it. write_checked_reg(ADDR_LOW_ODR, 0x08); return; } } DEVICE_DEBUG("FAILED TO DISABLE I2C"); #endif }
int L3GD20::set_samplerate(unsigned frequency) { uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; if (frequency == 0 || frequency == GYRO_SAMPLERATE_DEFAULT) { frequency = _is_l3g4200d ? 800 : 760; } /* * Use limits good for H or non-H models. Rates are slightly different * for L3G4200D part but register settings are the same. */ if (frequency <= 100) { _current_rate = _is_l3g4200d ? 100 : 95; bits |= RATE_95HZ_LP_25HZ; } else if (frequency <= 200) { _current_rate = _is_l3g4200d ? 200 : 190; bits |= RATE_190HZ_LP_50HZ; } else if (frequency <= 400) { _current_rate = _is_l3g4200d ? 400 : 380; bits |= RATE_380HZ_LP_50HZ; } else if (frequency <= 800) { _current_rate = _is_l3g4200d ? 800 : 760; bits |= RATE_760HZ_LP_50HZ; } else { return -EINVAL; } write_checked_reg(ADDR_CTRL_REG1, bits); return OK; }
int L3GD20::set_range(unsigned max_dps) { uint8_t bits = REG4_BDU; float new_range_scale_dps_digit; float new_range; if (max_dps == 0) { max_dps = 2000; } if (max_dps <= 250) { new_range = 250; bits |= RANGE_250DPS; new_range_scale_dps_digit = 8.75e-3f; } else if (max_dps <= 500) { new_range = 500; bits |= RANGE_500DPS; new_range_scale_dps_digit = 17.5e-3f; } else if (max_dps <= 2000) { new_range = 2000; bits |= RANGE_2000DPS; new_range_scale_dps_digit = 70e-3f; } else { return -EINVAL; } _gyro_range_rad_s = new_range / 180.0f * M_PI_F; _gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F; write_checked_reg(ADDR_CTRL_REG4, bits); return OK; }
int MPU9250::reset() { irqstate_t state; // Hold off sampling for 4 ms state = px4_enter_critical_section(); _reset_wait = hrt_absolute_time() + 10000; write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_AUTO); write_checked_reg(MPUREG_PWR_MGMT_2, 0); px4_leave_critical_section(state); usleep(1000); // Enable I2C bus or Disable I2C bus (recommended on data sheet) write_checked_reg(MPUREG_USER_CTRL, is_i2c() ? 0 : BIT_I2C_IF_DIS); // SAMPLE RATE _set_sample_rate(_sample_rate); // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) // was 90 Hz, but this ruins quality and does not improve the // system response _set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ); // Gyro scale 2000 deg/s () write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); // correct gyro scale factors // scale to rad/s in SI units // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s // scaling factor: // 1/(2^15)*(2000/180)*PI _gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); _gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F; set_accel_range(ACCEL_RANGE_G); // INT CFG => Interrupt on Data Ready write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready #ifdef USE_I2C bool bypass = !_mag->is_passthrough(); #else bool bypass = false; #endif /* INT: Clear on any read. * If this instance is for a device is on I2C bus the Mag will have an i2c interface * that it will use to access the either: a) the internal mag device on the internal I2C bus * or b) it could be used to access a downstream I2C devices connected to the chip on * it's AUX_{ASD|SCL} pins. In either case we need to disconnect (bypass) the internal master * controller that chip provides as a SPI to I2C bridge. * so bypass is true if the mag has an i2c non null interfaces. */ write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0)); write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ); uint8_t retries = 3; bool all_ok = false; while (!all_ok && retries--) { // Assume all checked values are as expected all_ok = true; uint8_t reg; for (uint8_t i = 0; i < MPU9250_NUM_CHECKED_REGISTERS; i++) { if ((reg = read_reg(_checked_registers[i])) != _checked_values[i]) { write_reg(_checked_registers[i], _checked_values[i]); PX4_ERR("Reg %d is:%d s/b:%d Tries:%d", _checked_registers[i], reg, _checked_values[i], retries); all_ok = false; } } } return all_ok ? OK : -EIO; }
int MPU9250::reset() { write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); up_udelay(10000); write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_AUTO); up_udelay(1000); write_checked_reg(MPUREG_PWR_MGMT_2, 0); up_udelay(1000); // SAMPLE RATE _set_sample_rate(_sample_rate); usleep(1000); // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) // was 90 Hz, but this ruins quality and does not improve the // system response _set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ); usleep(1000); // Gyro scale 2000 deg/s () write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); usleep(1000); // correct gyro scale factors // scale to rad/s in SI units // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s // scaling factor: // 1/(2^15)*(2000/180)*PI _gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); _gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F; set_accel_range(16); usleep(1000); // INT CFG => Interrupt on Data Ready write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready usleep(1000); #ifdef USE_I2C bool bypass = !_mag->is_passthrough(); #else bool bypass = false; #endif write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0)); // INT: Clear on any read, also use i2c bypass is master mode isn't needed usleep(1000); write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ); usleep(1000); uint8_t retries = 10; while (retries--) { bool all_ok = true; for (uint8_t i = 0; i < MPU9250_NUM_CHECKED_REGISTERS; i++) { if (read_reg(_checked_registers[i]) != _checked_values[i]) { write_reg(_checked_registers[i], _checked_values[i]); all_ok = false; } } if (all_ok) { break; } } return OK; }
/* set the DLPF filter frequency. This affects both accel and gyro. */ void MPU9250::_set_dlpf_filter(uint16_t frequency_hz) { uint8_t filter; switch (_whoami) { case MPU_WHOAMI_9250: case MPU_WHOAMI_6500: /* choose next highest filter frequency available */ if (frequency_hz == 0) { _dlpf_freq = 0; filter = BITS_DLPF_CFG_3600HZ; } else if (frequency_hz <= 5) { _dlpf_freq = 5; filter = BITS_DLPF_CFG_5HZ; } else if (frequency_hz <= 10) { _dlpf_freq = 10; filter = BITS_DLPF_CFG_10HZ; } else if (frequency_hz <= 20) { _dlpf_freq = 20; filter = BITS_DLPF_CFG_20HZ; } else if (frequency_hz <= 41) { _dlpf_freq = 41; filter = BITS_DLPF_CFG_41HZ; } else if (frequency_hz <= 92) { _dlpf_freq = 92; filter = BITS_DLPF_CFG_92HZ; } else if (frequency_hz <= 184) { _dlpf_freq = 184; filter = BITS_DLPF_CFG_184HZ; } else if (frequency_hz <= 250) { _dlpf_freq = 250; filter = BITS_DLPF_CFG_250HZ; } else { _dlpf_freq = 0; filter = BITS_DLPF_CFG_3600HZ; } write_checked_reg(MPUREG_CONFIG, filter); break; case ICM_WHOAMI_20948: /* choose next highest filter frequency available for gyroscope */ if (frequency_hz == 0) { _dlpf_freq_icm_gyro = 0; filter = ICM_BITS_GYRO_DLPF_CFG_361HZ; } else if (frequency_hz <= 5) { _dlpf_freq_icm_gyro = 5; filter = ICM_BITS_GYRO_DLPF_CFG_5HZ; } else if (frequency_hz <= 11) { _dlpf_freq_icm_gyro = 11; filter = ICM_BITS_GYRO_DLPF_CFG_11HZ; } else if (frequency_hz <= 23) { _dlpf_freq_icm_gyro = 23; filter = ICM_BITS_GYRO_DLPF_CFG_23HZ; } else if (frequency_hz <= 51) { _dlpf_freq_icm_gyro = 51; filter = ICM_BITS_GYRO_DLPF_CFG_51HZ; } else if (frequency_hz <= 119) { _dlpf_freq_icm_gyro = 119; filter = ICM_BITS_GYRO_DLPF_CFG_119HZ; } else if (frequency_hz <= 151) { _dlpf_freq_icm_gyro = 151; filter = ICM_BITS_GYRO_DLPF_CFG_151HZ; } else if (frequency_hz <= 197) { _dlpf_freq_icm_gyro = 197; filter = ICM_BITS_GYRO_DLPF_CFG_197HZ; } else { _dlpf_freq_icm_gyro = 0; filter = ICM_BITS_GYRO_DLPF_CFG_361HZ; } write_checked_reg(ICMREG_20948_GYRO_CONFIG_1, filter); /* choose next highest filter frequency available for accelerometer */ if (frequency_hz == 0) { _dlpf_freq_icm_accel = 0; filter = ICM_BITS_ACCEL_DLPF_CFG_473HZ; } else if (frequency_hz <= 5) { _dlpf_freq_icm_accel = 5; filter = ICM_BITS_ACCEL_DLPF_CFG_5HZ; } else if (frequency_hz <= 11) { _dlpf_freq_icm_accel = 11; filter = ICM_BITS_ACCEL_DLPF_CFG_11HZ; } else if (frequency_hz <= 23) { _dlpf_freq_icm_accel = 23; filter = ICM_BITS_ACCEL_DLPF_CFG_23HZ; } else if (frequency_hz <= 50) { _dlpf_freq_icm_accel = 50; filter = ICM_BITS_ACCEL_DLPF_CFG_50HZ; } else if (frequency_hz <= 111) { _dlpf_freq_icm_accel = 111; filter = ICM_BITS_ACCEL_DLPF_CFG_111HZ; } else if (frequency_hz <= 246) { _dlpf_freq_icm_accel = 246; filter = ICM_BITS_ACCEL_DLPF_CFG_246HZ; } else { _dlpf_freq_icm_accel = 0; filter = ICM_BITS_ACCEL_DLPF_CFG_473HZ; } write_checked_reg(ICMREG_20948_ACCEL_CONFIG, filter); break; } }
int MPU9250::reset_mpu() { uint8_t retries; switch (_whoami) { case MPU_WHOAMI_9250: case MPU_WHOAMI_6500: write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_AUTO); write_checked_reg(MPUREG_PWR_MGMT_2, 0); usleep(1000); break; } // Enable I2C bus or Disable I2C bus (recommended on data sheet) write_checked_reg(MPUREG_USER_CTRL, is_i2c() ? 0 : BIT_I2C_IF_DIS); // SAMPLE RATE _set_sample_rate(_sample_rate); _set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ); // Gyro scale 2000 deg/s () switch (_whoami) { case MPU_WHOAMI_9250: case MPU_WHOAMI_6500: write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); break; } // correct gyro scale factors // scale to rad/s in SI units // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s // scaling factor: // 1/(2^15)*(2000/180)*PI _gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); _gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F; set_accel_range(ACCEL_RANGE_G); // INT CFG => Interrupt on Data Ready write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready #ifdef USE_I2C bool bypass = !_mag->is_passthrough(); #else bool bypass = false; #endif /* INT: Clear on any read. * If this instance is for a device is on I2C bus the Mag will have an i2c interface * that it will use to access the either: a) the internal mag device on the internal I2C bus * or b) it could be used to access a downstream I2C devices connected to the chip on * it's AUX_{ASD|SCL} pins. In either case we need to disconnect (bypass) the internal master * controller that chip provides as a SPI to I2C bridge. * so bypass is true if the mag has an i2c non null interfaces. */ write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0)); write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ); retries = 3; bool all_ok = false; while (!all_ok && retries--) { // Assume all checked values are as expected all_ok = true; uint8_t reg; uint8_t bankcheck = 0; for (uint8_t i = 0; i < _num_checked_registers; i++) { if ((reg = read_reg(_checked_registers[i])) != _checked_values[i]) { write_reg(_checked_registers[i], _checked_values[i]); PX4_ERR("Reg %d is:%d s/b:%d Tries:%d - bank s/b %d, is %d", _checked_registers[i], reg, _checked_values[i], retries, REG_BANK(_checked_registers[i]), bankcheck); all_ok = false; } } } return all_ok ? OK : -EIO; }
void FXOS8700CQ::measure() { /* status register and data as read back from the device */ #pragma pack(push, 1) struct { uint8_t cmd[2]; uint8_t status; int16_t x; int16_t y; int16_t z; int16_t mx; int16_t my; int16_t mz; } raw_accel_mag_report; #pragma pack(pop) accel_report accel_report; /* start the performance counter */ perf_begin(_accel_sample_perf); check_registers(); if (_register_wait != 0) { // we are waiting for some good transfers before using // the sensor again. _register_wait--; perf_end(_accel_sample_perf); return; } /* fetch data from the sensor */ memset(&raw_accel_mag_report, 0, sizeof(raw_accel_mag_report)); raw_accel_mag_report.cmd[0] = DIR_READ(FXOS8700CQ_DR_STATUS); raw_accel_mag_report.cmd[1] = ADDR_7(FXOS8700CQ_DR_STATUS); transfer((uint8_t *)&raw_accel_mag_report, (uint8_t *)&raw_accel_mag_report, sizeof(raw_accel_mag_report)); if (!(raw_accel_mag_report.status & DR_STATUS_ZYXDR)) { perf_end(_accel_sample_perf); perf_count(_accel_duplicates); return; } /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) * 3) Scale the statically calibrated values with a linear * dynamically obtained factor * * Note: the static sensor offset is the number the sensor outputs * at a nominally 'zero' input. Therefore the offset has to * be subtracted. * * Example: A gyro outputs a value of 74 at zero angular rate * the offset is 74 from the origin and subtracting * 74 from all measurements centers them around zero. */ accel_report.timestamp = hrt_absolute_time(); /* * Eight-bit 2’s complement sensor temperature value with 0.96 °C/LSB sensitivity. * Temperature data is only valid between –40 °C and 125 °C. The temperature sensor * output is only valid when M_CTRL_REG1[m_hms] > 0b00. Please note that the * temperature sensor is uncalibrated and its output for a given temperature will vary from * one device to the next */ write_checked_reg(FXOS8700CQ_M_CTRL_REG1, M_CTRL_REG1_HMS_A | M_CTRL_REG1_OS(7)); _last_temperature = (read_reg(FXOS8700CQ_TEMP)) * 0.96f; write_checked_reg(FXOS8700CQ_M_CTRL_REG1, M_CTRL_REG1_HMS_AM | M_CTRL_REG1_OS(7)); accel_report.temperature = _last_temperature; // report the error count as the sum of the number of bad // register reads and bad values. This allows the higher level // code to decide if it should use this sensor based on // whether it has had failures accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); accel_report.x_raw = swap16RightJustify14(raw_accel_mag_report.x); accel_report.y_raw = swap16RightJustify14(raw_accel_mag_report.y); accel_report.z_raw = swap16RightJustify14(raw_accel_mag_report.z); /* Save off the Mag readings todo: revist integrating theses */ _last_raw_mag_x = swap16(raw_accel_mag_report.mx); _last_raw_mag_y = swap16(raw_accel_mag_report.my); _last_raw_mag_z = swap16(raw_accel_mag_report.mz); float xraw_f = accel_report.x_raw; float yraw_f = accel_report.y_raw; float zraw_f = accel_report.z_raw; // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; /* we have logs where the accelerometers get stuck at a fixed large value. We want to detect this and mark the sensor as being faulty */ if (fabsf(_last_accel[0] - x_in_new) < 0.001f && fabsf(_last_accel[1] - y_in_new) < 0.001f && fabsf(_last_accel[2] - z_in_new) < 0.001f && fabsf(x_in_new) > 20 && fabsf(y_in_new) > 20 && fabsf(z_in_new) > 20) { _constant_accel_count += 1; } else { _constant_accel_count = 0; } if (_constant_accel_count > 100) { // we've had 100 constant accel readings with large // values. The sensor is almost certainly dead. We // will raise the error_count so that the top level // flight code will know to avoid this sensor, but // we'll still give the data so that it can be logged // and viewed perf_count(_bad_values); _constant_accel_count = 0; } _last_accel[0] = x_in_new; _last_accel[1] = y_in_new; _last_accel[2] = z_in_new; accel_report.x = _accel_filter_x.apply(x_in_new); accel_report.y = _accel_filter_y.apply(y_in_new); accel_report.z = _accel_filter_z.apply(z_in_new); math::Vector<3> aval(x_in_new, y_in_new, z_in_new); math::Vector<3> aval_integrated; bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt); accel_report.x_integral = aval_integrated(0); accel_report.y_integral = aval_integrated(1); accel_report.z_integral = aval_integrated(2); accel_report.scaling = _accel_range_scale; accel_report.range_m_s2 = _accel_range_m_s2; /* return device ID */ accel_report.device_id = _device_id.devid; _accel_reports->force(&accel_report); /* notify anyone waiting for data */ if (accel_notify) { poll_notify(POLLIN); if (!(_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); } } _accel_read++; /* stop the perf counter */ perf_end(_accel_sample_perf); }