Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 13:15:32 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

mpu6000 driver support for setting rate

Browse Source
This commit is contained in:
sergeil
2013-05-31 11:44:20 +02:00
parent 27ee36b204
commit 496127ca45
1 changed files with 24 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/mpu6000/mpu6000.cpp
+24 -6
View File
@@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -272,6 +272,11 @@ private:
*/ */
void _set_dlpf_filter(uint16_t frequency_hz); 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); up_udelay(1000);
// SAMPLE RATE // 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); usleep(1000);
// FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter)
@@ -493,6 +499,18 @@ MPU6000::probe()
return -EIO; return -EIO;
} }
/*
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. set the DLPF filter frequency. This affects both accel and gyro.
*/ */
@@ -644,8 +662,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
case ACCELIOCSSAMPLERATE: case ACCELIOCSSAMPLERATE:
case ACCELIOCGSAMPLERATE: case ACCELIOCGSAMPLERATE:
/* XXX not implemented */ _set_sample_rate(arg);
return -EINVAL; return OK;
case ACCELIOCSLOWPASS: case ACCELIOCSLOWPASS:
case ACCELIOCGLOWPASS: case ACCELIOCGLOWPASS:
@@ -702,8 +720,8 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
case GYROIOCSSAMPLERATE: case GYROIOCSSAMPLERATE:
case GYROIOCGSAMPLERATE: case GYROIOCGSAMPLERATE:
/* XXX not implemented */ _set_sample_rate(arg);
return -EINVAL; return OK;
case GYROIOCSLOWPASS: case GYROIOCSLOWPASS:
case GYROIOCGLOWPASS: case GYROIOCGLOWPASS: