forked from Archive/PX4-Autopilot
lsm303d: use register_class_devname()
This commit is contained in:
Andrew Tridgell
Lorenz Meier
parent
5a88dc02a7
commit
e334377e6c
|
|
@ -80,8 +80,8 @@ static const int ERROR = -1;
|
|||
#define DIR_WRITE (0<<7)
|
||||
#define ADDR_INCREMENT (1<<6)
|
||||
|
||||
#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag"
|
||||
#define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel"
|
||||
#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag"
|
||||
|
||||
/* register addresses: A: accel, M: mag, T: temp */
|
||||
#define ADDR_WHO_AM_I 0x0F
|
||||
|
|
@ -277,7 +277,7 @@ private:
|
|||
unsigned _mag_samplerate;
|
||||
|
||||
orb_advert_t _accel_topic;
|
||||
orb_advert_t _mag_topic;
|
||||
int _class_instance;
|
||||
|
||||
unsigned _accel_read;
|
||||
unsigned _mag_read;
|
||||
|
|
@ -466,6 +466,8 @@ public:
|
|||
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
|
||||
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||
|
||||
virtual int init();
|
||||
|
||||
protected:
|
||||
friend class LSM303D;
|
||||
|
||||
|
|
@ -473,6 +475,9 @@ protected:
|
|||
private:
|
||||
LSM303D *_parent;
|
||||
|
||||
orb_advert_t _mag_topic;
|
||||
int _mag_class_instance;
|
||||
|
||||
void measure();
|
||||
|
||||
void measure_trampoline(void *arg);
|
||||
|
|
@ -494,7 +499,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
|
|||
_mag_range_scale(0.0f),
|
||||
_mag_samplerate(0),
|
||||
_accel_topic(-1),
|
||||
_mag_topic(-1),
|
||||
_class_instance(-1),
|
||||
_accel_read(0),
|
||||
_mag_read(0),
|
||||
_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
|
||||
|
|
@ -544,6 +549,9 @@ LSM303D::~LSM303D()
|
|||
if (_mag_reports != nullptr)
|
||||
delete _mag_reports;
|
||||
|
||||
if (_class_instance != -1)
|
||||
unregister_class_devname(ACCEL_DEVICE_PATH, _class_instance);
|
||||
|
||||
delete _mag;
|
||||
|
||||
/* delete the perf counter */
|
||||
|
|
@ -573,10 +581,6 @@ LSM303D::init()
|
|||
goto out;
|
||||
|
||||
/* advertise accel topic */
|
||||
struct accel_report zero_report;
|
||||
memset(&zero_report, 0, sizeof(zero_report));
|
||||
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
|
||||
|
||||
_mag_reports = new RingBuffer(2, sizeof(mag_report));
|
||||
|
||||
if (_mag_reports == nullptr)
|
||||
|
|
@ -584,53 +588,22 @@ LSM303D::init()
|
|||
|
||||
reset();
|
||||
|
||||
/* register the first instance as plain name, the 2nd as two and counting */
|
||||
ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this);
|
||||
|
||||
if (ret == OK) {
|
||||
log("default accel device");
|
||||
|
||||
} else {
|
||||
|
||||
unsigned instance = 1;
|
||||
do {
|
||||
char name[32];
|
||||
sprintf(name, "%s%d", ACCEL_DEVICE_PATH, instance);
|
||||
ret = register_driver(name, &fops, 0666, (void *)this);
|
||||
instance++;
|
||||
} while (ret);
|
||||
/* do CDev init for the mag device node */
|
||||
ret = _mag->init();
|
||||
if (ret != OK) {
|
||||
warnx("MAG init failed");
|
||||
goto out;
|
||||
}
|
||||
|
||||
/* try to claim the generic accel node as well - it's OK if we fail at this */
|
||||
mag_ret = register_driver(MAG_DEVICE_PATH, &fops, 0666, (void *)this);
|
||||
|
||||
if (mag_ret == OK) {
|
||||
log("default mag device");
|
||||
|
||||
} else {
|
||||
|
||||
unsigned instance = 1;
|
||||
do {
|
||||
char name[32];
|
||||
sprintf(name, "%s%d", MAG_DEVICE_PATH, instance);
|
||||
ret = register_driver(name, &fops, 0666, (void *)this);
|
||||
instance++;
|
||||
} while (ret);
|
||||
_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
|
||||
if (_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||
// we are the primary accel device, so advertise to
|
||||
// the ORB
|
||||
struct accel_report zero_report;
|
||||
memset(&zero_report, 0, sizeof(zero_report));
|
||||
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
|
||||
}
|
||||
|
||||
/* advertise mag topic */
|
||||
struct mag_report zero_mag_report;
|
||||
memset(&zero_mag_report, 0, sizeof(zero_mag_report));
|
||||
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report);
|
||||
|
||||
/* do CDev init for the mag device node, keep it optional */
|
||||
mag_ret = _mag->init();
|
||||
|
||||
if (mag_ret != OK) {
|
||||
_mag_topic = -1;
|
||||
}
|
||||
|
||||
ret = OK;
|
||||
out:
|
||||
return ret;
|
||||
}
|
||||
|
|
@ -1510,8 +1483,10 @@ LSM303D::measure()
|
|||
/* notify anyone waiting for data */
|
||||
poll_notify(POLLIN);
|
||||
|
||||
/* publish for subscribers */
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
|
||||
if (_accel_topic != -1) {
|
||||
/* publish for subscribers */
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
|
||||
}
|
||||
|
||||
_accel_read++;
|
||||
|
||||
|
|
@ -1582,8 +1557,10 @@ LSM303D::mag_measure()
|
|||
/* notify anyone waiting for data */
|
||||
poll_notify(POLLIN);
|
||||
|
||||
/* publish for subscribers */
|
||||
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
|
||||
if (_mag->_mag_topic != -1) {
|
||||
/* publish for subscribers */
|
||||
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
|
||||
}
|
||||
|
||||
_mag_read++;
|
||||
|
||||
|
|
@ -1673,13 +1650,39 @@ LSM303D::toggle_logging()
|
|||
}
|
||||
|
||||
LSM303D_mag::LSM303D_mag(LSM303D *parent) :
|
||||
CDev("LSM303D_mag", "/dev/lsm303d_mag"),
|
||||
_parent(parent)
|
||||
CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
|
||||
_parent(parent),
|
||||
_mag_topic(-1),
|
||||
_mag_class_instance(-1)
|
||||
{
|
||||
}
|
||||
|
||||
LSM303D_mag::~LSM303D_mag()
|
||||
{
|
||||
if (_mag_class_instance != -1)
|
||||
unregister_class_devname(MAG_DEVICE_PATH, _mag_class_instance);
|
||||
}
|
||||
|
||||
int
|
||||
LSM303D_mag::init()
|
||||
{
|
||||
int ret;
|
||||
|
||||
ret = CDev::init();
|
||||
if (ret != OK)
|
||||
goto out;
|
||||
|
||||
_mag_class_instance = register_class_devname(MAG_DEVICE_PATH);
|
||||
if (_mag_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||
// we are the primary mag device, so advertise to
|
||||
// the ORB
|
||||
struct mag_report zero_report;
|
||||
memset(&zero_report, 0, sizeof(zero_report));
|
||||
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
|
||||
}
|
||||
|
||||
out:
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -1739,7 +1742,7 @@ start()
|
|||
errx(0, "already started");
|
||||
|
||||
/* create the driver */
|
||||
g_dev = new LSM303D(1 /* XXX magic number */, LSM303D_DEVICE_PATH_MAG, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
|
||||
g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
warnx("failed instantiating LSM303D obj");
|
||||
|
|
@ -1858,7 +1861,7 @@ test()
|
|||
void
|
||||
reset()
|
||||
{
|
||||
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
||||
int fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
err(1, "failed ");
|
||||
|
|
@ -1869,7 +1872,7 @@ reset()
|
|||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||
err(1, "accel pollrate reset failed");
|
||||
|
||||
fd = open(MAG_DEVICE_PATH, O_RDONLY);
|
||||
fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
warnx("mag could not be opened, external mag might be used");
|
||||
|
|
|
|||
Loading…
Reference in New Issue