Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 01:04:19 +08:00
Code Issues Actions 13 Packages Projects Releases Wiki Activity

mpu6000: add set_accel_range

Browse Source
This commit is contained in:
Jonathan Challinger
2015-02-16 18:19:17 -08:00
committed by Lorenz Meier
parent 1bbfe571fa
commit 89d5ae69d3
1 changed files with 41 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/mpu6000/mpu6000.cpp
+41 -72
View File
@@ -362,7 +362,7 @@ private:
* @param max_g The maximum G value the range must support.
* @return OK if the value can be supported, -ERANGE otherwise.
*/
int set_range(unsigned max_g);
int set_accel_range(unsigned max_g);
/**
* Swap a 16-bit value read from the MPU6000 to native byte order.
@@ -717,37 +717,7 @@ int MPU6000::reset()
_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;
// product-specific scaling
switch (_product) {
case MPU6000ES_REV_C4:
case MPU6000ES_REV_C5:
case MPU6000_REV_C4:
case MPU6000_REV_C5:
// Accel scale 8g (4096 LSB/g)
// Rev C has different scaling than rev D
write_checked_reg(MPUREG_ACCEL_CONFIG, 1 << 3);
break;
case MPU6000ES_REV_D6:
case MPU6000ES_REV_D7:
case MPU6000ES_REV_D8:
case MPU6000_REV_D6:
case MPU6000_REV_D7:
case MPU6000_REV_D8:
case MPU6000_REV_D9:
case MPU6000_REV_D10:
// default case to cope with new chip revisions, which
// presumably won't have the accel scaling bug
default:
// Accel scale 8g (4096 LSB/g)
write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3);
break;
}
// Correct accel scale factors of 4096 LSB/g
// scale to m/s^2 ( 1g = 9.81 m/s^2)
_accel_range_scale = (MPU6000_ONE_G / 4096.0f);
_accel_range_m_s2 = 8.0f * MPU6000_ONE_G;
set_accel_range(8);
usleep(1000);
@@ -1301,11 +1271,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
return OK;
case ACCELIOCSRANGE:
/* XXX not implemented */
// XXX change these two values on set:
// _accel_range_scale = (9.81f / 4096.0f);
// _accel_range_m_s2 = 8.0f * 9.81f;
return -EINVAL;
return set_accel_range(arg);
case ACCELIOCGRANGE:
return (unsigned long)((_accel_range_m_s2)/MPU6000_ONE_G + 0.5f);
@@ -1455,44 +1422,46 @@ MPU6000::write_checked_reg(unsigned reg, uint8_t value)
}
int
MPU6000::set_range(unsigned max_g)
MPU6000::set_accel_range(unsigned max_g_in)
{
#if 0
uint8_t rangebits;
float rangescale;
if (max_g > 16) {
return -ERANGE;
} else if (max_g > 8) { /* 16G */
rangebits = OFFSET_LSB1_RANGE_16G;
rangescale = 1.98;
} else if (max_g > 4) { /* 8G */
rangebits = OFFSET_LSB1_RANGE_8G;
rangescale = 0.99;
} else if (max_g > 3) { /* 4G */
rangebits = OFFSET_LSB1_RANGE_4G;
rangescale = 0.5;
} else if (max_g > 2) { /* 3G */
rangebits = OFFSET_LSB1_RANGE_3G;
rangescale = 0.38;
} else if (max_g > 1) { /* 2G */
rangebits = OFFSET_LSB1_RANGE_2G;
rangescale = 0.25;
} else { /* 1G */
rangebits = OFFSET_LSB1_RANGE_1G;
rangescale = 0.13;
// workaround for bugged versions of MPU6k (rev C)
switch (_product) {
case MPU6000ES_REV_C4:
case MPU6000ES_REV_C5:
case MPU6000_REV_C4:
case MPU6000_REV_C5:
write_checked_reg(MPUREG_ACCEL_CONFIG, 1 << 3);
_accel_range_scale = (MPU6000_ONE_G / 4096.0f);
_accel_range_m_s2 = 8.0f * MPU6000_ONE_G;
return OK;
}
/* adjust sensor configuration */
modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits);
_range_scale = rangescale;
#endif
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;
}
write_checked_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3);
_accel_range_scale = (MPU6000_ONE_G / lsb_per_g);
_accel_range_m_s2 = max_accel_g * MPU6000_ONE_G;
return OK;
}