diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-28 19:19:23 +0100 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-03 18:13:01 +0100 |
| commit | a4961092af2d0e09a3dbe04e8c2f0a362476d21d (patch) | |
| tree | 8c7674278593ca6c6171f4c011c861b41a7f87da /src/drivers/mpu6000 | |
| parent | dedba4307de2b99ee3a9fd2d6c31c031bd4524ed (diff) | |
| download | px4-firmware-a4961092af2d0e09a3dbe04e8c2f0a362476d21d.tar.gz px4-firmware-a4961092af2d0e09a3dbe04e8c2f0a362476d21d.tar.bz2 px4-firmware-a4961092af2d0e09a3dbe04e8c2f0a362476d21d.zip | |
mpu6000: check for default sample rate
Also check if input variable is 0 and fix indentation
Diffstat (limited to 'src/drivers/mpu6000')
| -rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 21 |
1 files changed, 14 insertions, 7 deletions
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index e322e8b3a..b8c6cedb6 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -402,7 +402,7 @@ private: /* set sample rate (approximate) - 1kHz to 5Hz */ - void _set_sample_rate(uint16_t desired_sample_rate_hz); + void _set_sample_rate(unsigned desired_sample_rate_hz); /* check that key registers still have the right value @@ -794,13 +794,20 @@ MPU6000::probe() set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro */ void -MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz) +MPU6000::_set_sample_rate(unsigned desired_sample_rate_hz) { - 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; + if (desired_sample_rate_hz == 0 || + desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || + desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) + { + desired_sample_rate_hz = MPU6000_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; } /* |