aboutsummaryrefslogtreecommitdiff
path: root/apps/drivers/l3gd20/l3gd20.cpp
diff options
context:
space:
mode:
authorpx4dev <px4@purgatory.org>2012-10-23 23:38:45 -0700
committerpx4dev <px4@purgatory.org>2012-10-23 23:51:13 -0700
commit2fc10320697ecaa9c4e0c52d4d047424e41e6336 (patch)
tree4f18f494ab811e29dc55452f92a63fff9d271dda /apps/drivers/l3gd20/l3gd20.cpp
parent34f99c7dca1995f8ddd9e8d61c4cbd7289f40e99 (diff)
downloadpx4-firmware-2fc10320697ecaa9c4e0c52d4d047424e41e6336.tar.gz
px4-firmware-2fc10320697ecaa9c4e0c52d4d047424e41e6336.tar.bz2
px4-firmware-2fc10320697ecaa9c4e0c52d4d047424e41e6336.zip
Major formatting/whitespace cleanup
Diffstat (limited to 'apps/drivers/l3gd20/l3gd20.cpp')
-rw-r--r--apps/drivers/l3gd20/l3gd20.cpp30
1 files changed, 24 insertions, 6 deletions
diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp
index 4606cd654..4915b81e3 100644
--- a/apps/drivers/l3gd20/l3gd20.cpp
+++ b/apps/drivers/l3gd20/l3gd20.cpp
@@ -317,6 +317,7 @@ L3GD20::init()
_num_reports = 2;
_oldest_report = _next_report = 0;
_reports = new struct gyro_report[_num_reports];
+
if (_reports == nullptr)
goto out;
@@ -330,7 +331,7 @@ L3GD20::init()
write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */
write_reg(ADDR_CTRL_REG4, REG4_BDU);
write_reg(ADDR_CTRL_REG5, 0);
-
+
write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */
write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */
@@ -451,6 +452,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
case SENSORIOCGPOLLRATE:
if (_call_interval == 0)
return SENSOR_POLLRATE_MANUAL;
+
return 1000000 / _call_interval;
case SENSORIOCSQUEUEDEPTH: {
@@ -478,7 +480,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
}
case SENSORIOCGQUEUEDEPTH:
- return _num_reports -1;
+ return _num_reports - 1;
case SENSORIOCRESET:
/* XXX implement */
@@ -497,12 +499,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
case GYROIOCSSCALE:
/* copy scale in */
- memcpy(&_gyro_scale, (struct gyro_scale*) arg, sizeof(_gyro_scale));
+ memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale));
return OK;
case GYROIOCGSCALE:
/* copy scale out */
- memcpy((struct gyro_scale*) arg, &_gyro_scale, sizeof(_gyro_scale));
+ memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale));
return OK;
case GYROIOCSRANGE:
@@ -562,12 +564,15 @@ L3GD20::set_range(unsigned max_dps)
if (max_dps <= 250) {
_current_range = 250;
bits |= RANGE_250DPS;
+
} else if (max_dps <= 500) {
_current_range = 500;
bits |= RANGE_500DPS;
+
} else if (max_dps <= 2000) {
_current_range = 2000;
bits |= RANGE_2000DPS;
+
} else {
return -EINVAL;
}
@@ -590,15 +595,19 @@ L3GD20::set_samplerate(unsigned frequency)
if (frequency <= 95) {
_current_rate = 95;
bits |= RATE_95HZ_LP_25HZ;
+
} else if (frequency <= 190) {
_current_rate = 190;
bits |= RATE_190HZ_LP_25HZ;
+
} else if (frequency <= 380) {
_current_rate = 380;
bits |= RATE_380HZ_LP_30HZ;
+
} else if (frequency <= 760) {
_current_rate = 760;
bits |= RATE_760HZ_LP_30HZ;
+
} else {
return -EINVAL;
}
@@ -746,17 +755,21 @@ start()
/* set the poll rate to default, starts automatic data collection */
fd = open(GYRO_DEVICE_PATH, O_RDONLY);
+
if (fd < 0)
goto fail;
+
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
exit(0);
fail:
+
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
+
errx(1, "driver start failed");
}
@@ -774,15 +787,17 @@ test()
/* get the driver */
fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
+
if (fd_gyro < 0)
err(1, "%s open failed", GYRO_DEVICE_PATH);
/* reset to manual polling */
if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
err(1, "reset to manual polling");
-
+
/* do a simple demand read */
sz = read(fd_gyro, &g_report, sizeof(g_report));
+
if (sz != sizeof(g_report))
err(1, "immediate gyro read failed");
@@ -793,7 +808,7 @@ test()
warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
- (int)((g_report.range_rad_s / M_PI_F) * 180.0f+0.5f));
+ (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
/* XXX add poll-rate tests here too */
@@ -808,10 +823,13 @@ void
reset()
{
int fd = open(GYRO_DEVICE_PATH, O_RDONLY);
+
if (fd < 0)
err(1, "failed ");
+
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
err(1, "driver reset failed");
+
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
err(1, "driver poll restart failed");