From a4961092af2d0e09a3dbe04e8c2f0a362476d21d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 19:19:23 +0100 Subject: mpu6000: check for default sample rate Also check if input variable is 0 and fix indentation --- src/drivers/mpu6000/mpu6000.cpp | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) (limited to 'src/drivers/mpu6000') 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; } /* -- cgit v1.2.3