Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Save and check device id for acc and gyro calibration parameters. #1718

Closed
wants to merge 1 commit into from
Closed

Save and check device id for acc and gyro calibration parameters. #1718

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 0 additions & 6 deletions src/drivers/drv_mag.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,6 @@
#include <stdint.h>
#include <sys/ioctl.h>

#include "drv_device.h"
#include "drv_sensor.h"
#include "drv_orb_dev.h"

Expand Down Expand Up @@ -85,11 +84,6 @@ ORB_DECLARE(sensor_mag0);
ORB_DECLARE(sensor_mag1);
ORB_DECLARE(sensor_mag2);

/*
* mag device types, for _device_id
*/
#define DRV_MAG_DEVTYPE_HMC5883 1
#define DRV_MAG_DEVTYPE_LSM303D 2

/*
* ioctl() definitions
Expand Down
18 changes: 18 additions & 0 deletions src/drivers/drv_sensor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
*
Expand Down
16 changes: 9 additions & 7 deletions src/drivers/l3gd20/l3gd20.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -214,7 +214,7 @@ class L3GD20 : public device::SPI

struct hrt_call _call;
unsigned _call_interval;

RingBuffer *_reports;

struct gyro_scale _gyro_scale;
Expand Down Expand Up @@ -411,6 +411,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;
Expand Down Expand Up @@ -639,7 +641,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
return -ENOMEM;
}
irqrestore(flags);

return OK;
}

Expand Down Expand Up @@ -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 */
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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
Expand All @@ -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:
Expand Down Expand Up @@ -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]);
Expand Down
32 changes: 23 additions & 9 deletions src/drivers/lsm303d/lsm303d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -295,7 +295,7 @@ class LSM303D : public device::SPI

enum Rotation _rotation;

// values used to
// values used to
float _last_accel[3];
uint8_t _constant_accel_count;

Expand Down Expand Up @@ -556,11 +556,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;
Expand Down Expand Up @@ -661,6 +668,7 @@ LSM303D::init()
warnx("ADVERT ERR");
}


_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);

/* advertise sensor topic, measure manually to initialize valid report */
Expand Down Expand Up @@ -712,7 +720,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 */
Expand Down Expand Up @@ -746,7 +754,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;
Expand Down Expand Up @@ -1019,7 +1027,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))
Expand Down Expand Up @@ -1424,7 +1432,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);

Expand Down Expand Up @@ -1548,7 +1556,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;
Expand Down Expand Up @@ -1666,7 +1674,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]);
Expand Down Expand Up @@ -1783,7 +1791,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
Expand Down
Loading