diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-07-15 15:02:45 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-07-15 15:02:45 +0200 |
commit | bf2ff98856b7e6b107a7ec5bbde3b00e38713804 (patch) | |
tree | ad11c218b7524826e45c58257cdaac12e8a1ac53 /src/drivers/mpu6000 | |
parent | 88389ea2554c6f56a4fdd86cdd86a1e7b6affc21 (diff) | |
parent | 17338ca61aa8a58c92ae621de94240ddd22f28a2 (diff) | |
download | px4-firmware-bf2ff98856b7e6b107a7ec5bbde3b00e38713804.tar.gz px4-firmware-bf2ff98856b7e6b107a7ec5bbde3b00e38713804.tar.bz2 px4-firmware-bf2ff98856b7e6b107a7ec5bbde3b00e38713804.zip |
Merged master
Diffstat (limited to 'src/drivers/mpu6000')
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 30 |
1 files changed, 24 insertions, 6 deletions
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index df1958186..220842536 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2012 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -272,6 +272,11 @@ private: */ void _set_dlpf_filter(uint16_t frequency_hz); + /* + set sample rate (approximate) - 1kHz to 5Hz + */ + void _set_sample_rate(uint16_t desired_sample_rate_hz); + }; /** @@ -378,7 +383,8 @@ MPU6000::init() up_udelay(1000); // SAMPLE RATE - write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz + //write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz + _set_sample_rate(200); // default sample rate = 200Hz usleep(1000); // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) @@ -494,6 +500,18 @@ 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) +{ + uint8_t div = 1000 / desired_sample_rate_hz; + if(div>200) div=200; + if(div<1) div=1; + write_reg(MPUREG_SMPLRT_DIV, div-1); +} + +/* set the DLPF filter frequency. This affects both accel and gyro. */ void @@ -644,8 +662,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSSAMPLERATE: case ACCELIOCGSAMPLERATE: - /* XXX not implemented */ - return -EINVAL; + _set_sample_rate(arg); + return OK; case ACCELIOCSLOWPASS: case ACCELIOCGLOWPASS: @@ -702,8 +720,8 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSSAMPLERATE: case GYROIOCGSAMPLERATE: - /* XXX not implemented */ - return -EINVAL; + _set_sample_rate(arg); + return OK; case GYROIOCSLOWPASS: case GYROIOCGLOWPASS: |