diff options
author | px4dev <px4@purgatory.org> | 2012-08-29 23:16:38 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-08-29 23:16:38 -0700 |
commit | da3cd2435236d72f60b5b37e8736fd7b0aef7763 (patch) | |
tree | 5cb39e6db926397ca2607227befcb3213dbb3827 /apps/drivers/l3gd20/l3gd20.cpp | |
parent | d0898cb947b9287e7b25e7d1dad482a9a57f2b0f (diff) | |
download | px4-firmware-da3cd2435236d72f60b5b37e8736fd7b0aef7763.tar.gz px4-firmware-da3cd2435236d72f60b5b37e8736fd7b0aef7763.tar.bz2 px4-firmware-da3cd2435236d72f60b5b37e8736fd7b0aef7763.zip |
Be a bit more careful with which bits we stuff into the l3gd20 registers.
Diffstat (limited to 'apps/drivers/l3gd20/l3gd20.cpp')
-rw-r--r-- | apps/drivers/l3gd20/l3gd20.cpp | 98 |
1 files changed, 51 insertions, 47 deletions
diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp index 6caebeac5..b874c88fe 100644 --- a/apps/drivers/l3gd20/l3gd20.cpp +++ b/apps/drivers/l3gd20/l3gd20.cpp @@ -76,36 +76,47 @@ static const int ERROR = -1; #define ADDR_INCREMENT (1<<6) /* register addresses */ -#define ADDR_WHO_AM_I 0x0F +#define ADDR_WHO_AM_I 0x0F #define WHO_I_AM 0xD4 -#define ADDR_CTRL_REG1 0x20 -#define ADDR_CTRL_REG2 0x21 -#define ADDR_CTRL_REG3 0x22 -#define ADDR_CTRL_REG4 0x23 -#define ADDR_CTRL_REG5 0x24 -#define ADDR_REFERENCE 0x25 -#define ADDR_OUT_TEMP 0x26 -#define ADDR_STATUS_REG 0x27 -#define ADDR_OUT_X_L 0x28 -#define ADDR_OUT_X_H 0x29 -#define ADDR_OUT_Y_L 0x2A -#define ADDR_OUT_Y_H 0x2B -#define ADDR_OUT_Z_L 0x2C -#define ADDR_OUT_Z_H 0x2D -#define ADDR_FIFO_CTRL_REG 0x2E -#define ADDR_FIFO_SRC_REG 0x2F -#define ADDR_INT1_CFG 0x30 -#define ADDR_INT1_SRC 0x31 -#define ADDR_INT1_TSH_XH 0x32 -#define ADDR_INT1_TSH_XL 0x33 -#define ADDR_INT1_TSH_YH 0x34 -#define ADDR_INT1_TSH_YL 0x35 -#define ADDR_INT1_TSH_ZH 0x36 -#define ADDR_INT1_TSH_ZL 0x37 -#define ADDR_INT1_DURATION 0x38 +#define ADDR_CTRL_REG1 0x20 #define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */ +/* keep lowpass low to avoid noise issues */ +#define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) +#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) +#define RATE_380HZ_LP_30HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) +#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) + +#define ADDR_CTRL_REG2 0x21 +#define ADDR_CTRL_REG3 0x22 +#define ADDR_CTRL_REG4 0x23 #define REG4_RANGE_MASK 0x30 /* Mask to guard partial register update */ +#define RANGE_250DPS (0<<4) +#define RANGE_500DPS (1<<4) +#define RANGE_2000DPS (3<<4) + +#define ADDR_CTRL_REG5 0x24 +#define ADDR_REFERENCE 0x25 +#define ADDR_OUT_TEMP 0x26 +#define ADDR_STATUS_REG 0x27 +#define ADDR_OUT_X_L 0x28 +#define ADDR_OUT_X_H 0x29 +#define ADDR_OUT_Y_L 0x2A +#define ADDR_OUT_Y_H 0x2B +#define ADDR_OUT_Z_L 0x2C +#define ADDR_OUT_Z_H 0x2D +#define ADDR_FIFO_CTRL_REG 0x2E +#define ADDR_FIFO_SRC_REG 0x2F +#define ADDR_INT1_CFG 0x30 +#define ADDR_INT1_SRC 0x31 +#define ADDR_INT1_TSH_XH 0x32 +#define ADDR_INT1_TSH_XL 0x33 +#define ADDR_INT1_TSH_YH 0x34 +#define ADDR_INT1_TSH_YL 0x35 +#define ADDR_INT1_TSH_ZH 0x36 +#define ADDR_INT1_TSH_ZL 0x37 +#define ADDR_INT1_DURATION 0x38 + /* Internal configuration values */ #define REG1_POWER_NORMAL (1<<3) @@ -135,15 +146,8 @@ static const int ERROR = -1; #define FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5) #define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7) -#define L3GD20_RANGE_250DPS (0<<4) -#define L3GD20_RANGE_500DPS (1<<4) -#define L3GD20_RANGE_2000DPS (3<<4) -/* keep lowpass low to avoid noise issues */ -#define L3GD20_RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) -#define L3GD20_RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) -#define L3GD20_RATE_380HZ_LP_30HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) -#define L3GD20_RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) + extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } @@ -333,7 +337,7 @@ L3GD20::init() write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ - write_reg(ADDR_CTRL_REG4, 0x10); + write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ @@ -442,7 +446,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ - _call.period = _call_interval; + _call.period = _call_interval = ticks; /* if we need to start the poll state machine, do it */ if (want_start) @@ -559,27 +563,27 @@ L3GD20::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) int L3GD20::set_range(unsigned max_dps) { - uint8_t bits; + uint8_t bits = REG4_BDU; if (max_dps == 0) max_dps = 2000; if (max_dps <= 250) { _current_range = 250; - bits = L3GD20_RANGE_250DPS; + bits |= RANGE_250DPS; } else if (max_dps <= 500) { _current_range = 500; - bits = L3GD20_RANGE_500DPS; + bits |= RANGE_500DPS; } else if (max_dps <= 2000) { _current_range = 2000; - bits = L3GD20_RANGE_2000DPS; + bits |= RANGE_2000DPS; } else { return -EINVAL; } _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; _gyro_range_scale = _gyro_range_rad_s / 32768.0f; - modify_reg(ADDR_CTRL_REG4, REG4_RANGE_MASK, bits); + write_reg(ADDR_CTRL_REG4, bits); /* XXX update scale factors */ @@ -589,28 +593,28 @@ L3GD20::set_range(unsigned max_dps) int L3GD20::set_samplerate(unsigned frequency) { - uint8_t bits; + uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; if (frequency == 0) frequency = 760; if (frequency <= 95) { _current_rate = 95; - bits = L3GD20_RATE_95HZ_LP_25HZ; + bits |= RATE_95HZ_LP_25HZ; } else if (frequency <= 190) { _current_rate = 190; - bits = L3GD20_RATE_190HZ_LP_25HZ; + bits |= RATE_190HZ_LP_25HZ; } else if (frequency <= 380) { _current_rate = 380; - bits = L3GD20_RATE_380HZ_LP_30HZ; + bits |= RATE_380HZ_LP_30HZ; } else if (frequency <= 760) { _current_rate = 760; - bits = L3GD20_RATE_760HZ_LP_30HZ; + bits |= RATE_760HZ_LP_30HZ; } else { return -EINVAL; } - modify_reg(ADDR_CTRL_REG1, REG1_RATE_LP_MASK, bits); + write_reg(ADDR_CTRL_REG1, bits); return OK; } |