/*
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);
}
