diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-21 15:19:19 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-21 15:19:19 +0200 |
commit | 309ea8146055528c22395fca06b7a70b660c22b3 (patch) | |
tree | 13478630a0543077da64db78ad59a972af1a5520 /src/modules/sensors | |
parent | 5a8dc9c504f70b4ce1b45f91b3bdd9b7126ef0d3 (diff) | |
parent | db1229dca338890c4aef26e17ebc622e5ba61b59 (diff) | |
download | px4-firmware-309ea8146055528c22395fca06b7a70b660c22b3.tar.gz px4-firmware-309ea8146055528c22395fca06b7a70b660c22b3.tar.bz2 px4-firmware-309ea8146055528c22395fca06b7a70b660c22b3.zip |
Merged fmuv2_bringup
Diffstat (limited to 'src/modules/sensors')
-rw-r--r-- | src/modules/sensors/sensors.cpp | 33 |
1 files changed, 24 insertions, 9 deletions
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b237bd059..ded39c465 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -912,11 +912,11 @@ Sensors::gyro_init() #else - /* set the gyro internal sampling rate up to at leat 800Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, 800); + /* set the gyro internal sampling rate up to at least 760Hz */ + ioctl(fd, GYROIOCSSAMPLERATE, 760); - /* set the driver to poll at 800Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 800); + /* set the driver to poll at 760Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 760); #endif @@ -929,6 +929,7 @@ void Sensors::mag_init() { int fd; + int ret; fd = open(MAG_DEVICE_PATH, 0); @@ -937,13 +938,27 @@ Sensors::mag_init() errx(1, "FATAL: no magnetometer found"); } - /* set the mag internal poll rate to at least 150Hz */ - ioctl(fd, MAGIOCSSAMPLERATE, 150); + /* try different mag sampling rates */ - /* set the driver to poll at 150Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 150); - int ret = ioctl(fd, MAGIOCGEXTERNAL, 0); + ret = ioctl(fd, MAGIOCSSAMPLERATE, 150); + if (ret == OK) { + /* set the pollrate accordingly */ + ioctl(fd, SENSORIOCSPOLLRATE, 150); + } else { + ret = ioctl(fd, MAGIOCSSAMPLERATE, 100); + /* if the slower sampling rate still fails, something is wrong */ + if (ret == OK) { + /* set the driver to poll also at the slower rate */ + ioctl(fd, SENSORIOCSPOLLRATE, 100); + } else { + errx(1, "FATAL: mag sampling rate could not be set"); + } + } + + + + ret = ioctl(fd, MAGIOCGEXTERNAL, 0); if (ret < 0) errx(1, "FATAL: unknown if magnetometer is external or onboard"); else if (ret == 1) |