Browse Source

Sensor drivers should run all the time, not just when their device is open.

Disable this for the mpu6000 driver though, as it's currently busted in that regard.
sbg
px4dev 13 years ago
parent
commit
23d8b69e3d
  1. 42
      apps/drivers/hmc5883/hmc5883.cpp
  2. 27
      apps/drivers/mpu6000/mpu6000.cpp
  3. 55
      apps/drivers/ms5611/ms5611.cpp

42
apps/drivers/hmc5883/hmc5883.cpp

@ -126,9 +126,6 @@ public:
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); 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 ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int open_first(struct file *filp);
virtual int close_last(struct file *filp);
/** /**
* Diagnostics - print some basic information about the driver. * Diagnostics - print some basic information about the driver.
*/ */
@ -277,7 +274,7 @@ HMC5883::HMC5883(int bus) :
_scale.z_scale = 1.0f / 1090.0f; /* default range scale from counts to gauss */ _scale.z_scale = 1.0f / 1090.0f; /* default range scale from counts to gauss */
// work_cancel in the dtor will explode if we don't do this... // work_cancel in the dtor will explode if we don't do this...
_work.worker = nullptr; memset(&_work, 0, sizeof(_work));
} }
HMC5883::~HMC5883() HMC5883::~HMC5883()
@ -296,44 +293,19 @@ HMC5883::init()
int ret = ERROR; int ret = ERROR;
/* do I2C init (and probe) first */ /* do I2C init (and probe) first */
ret = I2C::init(); if (I2C::init() != OK)
if (ret != OK)
goto out; goto out;
out:
return ret;
}
int
HMC5883::open_first(struct file *filp)
{
/* reset to manual-poll mode */
_measure_ticks = 0;
/* allocate basic report buffers */ /* allocate basic report buffers */
_num_reports = 2; _num_reports = 2;
_reports = new struct mag_report[_num_reports]; _reports = new struct mag_report[_num_reports];
if (_reports == nullptr)
goto out;
_oldest_report = _next_report = 0; _oldest_report = _next_report = 0;
return OK; ret = OK;
} out:
return ret;
int
HMC5883::close_last(struct file *filp)
{
/* stop measurement */
stop();
/* free report buffers */
if (_reports != nullptr) {
delete[] _reports;
_num_reports = 0;
}
_measure_ticks = 0;
return OK;
} }
int int

27
apps/drivers/mpu6000/mpu6000.cpp

@ -157,9 +157,6 @@ public:
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); 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 ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int open_first(struct file *filp);
virtual int close_last(struct file *filp);
/** /**
* Diagnostics - print some basic information about the driver. * Diagnostics - print some basic information about the driver.
*/ */
@ -322,6 +319,7 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
memset(&_accel_report, 0, sizeof(_accel_report)); memset(&_accel_report, 0, sizeof(_accel_report));
memset(&_gyro_report, 0, sizeof(_gyro_report)); memset(&_gyro_report, 0, sizeof(_gyro_report));
memset(&_call, 0, sizeof(_call));
} }
MPU6000::~MPU6000() MPU6000::~MPU6000()
@ -444,26 +442,6 @@ MPU6000::init()
return ret; return ret;
} }
int
MPU6000::open_first(struct file *filp)
{
/* reset to manual-poll mode */
_call_interval = 0;
/* XXX set default sampling/acquisition parameters */
return OK;
}
int
MPU6000::close_last(struct file *filp)
{
/* stop measurement */
stop();
return OK;
}
int int
MPU6000::probe() MPU6000::probe()
{ {
@ -911,12 +889,15 @@ start()
if (OK != g_dev->init()) if (OK != g_dev->init())
goto fail; goto fail;
#if 0 /* XXX don't do this for now - the auto-poller is børked */
/* set the poll rate to default, starts automatic data collection */ /* set the poll rate to default, starts automatic data collection */
fd = open(ACCEL_DEVICE_PATH, O_RDONLY); fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (fd < 0) if (fd < 0)
goto fail; goto fail;
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail; goto fail;
#endif
exit(0); exit(0);
fail: fail:

55
apps/drivers/ms5611/ms5611.cpp

@ -64,6 +64,12 @@
#include <drivers/drv_baro.h> #include <drivers/drv_baro.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
/** /**
* Calibration PROM as reported by the device. * Calibration PROM as reported by the device.
*/ */
@ -99,9 +105,6 @@ public:
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); 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 ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int open_first(struct file *filp);
virtual int close_last(struct file *filp);
/** /**
* Diagnostics - print some basic information about the driver. * Diagnostics - print some basic information about the driver.
*/ */
@ -259,7 +262,7 @@ MS5611::MS5611(int bus) :
_debug_enabled = true; _debug_enabled = true;
// work_cancel in the dtor will explode if we don't do this... // work_cancel in the dtor will explode if we don't do this...
_work.worker = nullptr; memset(&_work, 0, sizeof(_work));
} }
MS5611::~MS5611() MS5611::~MS5611()
@ -275,43 +278,23 @@ MS5611::~MS5611()
int int
MS5611::init() MS5611::init()
{ {
int ret; int ret = ERROR;
/* do I2C init (and probe) first */ /* do I2C init (and probe) first */
ret = I2C::init(); if (I2C::init() != OK)
goto out;
return ret;
}
int
MS5611::open_first(struct file *filp)
{
/* reset to manual-poll mode */
_measure_ticks = 0;
/* allocate basic report buffers */ /* allocate basic report buffers */
_num_reports = 2; _num_reports = 2;
_reports = new struct baro_report[_num_reports]; _reports = new struct baro_report[_num_reports];
_oldest_report = _next_report = 0; if (_reports == nullptr)
goto out;
return OK;
}
int
MS5611::close_last(struct file *filp)
{
/* stop measurement */
stop();
/* free report buffers */
if (_reports != nullptr) {
delete[] _reports;
_num_reports = 0;
}
_measure_ticks = 0; _oldest_report = _next_report = 0;
return OK; ret = OK;
out:
return ret;
} }
int int
@ -807,12 +790,6 @@ MS5611::print_info()
namespace ms5611 namespace ms5611
{ {
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
MS5611 *g_dev; MS5611 *g_dev;
void start(); void start();

Loading…
Cancel
Save