aboutsummaryrefslogtreecommitdiff
path: root/src/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers')
-rw-r--r--src/drivers/drv_mag.h6
-rw-r--r--src/drivers/drv_sensor.h18
-rw-r--r--src/drivers/l3gd20/l3gd20.cpp16
-rw-r--r--src/drivers/lsm303d/lsm303d.cpp32
-rw-r--r--src/drivers/mpu6000/mpu6000.cpp56
5 files changed, 85 insertions, 43 deletions
diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
index d8fe1ae7a..193c816e0 100644
--- a/src/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.h
@@ -41,7 +41,6 @@
#include <stdint.h>
#include <sys/ioctl.h>
-#include "drv_device.h"
#include "drv_sensor.h"
#include "drv_orb_dev.h"
@@ -83,11 +82,6 @@ struct mag_scale {
*/
ORB_DECLARE(sensor_mag);
-/*
- * mag device types, for _device_id
- */
-#define DRV_MAG_DEVTYPE_HMC5883 1
-#define DRV_MAG_DEVTYPE_LSM303D 2
/*
* ioctl() definitions
diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h
index 5e4598de8..467dace08 100644
--- a/src/drivers/drv_sensor.h
+++ b/src/drivers/drv_sensor.h
@@ -43,6 +43,24 @@
#include <stdint.h>
#include <sys/ioctl.h>
+#include "drv_device.h"
+
+/**
+ * Sensor type definitions.
+ *
+ * Used to create a unique device id for redundant and combo sensors
+ */
+
+#define DRV_MAG_DEVTYPE_HMC5883 0x01
+#define DRV_MAG_DEVTYPE_LSM303D 0x02
+#define DRV_ACC_DEVTYPE_LSM303D 0x11
+#define DRV_ACC_DEVTYPE_BMA180 0x12
+#define DRV_ACC_DEVTYPE_MPU6000 0x13
+#define DRV_GYR_DEVTYPE_MPU6000 0x21
+#define DRV_GYR_DEVTYPE_L3GD20 0x22
+#define DRV_RNG_DEVTYPE_MB12XX 0x31
+#define DRV_RNG_DEVTYPE_LL40LS 0x32
+
/*
* ioctl() definitions
*
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index bd1bd9f86..f583bced4 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -220,7 +220,7 @@ private:
struct hrt_call _call;
unsigned _call_interval;
-
+
RingBuffer *_reports;
struct gyro_scale _gyro_scale;
@@ -424,6 +424,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
// enable debug() calls
_debug_enabled = true;
+ _device_id.devid_s.devtype = DRV_GYR_DEVTYPE_L3GD20;
+
// default scale factors
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
@@ -639,7 +641,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
return -ENOMEM;
}
irqrestore(flags);
-
+
return OK;
}
@@ -867,7 +869,7 @@ L3GD20::reset()
disable_i2c();
/* set default configuration */
- write_checked_reg(ADDR_CTRL_REG1,
+ write_checked_reg(ADDR_CTRL_REG1,
REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
write_checked_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */
write_checked_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */
@@ -911,7 +913,7 @@ L3GD20::check_registers(void)
if we get the wrong value then we know the SPI bus
or sensor is very sick. We set _register_wait to 20
and wait until we have seen 20 good values in a row
- before we consider the sensor to be OK again.
+ before we consider the sensor to be OK again.
*/
perf_count(_bad_registers);
@@ -974,7 +976,7 @@ L3GD20::measure()
we waited for DRDY, but did not see DRDY on all axes
when we captured. That means a transfer error of some sort
*/
- perf_count(_errors);
+ perf_count(_errors);
return;
}
#endif
@@ -994,7 +996,7 @@ L3GD20::measure()
*/
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_bad_registers);
-
+
switch (_orientation) {
case SENSOR_BOARD_ROTATION_000_DEG:
@@ -1072,7 +1074,7 @@ L3GD20::print_info()
for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) {
uint8_t v = read_reg(_checked_registers[i]);
if (v != _checked_values[i]) {
- ::printf("reg %02x:%02x should be %02x\n",
+ ::printf("reg %02x:%02x should be %02x\n",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_values[i]);
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index ff7068936..2a0bf522b 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -301,7 +301,7 @@ private:
enum Rotation _rotation;
- // values used to
+ // values used to
float _last_accel[3];
uint8_t _constant_accel_count;
@@ -569,11 +569,18 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
_constant_accel_count(0),
_checked_next(0)
{
- _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
+
// enable debug() calls
_debug_enabled = true;
+ _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_LSM303D;
+
+ /* Prime _mag with parents devid. */
+ _mag->_device_id.devid = _device_id.devid;
+ _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
+
+
// default scale factors
_accel_scale.x_offset = 0.0f;
_accel_scale.x_scale = 1.0f;
@@ -660,6 +667,7 @@ LSM303D::init()
warnx("ADVERT ERR");
}
+
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
/* advertise sensor topic, measure manually to initialize valid report */
@@ -698,7 +706,7 @@ LSM303D::reset()
disable_i2c();
/* enable accel*/
- write_checked_reg(ADDR_CTRL_REG1,
+ write_checked_reg(ADDR_CTRL_REG1,
REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A);
/* enable mag */
@@ -732,7 +740,7 @@ LSM303D::probe()
/* verify that the device is attached and functioning */
bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM);
-
+
if (success) {
_checked_values[0] = WHO_I_AM;
return OK;
@@ -1005,7 +1013,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
return SENSOR_POLLRATE_MANUAL;
return 1000000 / _call_mag_interval;
-
+
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
@@ -1410,7 +1418,7 @@ LSM303D::check_registers(void)
if we get the wrong value then we know the SPI bus
or sensor is very sick. We set _register_wait to 20
and wait until we have seen 20 good values in a row
- before we consider the sensor to be OK again.
+ before we consider the sensor to be OK again.
*/
perf_count(_bad_registers);
@@ -1534,7 +1542,7 @@ LSM303D::measure()
perf_count(_bad_values);
_constant_accel_count = 0;
}
-
+
_last_accel[0] = x_in_new;
_last_accel[1] = y_in_new;
_last_accel[2] = z_in_new;
@@ -1652,7 +1660,7 @@ LSM303D::print_info()
for (uint8_t i=0; i<LSM303D_NUM_CHECKED_REGISTERS; i++) {
uint8_t v = read_reg(_checked_registers[i]);
if (v != _checked_values[i]) {
- ::printf("reg %02x:%02x should be %02x\n",
+ ::printf("reg %02x:%02x should be %02x\n",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_values[i]);
@@ -1769,7 +1777,13 @@ LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen)
int
LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
{
- return _parent->mag_ioctl(filp, cmd, arg);
+ switch (cmd) {
+ case DEVIOCGDEVICEID:
+ return (int)CDev::ioctl(filp, cmd, arg);
+ break;
+ default:
+ return _parent->mag_ioctl(filp, cmd, arg);
+ }
}
void
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index e4e982490..e322e8b3a 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -446,7 +446,7 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU
MPUREG_INT_ENABLE,
MPUREG_INT_PIN_CFG };
-
+
/**
* Helper class implementing the gyro driver node.
@@ -523,6 +523,12 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
// disable debug() calls
_debug_enabled = false;
+ _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU6000;
+
+ /* Prime _gyro with parents devid. */
+ _gyro->_device_id.devid = _device_id.devid;
+ _gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU6000;
+
// default accel scale factors
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
@@ -609,6 +615,7 @@ MPU6000::init()
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
+
/* do CDev init for the gyro device node, keep it optional */
ret = _gyro->init();
/* if probe/setup failed, bail now */
@@ -668,7 +675,7 @@ int MPU6000::reset()
// for it to come out of sleep
write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
up_udelay(1000);
-
+
// Disable I2C bus (recommended on datasheet)
write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
irqrestore(state);
@@ -726,7 +733,7 @@ int MPU6000::reset()
case MPU6000_REV_D9:
case MPU6000_REV_D10:
// default case to cope with new chip revisions, which
- // presumably won't have the accel scaling bug
+ // presumably won't have the accel scaling bug
default:
// Accel scale 8g (4096 LSB/g)
write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3);
@@ -804,7 +811,7 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
{
uint8_t filter;
- /*
+ /*
choose next highest filter frequency available
*/
if (frequency_hz == 0) {
@@ -906,7 +913,7 @@ MPU6000::gyro_self_test()
if (self_test())
return 1;
- /*
+ /*
* Maximum deviation of 20 degrees, according to
* http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf
* Section 6.1, initial ZRO tolerance
@@ -967,7 +974,7 @@ MPU6000::factory_self_test()
// gyro self test has to be done at 250DPS
write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS);
- struct MPUReport mpu_report;
+ struct MPUReport mpu_report;
float accel_baseline[3];
float gyro_baseline[3];
float accel[3];
@@ -997,10 +1004,10 @@ MPU6000::factory_self_test()
}
#if 1
- write_reg(MPUREG_GYRO_CONFIG,
- BITS_FS_250DPS |
- BITS_GYRO_ST_X |
- BITS_GYRO_ST_Y |
+ write_reg(MPUREG_GYRO_CONFIG,
+ BITS_FS_250DPS |
+ BITS_GYRO_ST_X |
+ BITS_GYRO_ST_Y |
BITS_GYRO_ST_Z);
// accel 8g, self-test enabled all axes
@@ -1070,7 +1077,7 @@ MPU6000::factory_self_test()
::printf("FAIL\n");
ret = -EIO;
}
- }
+ }
for (uint8_t i=0; i<3; i++) {
float diff = gyro[i]-gyro_baseline[i];
float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i];
@@ -1085,7 +1092,7 @@ MPU6000::factory_self_test()
::printf("FAIL\n");
ret = -EIO;
}
- }
+ }
write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config);
write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config);
@@ -1232,14 +1239,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
-
+
irqstate_t flags = irqsave();
if (!_accel_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
-
+
return OK;
}
@@ -1521,13 +1528,13 @@ MPU6000::check_registers(void)
the data registers.
*/
uint8_t v;
- if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) !=
+ if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) !=
_checked_values[_checked_next]) {
/*
if we get the wrong value then we know the SPI bus
or sensor is very sick. We set _register_wait to 20
and wait until we have seen 20 good values in a row
- before we consider the sensor to be OK again.
+ before we consider the sensor to be OK again.
*/
perf_count(_bad_registers);
@@ -1640,7 +1647,7 @@ MPU6000::measure()
_register_wait--;
return;
}
-
+
/*
* Swap axes and negate y
@@ -1701,7 +1708,7 @@ MPU6000::measure()
float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
-
+
arb.x = _accel_filter_x.apply(x_in_new);
arb.y = _accel_filter_y.apply(y_in_new);
arb.z = _accel_filter_z.apply(z_in_new);
@@ -1722,7 +1729,7 @@ MPU6000::measure()
float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
-
+
grb.x = _gyro_filter_x.apply(x_gyro_in_new);
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
@@ -1776,7 +1783,7 @@ MPU6000::print_info()
for (uint8_t i=0; i<MPU6000_NUM_CHECKED_REGISTERS; i++) {
uint8_t v = read_reg(_checked_registers[i], MPU6000_HIGH_BUS_SPEED);
if (v != _checked_values[i]) {
- ::printf("reg %02x:%02x should be %02x\n",
+ ::printf("reg %02x:%02x should be %02x\n",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_values[i]);
@@ -1848,7 +1855,14 @@ MPU6000_gyro::read(struct file *filp, char *buffer, size_t buflen)
int
MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
{
- return _parent->gyro_ioctl(filp, cmd, arg);
+
+ switch (cmd) {
+ case DEVIOCGDEVICEID:
+ return (int)CDev::ioctl(filp, cmd, arg);
+ break;
+ default:
+ return _parent->gyro_ioctl(filp, cmd, arg);
+ }
}
/**
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-12 04:13:22 +0000